diff --git a/BrickController2/BrickController2.Android/PlatformServices/BluetoothLE/BluetoothLEDevice.cs b/BrickController2/BrickController2.Android/PlatformServices/BluetoothLE/BluetoothLEDevice.cs index 77645e23..ade8a9e1 100644 --- a/BrickController2/BrickController2.Android/PlatformServices/BluetoothLE/BluetoothLEDevice.cs +++ b/BrickController2/BrickController2.Android/PlatformServices/BluetoothLE/BluetoothLEDevice.cs @@ -114,6 +114,20 @@ internal void Disconnect() if (_bluetoothGatt != null) { _bluetoothGatt.Disconnect(); + + // Android BLE best practice: Refresh the service cache before closing + // This prevents stale service information from being cached between reconnections + // The refresh() method is a hidden API, so we use reflection to call it + try + { + var refreshMethod = _bluetoothGatt.Class?.GetMethod("refresh"); + refreshMethod?.Invoke(_bluetoothGatt); + } + catch + { + // Silently ignore if refresh fails (not critical, but helpful) + } + _bluetoothGatt.Close(); _bluetoothGatt.Dispose(); _bluetoothGatt = null; diff --git a/BrickController2/BrickController2.Tests/DeviceManagement/IO/OutputValuesGroupTests.cs b/BrickController2/BrickController2.Tests/DeviceManagement/IO/OutputValuesGroupTests.cs index 95464401..bc7f29b4 100644 --- a/BrickController2/BrickController2.Tests/DeviceManagement/IO/OutputValuesGroupTests.cs +++ b/BrickController2/BrickController2.Tests/DeviceManagement/IO/OutputValuesGroupTests.cs @@ -1,5 +1,6 @@ using BrickController2.DeviceManagement.IO; using FluentAssertions; +using System; using System.Collections.Generic; using Xunit; @@ -42,6 +43,22 @@ public void Initialize_NoCommit_ChangeIsReportedFiveTimesOnly() lastValues.ToArray().Should().AllBeEquivalentTo(0); } + [Fact] + public void Clear_AnyChange_ReturnsFalseAndAllDefaultValues() + { + // Arrange + var group = new OutputValuesGroup(5); + group.SetOutput(3, Half.Pi); + // Act + group.Clear(); + + // Assert + var result = group.TryGetValues(out var values); + result.Should().BeFalse(); + values.Length.Should().Be(5); + values.ToArray().Should().AllBeEquivalentTo(Half.Zero); + } + [Theory] [InlineData(2)] [InlineData(7)] @@ -115,7 +132,7 @@ public void Commit_ExistingChange_NoChangeIsReportedThen() changedValues.Should().BeEquivalentTo([new KeyValuePair(0, 7)]); // Act - group.Commmit(); + group.Commit(); // Assert group.TryGetChanges(out var values).Should().BeFalse(); diff --git a/BrickController2/BrickController2/DeviceManagement/BluetoothDevice.cs b/BrickController2/BrickController2/DeviceManagement/BluetoothDevice.cs index 0a38e46b..8ca7c303 100644 --- a/BrickController2/BrickController2/DeviceManagement/BluetoothDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/BluetoothDevice.cs @@ -105,6 +105,8 @@ protected virtual void OnCharacteristicChanged(Guid characteristicGuid, byte[] d { } + protected abstract void OnDeviceDisconnecting(); + protected virtual Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { return Task.FromResult(true); @@ -116,9 +118,13 @@ private async Task DisconnectInternalAsync() { await StopOutputTaskAsync(); DeviceState = DeviceState.Disconnecting; + // notify disconnection + OnDeviceDisconnecting(); + // execute native device disconnection + cleanup await _bleDevice.DisconnectAsync(); _bleDevice = null; } + _onDeviceDisconnected = null; DeviceState = DeviceState.Disconnected; } @@ -129,8 +135,10 @@ private void OnDeviceDisconnected(IBluetoothLEDevice bluetoothLEDevice) { using (await _asyncLock.LockAsync()) { + var disconnectedCallback = _onDeviceDisconnected; await DisconnectInternalAsync(); - _onDeviceDisconnected?.Invoke(this); + // notify + disconnectedCallback?.Invoke(this); } }); } diff --git a/BrickController2/BrickController2/DeviceManagement/BuWizz2Device.cs b/BrickController2/BrickController2/DeviceManagement/BuWizz2Device.cs index c5741548..52e4e41b 100644 --- a/BrickController2/BrickController2/DeviceManagement/BuWizz2Device.cs +++ b/BrickController2/BrickController2/DeviceManagement/BuWizz2Device.cs @@ -107,6 +107,14 @@ protected override async Task ValidateServicesAsync(IEnumerable ValidateServicesAsync(IEnumerable? s return Task.FromResult(_characteristic != null && _firmwareRevisionCharacteristic != null && _modelNumberCharacteristic != null); } + protected override void OnDeviceDisconnecting() + { + // Clear cached characteristic references to prevent using stale native Android objects on reconnection + _characteristic = null; + _modelNumberCharacteristic = null; + _firmwareRevisionCharacteristic = null; + } protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] data) { diff --git a/BrickController2/BrickController2/DeviceManagement/BuwizzDevice.cs b/BrickController2/BrickController2/DeviceManagement/BuwizzDevice.cs index d19703a3..34bc9d83 100644 --- a/BrickController2/BrickController2/DeviceManagement/BuwizzDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/BuwizzDevice.cs @@ -81,6 +81,13 @@ protected override Task ValidateServicesAsync(IEnumerable? s return Task.FromResult(_characteristic is not null); } + + protected override void OnDeviceDisconnecting() + { + // Clear cached characteristic reference to prevent using stale native Android objects on reconnection + _characteristic = null; + } + protected override async Task ProcessOutputsAsync(CancellationToken token) { lock (_outputLock) diff --git a/BrickController2/BrickController2/DeviceManagement/CircuitCubeDevice.cs b/BrickController2/BrickController2/DeviceManagement/CircuitCubeDevice.cs index 636f9e04..c421f28b 100644 --- a/BrickController2/BrickController2/DeviceManagement/CircuitCubeDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/CircuitCubeDevice.cs @@ -97,6 +97,13 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] BatteryVoltage = bateryVoltage; } } + protected override void OnDeviceDisconnecting() + { + _writeCharacteristic = null; + _notifyCharacteristic = null; + _hardwareRevisionCharacteristic = null; + _firmwareRevisionCharacteristic = null; + } protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index 99e2e035..71ec026b 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -1,9 +1,9 @@ using BrickController2.CreationManagement; +using BrickController2.DeviceManagement.Lego; using BrickController2.PlatformServices.BluetoothLE; using System; using System.Collections.Generic; using System.Diagnostics; -using System.Linq; using System.Threading; using System.Threading.Tasks; @@ -11,11 +11,10 @@ namespace BrickController2.DeviceManagement { - internal abstract class ControlPlusDevice : BluetoothDevice + internal abstract class ControlPlusDevice : ControlPlusDeviceBase { private const int MAX_SEND_ATTEMPTS = 10; - private static readonly TimeSpan SEND_DELAY = TimeSpan.FromMilliseconds(10); private static readonly TimeSpan POSITION_EXPIRATION = TimeSpan.FromMilliseconds(200); private readonly byte[] _sendBuffer = new byte[] { 8, 0x00, 0x81, 0x00, 0x11, 0x51, 0x00, 0x00 }; @@ -40,8 +39,6 @@ internal abstract class ControlPlusDevice : BluetoothDevice private readonly object _positionLock = new object(); private readonly Stopwatch _lastSent_NormalMotor = new Stopwatch(); - private IGattCharacteristic? _characteristic; - public ControlPlusDevice(string name, string address, IDeviceRepository deviceRepository, IBluetoothLEService bleService) : base(name, address, deviceRepository, bleService) { @@ -60,8 +57,6 @@ public ControlPlusDevice(string name, string address, IDeviceRepository deviceRe _positionUpdateTimes = new DateTime[NumberOfChannels]; } - public override string BatteryVoltageSign => "%"; - public override bool IsOutputTypeSupported(int channel, ChannelOutputType outputType) // support all output types on all channels => true; @@ -166,48 +161,14 @@ public override async Task ResetOutputAsync(int channel, float value, Cancellati return await AutoCalibrateServoAsync(channel, token); } - protected override async Task ValidateServicesAsync(IEnumerable? services, CancellationToken token) - { - var service = services?.FirstOrDefault(s => s.Uuid == ServiceUuid); - _characteristic = service?.Characteristics?.FirstOrDefault(c => c.Uuid == CharacteristicUuid); - - if (_characteristic is not null) - { - return await _bleDevice!.EnableNotificationAsync(_characteristic, token); - } - - return false; - } - - protected async Task WriteNoResponseAsync(byte[] data, bool withSendDelay = false, CancellationToken token = default) - { - var result = await _bleDevice!.WriteNoResponseAsync(_characteristic!, data, token); - - if (withSendDelay) - { - await Task.Delay(SEND_DELAY, token); - } - return result; - } - - protected Task WriteAsync(byte[] data, CancellationToken token = default) - => _bleDevice!.WriteAsync(_characteristic!, data, token); - - protected virtual byte GetPortId(int channelIndex) => (byte)channelIndex; - protected virtual int GetChannelIndex(byte portId) => portId; - - protected virtual byte GetChannelValue(int value) - // calculate raw motor value - => (byte)(value < 0 ? (255 + value) : value); - - protected void ResetSendAttemps(int channel, int attemps = MAX_SEND_ATTEMPTS) + protected void ResetSendAttempts(int channel, int attempts = MAX_SEND_ATTEMPTS) { lock (_outputLock) { // do it conditionally if (_sendAttemptsLeft[channel] != MAX_SEND_ATTEMPTS) { - _sendAttemptsLeft[channel] = attemps; + _sendAttemptsLeft[channel] = attempts; } } } @@ -216,7 +177,7 @@ protected virtual byte[] GetOutputCommand(int channel, int value) { // send base motor value (-100 .. 100 %) _sendBuffer[3] = GetPortId(channel); - _sendBuffer[7] = GetChannelValue(value); + _sendBuffer[7] = ToByte(value); return _sendBuffer; } @@ -233,148 +194,6 @@ protected virtual byte[] GetServoCommand(int channel, int servoValue, int servoS return _servoSendBuffer; } - protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] data) - { - if (characteristicGuid != CharacteristicUuid || data.Length < 4) - { - return; - } - - var messageCode = data[2]; - - switch (messageCode) - { - case 0x01: // Hub properties - ProcessHubPropertyData(data); - break; - - case 0x02: // Hub actions - DumpData("Hub actions", data); - break; - - case 0x03: // Hub alerts - DumpData("Hub alerts", data); - break; - - case 0x04: // Hub attached I/O - DumpData("Hub attached I/O", data); - break; - - case 0x05: // Generic error messages - DumpData("Generic error messages", data); - break; - - case 0x08: // HW network commands - DumpData("HW network commands", data); - break; - - case 0x13: // FW lock status - DumpData("FW lock status", data); - break; - - case 0x43: // Port information - DumpData("Port information", data); - break; - - case 0x44: // Port mode information - DumpData("Port mode information", data); - break; - - case 0x45: // Port value (single mode) - lock (_positionLock) - { - if (data.Length == 6) - { - // assume 16bit data is ABS - var channel = GetChannelIndex(data[3]); - var absPosition = ToInt16(data, 4); - _absolutePositions[channel] = absPosition; - } - else if (data.Length == 8) - { - // assume 32 bit data is REL - var channel = GetChannelIndex(data[3]); - var relPosition = ToInt32(data, 4); - _relativePositions[channel] = relPosition; - - _positionsUpdated[channel] = true; - _positionUpdateTimes[channel] = DateTime.Now; - } - } - break; - - case 0x46: // Port value (combined mode) - lock (_positionLock) - { - var channel = GetChannelIndex(data[3]); - var modeMask = data[5]; - var dataIndex = 6; - - if ((modeMask & 0x01) != 0) - { - var absPosBuffer = BitConverter.IsLittleEndian ? - new byte[] { data[dataIndex + 0], data[dataIndex + 1] } : - new byte[] { data[dataIndex + 1], data[dataIndex + 0] }; - - var absPosition = BitConverter.ToInt16(absPosBuffer, 0); - _absolutePositions[channel] = absPosition; - - dataIndex += 2; - } - - if ((modeMask & 0x02) != 0) - { - // TODO: Read the post value format response and determine the value length accordingly - if ((dataIndex + 3) < data.Length) - { - var relPosBuffer = BitConverter.IsLittleEndian ? - new byte[] { data[dataIndex + 0], data[dataIndex + 1], data[dataIndex + 2], data[dataIndex + 3] } : - new byte[] { data[dataIndex + 3], data[dataIndex + 2], data[dataIndex + 1], data[dataIndex + 0] }; - - var relPosition = BitConverter.ToInt32(relPosBuffer, 0); - _relativePositions[channel] = relPosition; - } - else if ((dataIndex + 1) < data.Length) - { - var relPosBuffer = BitConverter.IsLittleEndian ? - new byte[] { data[dataIndex + 0], data[dataIndex + 1] } : - new byte[] { data[dataIndex + 1], data[dataIndex + 0] }; - - var relPosition = BitConverter.ToInt16(relPosBuffer, 0); - _relativePositions[channel] = relPosition; - } - else - { - _relativePositions[channel] = data[dataIndex]; - } - - _positionsUpdated[channel] = true; - _positionUpdateTimes[channel] = DateTime.Now; - } - } - - break; - - case 0x47: // Port input format (Single mode) - DumpData("Port input format (single)", data); - break; - - case 0x48: // Port input format (Combined mode) - DumpData("Port input format (combined)", data); - break; - - case 0x82: // Port output command feedback - DumpData("Output command feedback", data); - break; - } - } - - private void DumpData(string header, byte[] data) - { - //var s = BitConverter.ToString(data); - //Console.WriteLine(header + " - " + s); - } - protected override async Task ProcessOutputsAsync(CancellationToken token) { try @@ -384,7 +203,11 @@ protected override async Task ProcessOutputsAsync(CancellationToken token) { for (int channel = 0; channel < NumberOfChannels; channel++) { - InitializeChannelInfo(channel); + _outputValues[channel] = 0; + _lastOutputValues[channel] = 1; + _sendAttemptsLeft[channel] = MAX_SEND_ATTEMPTS; + _positionsUpdated[channel] = false; + _positionUpdateTimes[channel] = DateTime.MinValue; } } _lastSent_NormalMotor.Reset(); @@ -400,20 +223,6 @@ protected override async Task ProcessOutputsAsync(CancellationToken token) catch { } } - /// - /// Initialize channel data when output processing is going to be started - /// - protected virtual void InitializeChannelInfo(int channel, - int lastOutputValue = 1, - int sendAttempsLeft = MAX_SEND_ATTEMPTS) - { - _outputValues[channel] = 0; - _lastOutputValues[channel] = lastOutputValue; - _sendAttemptsLeft[channel] = sendAttempsLeft; - _positionsUpdated[channel] = false; - _positionUpdateTimes[channel] = DateTime.MinValue; - } - protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { try @@ -450,7 +259,7 @@ private async Task SendOutputValuesAsync(CancellationToken token) { var result = true; - for (int channel = 0; channel < NumberOfChannels; channel++) + for (int channel = 0; channel < NumberOfChannels && !token.IsCancellationRequested; channel++) { switch (_channelOutputTypes[channel]) { @@ -459,7 +268,6 @@ private async Task SendOutputValuesAsync(CancellationToken token) break; case ChannelOutputType.ServoMotor: - var maxServoAngle = _maxServoAngles[channel]; result = result && await SendServoOutputValueAsync(channel, token); break; @@ -495,12 +303,12 @@ private async Task SendOutputValueAsync(int channel, CancellationToken tok _lastSent_NormalMotor.Elapsed > ResendDelay_NormalMotor) { var outputCmd = GetOutputCommand(channel, v); - if (await _bleDevice!.WriteNoResponseAsync(_characteristic!, outputCmd, token)) + if (await WriteNoResponseAsync(outputCmd, token)) { _lastSent_NormalMotor.Restart(); _lastOutputValues[channel] = v; - ResetSendAttemps(channel, 0); + ResetSendAttempts(channel, 0); await Task.Delay(SEND_DELAY, token); return true; } @@ -528,7 +336,7 @@ private async Task SendOutputValueVirtualAsync(int virtualChannel, int cha _virtualPortSendBuffer[6] = (byte)(value1 < 0 ? (255 + value1) : value1); _virtualPortSendBuffer[7] = (byte)(value2 < 0 ? (255 + value2) : value2); - if (await _bleDevice!.WriteNoResponseAsync(_characteristic!, _virtualPortSendBuffer, token)) + if (await WriteNoResponseAsync(_virtualPortSendBuffer, token)) { _lastOutputValues[channel1] = value1; _lastOutputValues[channel2] = value2; @@ -574,10 +382,10 @@ private async Task SendServoOutputValueAsync(int channel, CancellationToke } var servoCmd = GetServoCommand(channel, servoValue, servoSpeed); - if (await _bleDevice!.WriteNoResponseAsync(_characteristic!, servoCmd, token)) + if (await WriteNoResponseAsync(servoCmd, token)) { _lastOutputValues[channel] = v; - ResetSendAttemps(channel, 0); + ResetSendAttempts(channel, 0); await Task.Delay(SEND_DELAY, token); return true; } @@ -618,10 +426,10 @@ private async Task SendStepperOutputValueAsync(int channel, CancellationTo if (v != _lastOutputValues[channel] && Math.Abs(v) == 100) { - if (await _bleDevice!.WriteNoResponseAsync(_characteristic!, _stepperSendBuffer, token)) + if (await WriteNoResponseAsync(_stepperSendBuffer, token)) { _lastOutputValues[channel] = v; - ResetSendAttemps(channel, 0); + ResetSendAttempts(channel, 0); await Task.Delay(SEND_DELAY, token); return true; } @@ -655,15 +463,15 @@ protected virtual async Task SetupChannelForPortInformationAsync(int chann var unlockAndEnableBuffer = new byte[] { 0x05, 0x00, 0x42, portId, 0x03 }; var result = true; - result = result && await _bleDevice!.WriteAsync(_characteristic!, lockBuffer, token); + result = result && await WriteAsync(lockBuffer, token); await Task.Delay(20, token); - result = result && await _bleDevice!.WriteAsync(_characteristic!, inputFormatForAbsAngleBuffer, token); + result = result && await WriteAsync(inputFormatForAbsAngleBuffer, token); await Task.Delay(20, token); - result = result && await _bleDevice!.WriteAsync(_characteristic!, inputFormatForRelAngleBuffer, token); + result = result && await WriteAsync(inputFormatForRelAngleBuffer, token); await Task.Delay(20, token); - result = result && await _bleDevice!.WriteAsync(_characteristic!, modeAndDataSetBuffer, token); + result = result && await WriteAsync(modeAndDataSetBuffer, token); await Task.Delay(20, token); - result = result && await _bleDevice!.WriteAsync(_characteristic!, unlockAndEnableBuffer, token); + result = result && await WriteAsync(unlockAndEnableBuffer, token); return result; } @@ -696,7 +504,7 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C var diff = Math.Abs(NormalizeAngle(_absolutePositions[channel] - baseAngle)); if (diff > 5) { - // Can't reset to base angle, rebease to current position not to stress the plastic + // Can't reset to base angle, rebase to current position not to stress the plastic result = result && await ResetAsync(channel, 0, token); result = result && await StopAsync(channel, token); result = result && await TurnAsync(channel, 0, 40, token); @@ -762,7 +570,7 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C } } - private int NormalizeAngle(int angle) + private static int NormalizeAngle(int angle) { if (angle >= 180) { @@ -776,7 +584,7 @@ private int NormalizeAngle(int angle) return angle; } - private int RoundAngleToNearest90(int angle) + private static int RoundAngleToNearest90(int angle) { angle = NormalizeAngle(angle); if (angle < -135) return -180; @@ -813,7 +621,7 @@ private int CalculateServoSpeed(int channel, int targetAngle) private Task StopAsync(int channel, CancellationToken token) { var portId = GetPortId(channel); - return _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x08, 0x00, 0x81, portId, 0x11, 0x51, 0x00, 0x00 }, token); + return WriteAsync([0x08, 0x00, 0x81, portId, 0x11, 0x51, 0x00, 0x00], token); } private Task TurnAsync(int channel, int angle, int speed, CancellationToken token) @@ -826,7 +634,7 @@ private Task TurnAsync(int channel, int angle, int speed, CancellationToke var a2 = (byte)((angle >> 16) & 0xff); var a3 = (byte)((angle >> 24) & 0xff); - return _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x0e, 0x00, 0x81, portId, 0x11, 0x0d, a0, a1, a2, a3, (byte)speed, 0x64, 0x7e, 0x00 }, token); + return WriteAsync([0x0e, 0x00, 0x81, portId, 0x11, 0x0d, a0, a1, a2, a3, (byte)speed, 0x64, 0x7e, 0x00], token); } private Task ResetAsync(int channel, int angle, CancellationToken token) @@ -839,99 +647,7 @@ private Task ResetAsync(int channel, int angle, CancellationToken token) var a2 = (byte)((angle >> 16) & 0xff); var a3 = (byte)((angle >> 24) & 0xff); - return _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x0b, 0x00, 0x81, portId, 0x11, 0x51, 0x02, a0, a1, a2, a3 }, token); - } - - private async Task RequestHubPropertiesAsync(CancellationToken token) - { - try - { - // Request firmware version - await Task.Delay(TimeSpan.FromMilliseconds(300), token); - await _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x05, 0x00, 0x01, 0x03, 0x05 }, token); - var data = await _bleDevice!.ReadAsync(_characteristic!, token); - ProcessHubPropertyData(data); - - // Request hardware version - await Task.Delay(TimeSpan.FromMilliseconds(300), token); - await _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x05, 0x00, 0x01, 0x04, 0x05 }, token); - data = await _bleDevice!.ReadAsync(_characteristic!, token); - ProcessHubPropertyData(data); - - // Request battery voltage - await Task.Delay(TimeSpan.FromMilliseconds(300), token); - await _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x05, 0x00, 0x01, 0x06, 0x05 }, token); - data = await _bleDevice!.ReadAsync(_characteristic!, token); - ProcessHubPropertyData(data); - } - catch { } - } - - private void ProcessHubPropertyData(byte[]? data) - { - try - { - if (data is null || data.Length < 6) - { - return; - } - - var dataLength = data[0]; - var messageId = data[2]; - var propertyId = data[3]; - var propertyOperation = data[4]; - - if (messageId != MESSAGE_TYPE_HUB_PROPERTIES || propertyOperation != HUB_PROPERTY_OPERATION_UPDATE) - { - // Operation is not 'update' - return; - } - - switch (propertyId) - { - case HUB_PROPERTY_FW_VERSION: // FW version - var firmwareVersion = ProcessVersionNumber(data, 5); - if (!string.IsNullOrEmpty(firmwareVersion)) - { - FirmwareVersion = firmwareVersion; - } - break; - - case HUB_PROPERTY_HW_VERSION: // HW version - var hardwareVersion = ProcessVersionNumber(data, 5); - if (!string.IsNullOrEmpty(hardwareVersion)) - { - HardwareVersion = hardwareVersion; - } - break; - - case HUB_PROPERTY_VOLTAGE: // Battery voltage - var voltage = data[5]; - BatteryVoltage = voltage.ToString("F0"); - break; - } - } - catch { } - } - - private string ProcessVersionNumber(byte[] data, int index) - { - if (data.Length < index + 4) - { - return string.Empty; - } - - var v0 = data[index]; - var v1 = data[index + 1]; - var v2 = data[index + 2]; - var v3 = data[index + 3]; - - var major = v3 >> 4; - var minor = v3 & 0xf; - var bugfix = ((v2 >> 4) * 10) + (v2 & 0xf); - var build = ((v1 >> 4) * 1000) + ((v1 & 0xf) * 100) + ((v0 >> 4) * 10) + (v0 & 0xf); - - return $"{major}.{minor}.{bugfix}.{build}"; + return WriteAsync([0x0b, 0x00, 0x81, portId, 0x11, 0x51, 0x02, a0, a1, a2, a3], token); } } } diff --git a/BrickController2/BrickController2/DeviceManagement/Device.cs b/BrickController2/BrickController2/DeviceManagement/Device.cs index ccf2c7df..a3a91d01 100644 --- a/BrickController2/BrickController2/DeviceManagement/Device.cs +++ b/BrickController2/BrickController2/DeviceManagement/Device.cs @@ -177,12 +177,13 @@ public override string ToString() return Name; } - protected void CheckChannel(int channel) + protected int CheckChannel(int channel) { if (channel < 0 || channel >= NumberOfChannels) { throw new ArgumentOutOfRangeException($"Invalid channel value: {channel}."); } + return channel; } protected float CutOutputValue(float outputValue) diff --git a/BrickController2/BrickController2/DeviceManagement/IO/DeviceInitializationWaiter.cs b/BrickController2/BrickController2/DeviceManagement/IO/DeviceInitializationWaiter.cs new file mode 100644 index 00000000..08a58a3a --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/IO/DeviceInitializationWaiter.cs @@ -0,0 +1,52 @@ +using System; +using System.Threading; +using System.Threading.Tasks; + +namespace BrickController2.DeviceManagement.IO; + +internal class DeviceInitializationWaiter +{ + // Time to wait after the LAST port message before assuming sync is complete + private static readonly TimeSpan BurstTimeout = TimeSpan.FromMilliseconds(500); + // Absolute maximum time to wait to prevent infinite hanging + private static readonly TimeSpan AbsoluteTimeout = TimeSpan.FromMilliseconds(4000); + + private readonly TaskCompletionSource _initializationTcs = new(); + private CancellationTokenSource _debounceCts = new(); + + /// + /// Waits until the debounce window elapses with no new port events, + /// or until the absolute timeout is reached. + /// + /// true if initialization completed normally; false if the absolute timeout elapsed. + public async Task WaitAsync(CancellationToken token) + { + var absoluteTimeoutTask = Task.Delay(AbsoluteTimeout, token); + var completedTask = await Task.WhenAny(_initializationTcs.Task, absoluteTimeoutTask); + + if (completedTask == absoluteTimeoutTask) + { + return false; + } + + return await _initializationTcs.Task; + } + + /// + /// Resets the sliding debounce window. Call this each time a port attach event is received. + /// + public void NotifyPortAttached() + { + _debounceCts?.Cancel(); + _debounceCts?.Dispose(); + _debounceCts = new CancellationTokenSource(); + + Task.Delay(BurstTimeout, _debounceCts.Token).ContinueWith(t => + { + if (!t.IsCanceled && _initializationTcs is { Task.IsCompleted: false }) + { + _initializationTcs.TrySetResult(true); + } + }); + } +} diff --git a/BrickController2/BrickController2/DeviceManagement/IO/OutputValuesGroup.cs b/BrickController2/BrickController2/DeviceManagement/IO/OutputValuesGroup.cs index cf71602a..71169ae9 100644 --- a/BrickController2/BrickController2/DeviceManagement/IO/OutputValuesGroup.cs +++ b/BrickController2/BrickController2/DeviceManagement/IO/OutputValuesGroup.cs @@ -54,6 +54,18 @@ public void Initialize() } } + public void Clear() + { + lock (_outputLock) + { + // reset all values + _outputValues.AsSpan().Clear(); + _commitedOutputValues.AsSpan().Clear(); + _values.AsSpan().Clear(); + _sendAttemptsLeft = 0; + } + } + /// /// Try to get the output values to be sent, if there was any change or last application was not succcessfull /// @@ -75,7 +87,7 @@ public bool TryGetValues(out ReadOnlySpan values) } /// - /// Try to get the output values to be sent, if there was any change or last application was not succcessfull + /// Try to get the output values to be sent, if there was any change or last application was not successful /// /// Collection of changes /// true there is any reason to apply changes @@ -104,12 +116,12 @@ public bool TryGetChanges(out IReadOnlyCollection> chan /// /// Confirm that the values have been sent and applied. /// - public void Commmit() + public void Commit() { // store as last applied values _values.CopyTo(_commitedOutputValues.AsSpan()); - // reset attemps due to success + // reset attempts due to success lock (_outputLock) { // do it conditionally diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs b/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs new file mode 100644 index 00000000..11abe135 --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs @@ -0,0 +1,323 @@ +using BrickController2.DeviceManagement.IO; +using BrickController2.PlatformServices.BluetoothLE; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Threading; +using System.Threading.Tasks; + +using static BrickController2.Protocols.LegoWirelessProtocol; + +namespace BrickController2.DeviceManagement.Lego; + +internal abstract class ControlPlusDeviceBase : BluetoothDevice +{ + protected static readonly TimeSpan SEND_DELAY = TimeSpan.FromMilliseconds(25); + + private DeviceInitializationWaiter? _initializationWaiter; + + protected readonly HashSet AttachedChannels = []; + + protected IGattCharacteristic? Characteristic; + + protected ControlPlusDeviceBase(string name, string address, IDeviceRepository deviceRepository, IBluetoothLEService bleService) + : base(name, address, deviceRepository, bleService) + { + } + + public override string BatteryVoltageSign => "%"; + + protected virtual byte GetPortId(int channelIndex) => (byte)channelIndex; + protected virtual bool TryGetChannelIndex(byte portId, out int channelIndex) + { + channelIndex = portId; + return portId < NumberOfChannels; + } + + protected override async Task ValidateServicesAsync(IEnumerable? services, CancellationToken token) + { + var service = services?.FirstOrDefault(s => s.Uuid == ServiceUuid); + Characteristic = service?.Characteristics?.FirstOrDefault(c => c.Uuid == CharacteristicUuid); + + if (Characteristic is not null) + { + // reset channel state + AttachedChannels.Clear(); + // init waiter + _initializationWaiter = new(); + + return await _bleDevice!.EnableNotificationAsync(Characteristic, token); + } + + return false; + } + + protected override void OnDeviceDisconnecting() + { + // reset channel state + AttachedChannels.Clear(); + _initializationWaiter = null; + Characteristic = null; + } + + protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] data) + { + if (characteristicGuid != CharacteristicUuid || data.Length < 4) + { + return; + } + + var messageCode = data[2]; + + switch (messageCode) + { + case 0x01: // Hub properties + ProcessHubPropertyData(data); + break; + + case 0x02: // Hub actions + DumpData("Hub actions", data); + break; + + case 0x03: // Hub alerts + DumpData("Hub alerts", data); + break; + + case 0x04: // Hub attached I/O + DumpData("Hub attached I/O", data); + byte portId = data[3]; + byte eventType = data[4]; // 0x01 = Attached, 0x00 = Detached, 0x02 = Attached Virtual + + if (TryGetChannelIndex(portId, out var channel)) + { + if (eventType == 0x01 || eventType == 0x02) + { + AttachedChannels.Add(channel); + _initializationWaiter?.NotifyPortAttached(); + } + else if (eventType == 0x00) + { + AttachedChannels.Remove(channel); + } + } + break; + + case 0x05: // Generic error messages + DumpData("Generic error messages", data); + break; + + case 0x08: // HW network commands + DumpData("HW network commands", data); + break; + + case 0x13: // FW lock status + DumpData("FW lock status", data); + break; + + case 0x43: // Port information + DumpData("Port information", data); + break; + + case 0x44: // Port mode information + DumpData("Port mode information", data); + break; + + case 0x45: // Port value (single mode) + //lock (_positionLock) + //{ + // if (data.Length == 6) + // { + // // assume 16bit data is ABS + // if (TryGetChannelIndex(data[3], out var channel)) + // { + // var absPosition = ToInt16(data, 4); + // _absolutePositions[channel] = absPosition; + // } + // } + // else if (data.Length == 8) + // { + // // assume 32 bit data is REL + // if (TryGetChannelIndex(data[3], out var channel)) + // { + // var relPosition = ToInt32(data, 4); + // _relativePositions[channel] = relPosition; + + // _positionsUpdated[channel] = true; + // _positionUpdateTimes[channel] = DateTime.Now; + // } + // } + //} + break; + + case 0x46: // Port value (combined mode) + //lock (_positionLock) + //{ + // if (!TryGetChannelIndex(data[3], out var channel)) + // { + // break; + // } + + // var modeMask = data[5]; + // var dataIndex = 6; + + // if ((modeMask & 0x01) != 0) + // { + // var absPosition = ToInt32(data, dataIndex); + // _absolutePositions[channel] = absPosition; + + // dataIndex += 2; + // } + + // if ((modeMask & 0x02) != 0) + // { + // // TODO: Read the post value format response and determine the value length accordingly + // if ((dataIndex + 3) < data.Length) + // { + // var relPosition = ToInt32(data, dataIndex); + // _relativePositions[channel] = relPosition; + // } + // else if ((dataIndex + 1) < data.Length) + // { + // var relPosition = ToInt16(data, dataIndex); + // _relativePositions[channel] = relPosition; + // } + // else + // { + // _relativePositions[channel] = data[dataIndex]; + // } + + // _positionsUpdated[channel] = true; + // _positionUpdateTimes[channel] = DateTime.Now; + // } + //} + + break; + + case 0x47: // Port input format (Single mode) + DumpData("Port input format (single)", data); + break; + + case 0x48: // Port input format (Combined mode) + DumpData("Port input format (combined)", data); + break; + + case 0x82: // Port output command feedback + OnPortOutputCommandFeedback(data); + break; + } + } + + protected virtual void OnPortOutputCommandFeedback(ReadOnlySpan data) + { +#if DEBUG + DumpData("Output command feedback", data); +#endif + } + + protected virtual void OnHubPropertyData(byte messageId, byte propertyId, ReadOnlySpan propertyData) + { + switch (propertyId) + { + case HUB_PROPERTY_FW_VERSION: // FW version + var firmwareVersion = GetVersionString(propertyData); + if (!string.IsNullOrEmpty(firmwareVersion)) + { + FirmwareVersion = firmwareVersion; + } + break; + + case HUB_PROPERTY_HW_VERSION: // HW version + var hardwareVersion = GetVersionString(propertyData); + if (!string.IsNullOrEmpty(hardwareVersion)) + { + HardwareVersion = hardwareVersion; + } + break; + + case HUB_PROPERTY_VOLTAGE: // Battery voltage + var voltage = propertyData[0]; + BatteryVoltage = voltage.ToString("F0"); + break; + } + } + + protected async Task WriteNoResponseAsync(byte[] data, TimeSpan sentDelay, CancellationToken token = default) + { + var result = await _bleDevice!.WriteNoResponseAsync(Characteristic!, data, token); + await Task.Delay(sentDelay, token); + return result; + } + + protected Task WriteNoResponseAsync(byte[] data, CancellationToken token = default) + => _bleDevice!.WriteNoResponseAsync(Characteristic!, data, token); + + protected Task WriteAsync(byte[] data, CancellationToken token = default) + => _bleDevice!.WriteAsync(Characteristic!, data, token); + + protected async Task RequestHubPropertiesAsync(CancellationToken token) + { + try + { + // Request firmware version + await RequestHubPropertyAsync(HUB_PROPERTY_FW_VERSION, token); + // Request hardware version + await RequestHubPropertyAsync(HUB_PROPERTY_HW_VERSION, token); + // Request battery voltage + await RequestHubPropertyAsync(HUB_PROPERTY_VOLTAGE, token); + } + catch { } + } + + protected async Task RequestHubPropertyAsync(byte propertyId, CancellationToken token) + { + try + { + // Request firmware version + await Task.Delay(TimeSpan.FromMilliseconds(100), token); + await _bleDevice!.WriteAsync(Characteristic!, [0x05, 0x00, 0x01, propertyId, 0x05], token); + var data = await _bleDevice!.ReadAsync(Characteristic!, token); + ProcessHubPropertyData(data); + } + catch { } + } + + /// + /// Call this immediately after enabling characteristic notifications. + /// + protected Task WaitForInitializationAsync(CancellationToken token) + => _initializationWaiter?.WaitAsync(token) ?? Task.FromResult(false); + + private void ProcessHubPropertyData(ReadOnlySpan data) + { + try + { + if (data.Length < 6) + { + return; + } + + var messageId = data[2]; + var propertyId = data[3]; + var propertyOperation = data[4]; + + if (messageId != MESSAGE_TYPE_HUB_PROPERTIES || propertyOperation != HUB_PROPERTY_OPERATION_UPDATE) + { + // Operation is not 'update' + return; + } + + OnHubPropertyData(messageId, propertyId, data[5..]); + } + catch { } + } + + + private static void DumpData(string header, ReadOnlySpan data) + { +#if DEBUG + var s = Convert.ToHexString(data); + Debug.WriteLine(DateTime.Now + " " + header + " - " + s); +#endif + } +} diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs index a8f8fb3e..9c5bd23d 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs @@ -15,12 +15,11 @@ namespace BrickController2.DeviceManagement.Lego; /// /// Represents a LEGO® Powered Up 88010 Remote Control /// -internal class RemoteControl : BluetoothDevice +internal class RemoteControl : ControlPlusDeviceBase { private const string ENABLED_SETTING_NAME = "RemoteControlEnabled"; private const bool DEFAULT_ENABLED = false; - private IGattCharacteristic? _characteristic; private InputDeviceBase? _inputController; public RemoteControl(string name, string address, IEnumerable settings, IDeviceRepository deviceRepository, IBluetoothLEService bleService) @@ -33,8 +32,6 @@ public RemoteControl(string name, string address, IEnumerable sett public override int NumberOfChannels => 0; - public override string BatteryVoltageSign => "%"; - public bool IsEnabled => GetSettingValue(ENABLED_SETTING_NAME, DEFAULT_ENABLED); protected override bool AutoConnectOnFirstConnect => false; @@ -64,19 +61,6 @@ internal void ResetEvents() => RaiseButtonEvents( protected override Task ProcessOutputsAsync(CancellationToken token) => Task.CompletedTask; - protected override async Task ValidateServicesAsync(IEnumerable? services, CancellationToken token) - { - var service = services?.FirstOrDefault(s => s.Uuid == ServiceUuid); - _characteristic = service?.Characteristics?.FirstOrDefault(c => c.Uuid == CharacteristicUuid); - - if (_characteristic is not null) - { - return await _bleDevice!.EnableNotificationAsync(_characteristic, token); - } - - return false; - } - protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { // wait until ports finish communicating with the hub @@ -84,17 +68,15 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf if (requestDeviceInformation) { - // Request battery voltage - await _bleDevice!.WriteAsync(_characteristic!, [0x05, 0x00, 0x01, 0x06, 0x05], token); - await Task.Delay(TimeSpan.FromMilliseconds(50), token); + await RequestHubPropertiesAsync(token); } // setup ports - 0x04 - REMOTE_MODE_KEYS var remoteButtonA = BuildPortInputFormatSetup(REMOTE_BUTTONS_LEFT, REMOTE_MODE_KEYS, interval: 1); - await _bleDevice!.WriteAsync(_characteristic!, remoteButtonA, token); + await _bleDevice!.WriteAsync(Characteristic!, remoteButtonA, token); var remoteButtonB = BuildPortInputFormatSetup(REMOTE_BUTTONS_RIGHT, REMOTE_MODE_KEYS, interval: 1); - return await _bleDevice!.WriteAsync(_characteristic!, remoteButtonB, token); + return await _bleDevice!.WriteAsync(Characteristic!, remoteButtonB, token); } protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] data) diff --git a/BrickController2/BrickController2/DeviceManagement/MouldKing/MK_DIY.cs b/BrickController2/BrickController2/DeviceManagement/MouldKing/MK_DIY.cs index 752e8353..bc172179 100644 --- a/BrickController2/BrickController2/DeviceManagement/MouldKing/MK_DIY.cs +++ b/BrickController2/BrickController2/DeviceManagement/MouldKing/MK_DIY.cs @@ -75,6 +75,11 @@ protected override Task ValidateServicesAsync(IEnumerable? s return Task.FromResult(_characteristic_AE3B_CMD != null); } + protected override void OnDeviceDisconnecting() + { + _characteristic_AE3B_CMD = null; + } + protected override async Task ProcessOutputsAsync(CancellationToken token) { try diff --git a/BrickController2/BrickController2/DeviceManagement/PfxBrickDevice.cs b/BrickController2/BrickController2/DeviceManagement/PfxBrickDevice.cs index 71f903db..ff7e515a 100644 --- a/BrickController2/BrickController2/DeviceManagement/PfxBrickDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/PfxBrickDevice.cs @@ -84,6 +84,12 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] } } + protected override void OnDeviceDisconnecting() + { + _writeCharacteristic = null; + _notifyCharacteristic = null; + } + protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { try @@ -115,7 +121,7 @@ protected override async Task ProcessOutputsAsync(CancellationToken token) if (await SendOutputValuesAsync(motorChanges, token).ConfigureAwait(false)) { // confirm successfull sending - _motorOutputs.Commmit(); + _motorOutputs.Commit(); await Task.Delay(5, token).ConfigureAwait(false); } changed = true; @@ -127,7 +133,7 @@ protected override async Task ProcessOutputsAsync(CancellationToken token) if (await SendLightValuesAsync(lightChanges, token).ConfigureAwait(false)) { // confirm successfull sending - _lightOutputs.Commmit(); + _lightOutputs.Commit(); await Task.Delay(5, token).ConfigureAwait(false); } changed = true; diff --git a/BrickController2/BrickController2/DeviceManagement/SBrickDevice.cs b/BrickController2/BrickController2/DeviceManagement/SBrickDevice.cs index 99972cd0..988ead47 100644 --- a/BrickController2/BrickController2/DeviceManagement/SBrickDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/SBrickDevice.cs @@ -94,6 +94,14 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf return true; } + protected override void OnDeviceDisconnecting() + { + _firmwareRevisionCharacteristic = null; + _hardwareRevisionCharacteristic = null; + _remoteControlCharacteristic = null; + _quickDriveCharacteristic = null; + } + protected override async Task ProcessOutputsAsync(CancellationToken token) { try diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index bd3e28b6..7ecd53c5 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -1,4 +1,6 @@ using BrickController2.CreationManagement; +using BrickController2.DeviceManagement.IO; +using BrickController2.DeviceManagement.Lego; using BrickController2.PlatformServices.BluetoothLE; using BrickController2.Settings; using System; @@ -6,23 +8,34 @@ using System.Linq; using System.Threading; using System.Threading.Tasks; + using static BrickController2.Protocols.LegoWirelessProtocol; namespace BrickController2.DeviceManagement { - internal class TechnicMoveDevice : ControlPlusDevice + internal class TechnicMoveDevice : ControlPlusDeviceBase { public const int CHANNEL_VM = 12; // artificial channel to mimic combined AB ports in PLAYVM + private const int CHANNEL_A = 0; + private const int CHANNEL_B = 1; private const int CHANNEL_C = 2; + private const int CHANNEL_1 = 3; // Light #1 + private const int CHANNEL_6 = 8; // Light #6 + private const int PLAYVM_CHANNEL_DRIVE = 0; + private const int PLAYVM_CHANNEL_STEER = 1; private const string EnablePlayVmSettingName = "PlayVmEnabled"; + private readonly OutputValuesGroup _outputValues = new(9); + private readonly OutputValuesGroup _playVmValues = new(2); + + private int _maxServoAngle; + private int _servoBaseAngle; private bool _applyPlayVmMode; - private volatile byte _virtualMotorValue; + private TaskCompletionSource? _playVmCalibrationTcs; public TechnicMoveDevice(string name, string address, - byte[] deviceData, IEnumerable settings, IDeviceRepository deviceRepository, IBluetoothLEService bleService) @@ -37,7 +50,8 @@ public TechnicMoveDevice(string name, public bool EnablePlayVmMode => GetSettingValue(EnablePlayVmSettingName, true); - public override bool CanAutoCalibrateOutput(int channel) => false; + protected override bool AutoConnectOnFirstConnect => true; + public override bool CanResetOutput(int channel) => EnablePlayVmMode && channel == CHANNEL_C; public override bool IsOutputTypeSupported(int channel, ChannelOutputType outputType) @@ -57,27 +71,44 @@ public override Task ConnectAsync(bool reconnect, Action _applyPlayVmMode = startOutputProcessing && channelConfigurations.Any(c => c.Channel == CHANNEL_VM || (c.Channel == CHANNEL_C && c.ChannelOutputType == ChannelOutputType.ServoMotor)); - // filter out non standard channels - var filteredConfigurtions = channelConfigurations - .Where(c => c.Channel != CHANNEL_VM); + // filter out non-standard channels and configurations with unsupported output types + var filteredConfigurations = channelConfigurations + .Where(c => c.Channel != CHANNEL_VM) + .Where(c => IsOutputTypeSupported(c.Channel, c.ChannelOutputType)) + .ToArray(); + + // update servo config, if set + var servoConfig = filteredConfigurations.FirstOrDefault(c => c.Channel == CHANNEL_C && c.ChannelOutputType == ChannelOutputType.ServoMotor); + _maxServoAngle = servoConfig.MaxServoAngle; + _servoBaseAngle = servoConfig.ServoBaseAngle; - return base.ConnectAsync(reconnect, onDeviceDisconnected, filteredConfigurtions, startOutputProcessing, requestDeviceInformation, token); + return base.ConnectAsync(reconnect, onDeviceDisconnected, filteredConfigurations, startOutputProcessing, requestDeviceInformation, token); } public override void SetOutput(int channel, float value) { - if (channel == CHANNEL_VM) - { - // reset servo writes to enforce update - ResetSendAttemps(CHANNEL_C); - // store virtual motor value to be later send with PLAYVM - var intValue = (int)(100 * CutOutputValue(value)); - _virtualMotorValue = GetChannelValue(intValue); - } - else + var rawValue = (Half)(100 * CutOutputValue(value)); + + _ = channel switch { - base.SetOutput(channel, value); - } + // store A+B virtual channel value for PLAYVM + CHANNEL_VM => _playVmValues.SetOutput(PLAYVM_CHANNEL_DRIVE, rawValue), + // store C channel value for PLAYVM + CHANNEL_C when _applyPlayVmMode => _playVmValues.SetOutput(PLAYVM_CHANNEL_STEER, rawValue), + // Light channels 1 - 6 require absolute value + >= CHANNEL_1 and <= CHANNEL_6 => _outputValues.SetOutput(channel, Half.Abs(rawValue)), + // rest of ports: such as A, B or C when not in PLAYVM mode - use value as is + _ => _outputValues.SetOutput(CheckChannel(channel), rawValue) + }; + } + + public override async Task ResetOutputAsync(int channel, float value, CancellationToken token) + { + CheckChannel(channel); + + await SetupChannelForPortInformationAsync(token); + await Task.Delay(300, token); + await ResetServoAsync(Convert.ToInt32(value * 180), token); } protected override byte GetPortId(int channelIndex) => channelIndex switch @@ -89,85 +120,91 @@ public override void SetOutput(int channel, float value) _ => throw new ArgumentException($"Value of channel '{channelIndex}' is out of supported range.", nameof(channelIndex)) }; - protected override int GetChannelIndex(byte portId) => portId switch - { - PORT_DRIVE_MOTOR_1 => 0, - PORT_DRIVE_MOTOR_2 => 1, - PORT_STEERING_MOTOR => 2, - // PORT_6LEDS is not supported - _ => throw new ArgumentException($"Value of port ID '{portId}' is out of supported ranges.", nameof(portId)) - }; - - protected override byte GetChannelValue(int value) => ToByte(value); - - protected override void InitializeChannelInfo(int channel, int lastOutputValue = 1, int sendAttempsLeft = 10) + protected override bool TryGetChannelIndex(byte portId, out int channelIndex) { - // if PLAYVM enabled, reset A / B channels diffrently in order to avoid output writes - if (_applyPlayVmMode && channel < CHANNEL_C) + channelIndex = portId switch { - lastOutputValue = 0; - sendAttempsLeft = 0; - } - base.InitializeChannelInfo(channel, lastOutputValue, sendAttempsLeft); + PORT_DRIVE_MOTOR_1 => 0, + PORT_DRIVE_MOTOR_2 => 1, + PORT_STEERING_MOTOR => 2, + // all other ports (PORT_6LEDS, PORT_PLAYVM, PORT_HUB_LED, etc.) are not tracked + _ => -1 + }; + return channelIndex != -1; } - protected override byte[] GetOutputCommand(int channel, int value) + protected override async Task ProcessOutputsAsync(CancellationToken token) { - // 6LED - var ledIndex = channel - 3; - if (ledIndex >= 0) + try { - var rawValue = ToByte(Math.Abs(value)); - var ledMask = ToByte(1 << ledIndex); - return BuildPortOutput_LedMask(PORT_6LEDS, PORT_MODE_0, ledMask, rawValue); - } - return base.GetOutputCommand(channel, value); - } + if (_applyPlayVmMode) + { + _playVmValues.Initialize(); + // output values - clear always lights — suppress initial burst to avoid flooding the hub + _outputValues.Clear(); + } + else + { + _playVmValues.Clear(); + // otherwise all channels to be initialized + _outputValues.Initialize(); + } - protected override byte[] GetServoCommand(int channel, int servoValue, int servoSpeed) - { - if (_applyPlayVmMode) - { - return BuildPortOutput_PlayVm(speedValue: _virtualMotorValue, servoValue: servoValue); + while (!token.IsCancellationRequested) + { + if (!await SendOutputValuesAsync(token).ConfigureAwait(false)) + { + await Task.Delay(10, token).ConfigureAwait(false); + } + } } - return base.GetServoCommand(channel, servoValue, servoSpeed); + catch { } } protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { - if (await base.AfterConnectSetupAsync(requestDeviceInformation, token)) + try { - try + // wait for initialization to complete before sending any commands + await WaitForInitializationAsync(token); + + // hub LED - light blue + await SendPortOutput_HubLedAsync(HUB_LED_COLOR_LIGHT_BLUE, token); + + // switch lights off + var lightsOffCmd = BuildPortOutput_LedMask(PORT_6LEDS, PORT_MODE_0, 0xff, 0x00); + await WriteAsync(lightsOffCmd, token); + await Task.Delay(100, token); + + if (requestDeviceInformation) { - // hub LED - var color = _applyPlayVmMode ? HUB_LED_COLOR_MAGENTA : HUB_LED_COLOR_WHITE; - var ledCmd = BuildPortOutput_HubLed(PORT_HUB_LED, HUB_LED_MODE_COLOR, color); - await WriteNoResponseAsync(ledCmd, withSendDelay: true, token: token); - - // switch lights off - var lightsOffCmd = BuildPortOutput_LedMask(PORT_6LEDS, PORT_MODE_0, 0xff, 0x00); - return await WriteNoResponseAsync(lightsOffCmd, withSendDelay: true, token: token); + await RequestHubPropertiesAsync(token); } - catch + + if (_applyPlayVmMode) { + // setup C channel for port information and reset servo to base angles + await SetupChannelForPortInformationAsync(token); + await Task.Delay(300, token); + await ResetServoAsync(_servoBaseAngle, token); + + // hub LED - magenta + await SendPortOutput_HubLedAsync(HUB_LED_COLOR_MAGENTA, token); } + return true; } - - return false; - } - - protected override async Task SetupChannelForPortInformationAsync(int channel, CancellationToken token) - { - if (!EnablePlayVmMode) + catch { - return await base.SetupChannelForPortInformationAsync(channel, token); + return false; } + } + private async Task SetupChannelForPortInformationAsync(CancellationToken token) + { try { - // setup channel to report ABS position - var portId = GetPortId(channel); - var inputFormatForAbsAngle = BuildPortInputFormatSetup(portId, PORT_MODE_3); + // setup channel to report ABS position - port mode 3 + var inputFormatForAbsAngle = BuildPortInputFormatSetup(PORT_STEERING_MOTOR, PORT_MODE_3); return await WriteAsync(inputFormatForAbsAngle, token); } catch @@ -176,11 +213,11 @@ protected override async Task SetupChannelForPortInformationAsync(int chan } } - protected override async Task ResetServoAsync(int channel, int baseAngle, CancellationToken token) + private async Task ResetServoAsync(int baseAngle, CancellationToken token) { if (!EnablePlayVmMode) { - return await base.ResetServoAsync(channel, baseAngle, token); + return false; } try @@ -188,13 +225,27 @@ protected override async Task ResetServoAsync(int channel, int baseAngle, // reset servo via PLAYVM // PLAYVM cmd supports only servo on C channel var servoCmd = BuildPortOutput_PlayVm(servoValue: baseAngle, vmCmd: PLAYVM_COMMAND); - await WriteNoResponseAsync(servoCmd, token: token); + await WriteAsync(servoCmd, token); await Task.Delay(100, token); + // set up completion waiter before sending calibrate to avoid the race where + // feedback arrives before we start waiting + _playVmCalibrationTcs = new TaskCompletionSource(TaskCreationOptions.RunContinuationsAsynchronously); + // do calibration var calibrateCmd = BuildPortOutput_PlayVm(servoValue: baseAngle, vmCmd: PLAYVM_CALIBRATE_STEERING); - await WriteNoResponseAsync(calibrateCmd, token: token); - await Task.Delay(750, token); + await WriteAsync(calibrateCmd, token); + + // wait for the hub's completion feedback instead of a fixed delay + try + { + await _playVmCalibrationTcs.Task.WaitAsync(TimeSpan.FromSeconds(2), token); + } + catch (TimeoutException) + { + // hub did not respond in time, fall back to a short safety delay + await Task.Delay(500, token); + } return true; } @@ -202,6 +253,124 @@ protected override async Task ResetServoAsync(int channel, int baseAngle, { return false; } + finally + { + _playVmCalibrationTcs = null; + } + } + + protected override void OnPortOutputCommandFeedback(ReadOnlySpan data) + { + // PORT_PLAYVM completion feedback (0x82) signals calibration finished + if (data.Length >= 5 && data[3] == PORT_PLAYVM && (data[4] & 0x02) != 0) + { + _playVmCalibrationTcs?.TrySetResult(true); + } + base.OnPortOutputCommandFeedback(data); + } + + private async Task SendOutputValuesAsync(CancellationToken token) + { + try + { + // conditionally send PLAYVM command if PLAYVM mode is active + var result = await SendPlayVmOutputValueAsync(token); + + // process changes for other channels as it's a light or a classic drive + if (result && _outputValues.TryGetChanges(out var changes)) + { + foreach (KeyValuePair change in changes) + { + var value = ToByte(change.Value); + + var writeTask = change.Key switch + { + // Light channels 1 - 6 require absolute value + >= CHANNEL_1 and <= CHANNEL_6 => SendPortOutput_6LedAsync(ledIndex: change.Key - CHANNEL_1, value, token), + // all channels command + int.MaxValue => SendAllOutputValuesAsync(value, token), + // classic output command for A, B, C channels + _ => SendPortOutput_ValueAsync(change.Key, value, token) + }; + + if (!await writeTask) + { + return false; + } + } + + _outputValues.Commit(); + } + + return result; + } + catch + { + return false; + } + } + + private async Task SendPlayVmOutputValueAsync(CancellationToken token) + { + try + { + if (_applyPlayVmMode && _playVmValues.TryGetValues(out var values)) + { + var speed = ToByte(values[PLAYVM_CHANNEL_DRIVE]); + var servoValue = _maxServoAngle * (int)values[PLAYVM_CHANNEL_STEER] / 100; + var playVmCmd = BuildPortOutput_PlayVm(speed, servoValue); + + if (!await WriteAsync(playVmCmd, token)) + { + await Task.Delay(SEND_DELAY, token); + return false; + } + + // commit when successfully sent + _playVmValues.Commit(); + } + return true; + } + catch + { + return false; + } + } + + private Task SendPortOutput_HubLedAsync(byte color, CancellationToken token) + { + // hub LED + var ledCmd = BuildPortOutput_HubLed(PORT_HUB_LED, HUB_LED_MODE_COLOR, color); + return WriteAsync(ledCmd, token); + } + + private Task SendPortOutput_6LedAsync(int ledIndex, byte value, CancellationToken token) + => SendPortOutput_6LedMaskAsync(ToByte(1 << ledIndex), value, token); + + private Task SendPortOutput_6LedMaskAsync(byte lightMask, byte value, CancellationToken token) + { + var cmd = BuildPortOutput_LedMask(PORT_6LEDS, PORT_MODE_0, lightMask, value); + return WriteNoResponseAsync(cmd, SEND_DELAY, token); + } + + private Task SendPortOutput_ValueAsync(int channel, byte value, CancellationToken token) + { + byte[] cmd = [8, 0x00, 0x81, GetPortId(channel), 0x11, 0x51, 0x00, value]; + return WriteNoResponseAsync(cmd, SEND_DELAY, token); + } + + private async Task SendAllOutputValuesAsync(byte value, CancellationToken token) + { + // all LEDs at once + var result = await SendPortOutput_6LedMaskAsync(PORT_6LEDS_ALL_LIGHTS, value, token); + + // A, B, C channels + foreach (var channel in new[] { CHANNEL_A, CHANNEL_B, CHANNEL_C }) + { + result = result && await SendPortOutput_ValueAsync(channel, value, token); + } + + return result; } } } diff --git a/BrickController2/BrickController2/DeviceManagement/Vengit/SBrickLightDevice.cs b/BrickController2/BrickController2/DeviceManagement/Vengit/SBrickLightDevice.cs index 122f30ab..ba03a6f3 100644 --- a/BrickController2/BrickController2/DeviceManagement/Vengit/SBrickLightDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Vengit/SBrickLightDevice.cs @@ -126,6 +126,13 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf return true; } + protected override void OnDeviceDisconnecting() + { + _firmwareRevisionCharacteristic = null; + _hardwareRevisionCharacteristic = null; + _remoteControlCharacteristic = null; + } + protected override async Task ProcessOutputsAsync(CancellationToken token) { try @@ -187,7 +194,7 @@ private async Task TryProcessChanges(OutputValuesGroup valueBank, by if (success) { // confirm successful sending - valueBank.Commmit(); + valueBank.Commit(); await Task.Delay(5, token).ConfigureAwait(false); return true; } diff --git a/BrickController2/BrickController2/DeviceManagement/Wedo2Device.cs b/BrickController2/BrickController2/DeviceManagement/Wedo2Device.cs index ac58db1d..eb598f01 100644 --- a/BrickController2/BrickController2/DeviceManagement/Wedo2Device.cs +++ b/BrickController2/BrickController2/DeviceManagement/Wedo2Device.cs @@ -98,6 +98,15 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] } } + protected override void OnDeviceDisconnecting() + { + // Clear cached characteristic references to prevent using stale native Android objects on reconnection + _motorCharacteristic = null; + _sensorValueCharacteristic = null; + _inputCharacteristic = null; + _firmwareRevisionCharacteristic = null; + } + protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { try diff --git a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs index a06cb48b..82442215 100644 --- a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs +++ b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs @@ -65,6 +65,9 @@ internal static class LegoWirelessProtocol public const byte HUB_LED_COLOR_RED = 0x09; public const byte HUB_LED_COLOR_WHITE = 0xA; + // 6LEDS port + public const byte PORT_6LEDS_ALL_LIGHTS = 0xFF; + // Hub Property Message(s) public const byte HUB_PROPERTY_FW_VERSION = 0x03; public const byte HUB_PROPERTY_HW_VERSION = 0x04; @@ -98,6 +101,7 @@ public static void ToBytes(int value, out byte b0, out byte b1, out byte b2, out } public static byte ToByte(int value) => (byte)(value & 0xFF); + public static byte ToByte(Half value) => ToByte((int)value); public static short ToInt16(byte[] value, int startIndex) => ToInt16(value.AsSpan(startIndex)); public static int ToInt32(byte[] value, int startIndex) => ToInt32(value.AsSpan(startIndex)); @@ -105,6 +109,26 @@ public static void ToBytes(int value, out byte b0, out byte b1, out byte b2, out public static short ToInt16(ReadOnlySpan value) => BinaryPrimitives.ReadInt16LittleEndian(value); public static int ToInt32(ReadOnlySpan value) => BinaryPrimitives.ReadInt32LittleEndian(value); + public static string GetVersionString(ReadOnlySpan data) + { + if (data.Length < 4) + { + return string.Empty; + } + + var v0 = data[0]; + var v1 = data[1]; + var v2 = data[2]; + var v3 = data[3]; + + var major = v3 >> 4; + var minor = v3 & 0xf; + var bugfix = ((v2 >> 4) * 10) + (v2 & 0xf); + var build = ((v1 >> 4) * 1000) + ((v1 & 0xf) * 100) + ((v0 >> 4) * 10) + (v0 & 0xf); + + return $"{major}.{minor}.{bugfix}.{build}"; + } + // message builders public static byte[] BuildPortInputFormatSetup(byte portId, byte portMode, int interval = 2, byte notification = PORT_VALUE_NOTIFICATION_ENABLED) {