123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121 |
- using System;
- using System.Collections.Generic;
- using ANT_Managed_Library;
- using UnityEngine;
- namespace Sensors.ANT
- {
- [Serializable]
- public struct SpeedSensorConfig
- {
- public int deviceId;
- public float wheelCircumference;
- public SpeedSensorConfig(int deviceId = 0, float wheelCircumference = 2.096f)
- {
- this.deviceId = deviceId;
- this.wheelCircumference = wheelCircumference;
- }
- }
- public struct SpeedSensorData
- {
- public float Speed;
- public float SpeedKmh => Speed * 3.6f;
- public float Distance;
- }
- public class SpeedSensorReceiver: AwaitDevice
- {
- private AntChannel backgroundScanChannel;
- private AntChannel deviceChannel;
- private int prevMeasTimeSpeed;
- private int prevRevCountSpeed;
- private int revCountZero;
- private SpeedSensorData sensorData;
- private int stopRevCounterSpeed;
- private bool connected;
-
- public SpeedSensorConfig Config { get; }
- public override bool Connected => connected;
- public SpeedSensorData SensorData => sensorData;
- public override int DeviceId => Config.deviceId;
- public SpeedSensorReceiver()
- {
- }
- public SpeedSensorReceiver(SpeedSensorConfig config)
- {
- Config = config;
- }
- public SpeedSensorReceiver(int deviceId)
- {
- Config = new SpeedSensorConfig(deviceId);
- }
- public SpeedSensorReceiver(int deviceId, float wheelCircumference)
- {
- Config = new SpeedSensorConfig(deviceId, wheelCircumference);
- }
- public override void Connect(AntDevice device)
- {
- var channelID = AntManager.Instance.GetFreeChannelID();
- deviceChannel = AntManager.Instance.OpenChannel(ANT_ReferenceLibrary.ChannelType.BASE_Slave_Receive_0x00,
- channelID, (ushort) device.deviceNumber, device.deviceType, device.transType, (byte) device.radiofreq,
- (ushort) device.period, false);
- connected = true;
- deviceChannel.onReceiveData += Data;
- deviceChannel.onChannelResponse += ChannelResponse;
- deviceChannel.hideRXFAIL = false;
- }
- //Deal with the received Data
- private void Data(byte[] data)
- {
- //SPEED
- var measTime_speed = data[4] | (data[5] << 8);
- var revCount_speed = data[6] | (data[7] << 8);
- if (prevMeasTimeSpeed != 0 && measTime_speed != prevMeasTimeSpeed && prevMeasTimeSpeed < measTime_speed &&
- prevRevCountSpeed < revCount_speed)
- {
- sensorData.Speed = Config.wheelCircumference * (revCount_speed - prevRevCountSpeed) * 1024 /
- (measTime_speed - prevMeasTimeSpeed);
- stopRevCounterSpeed = 0;
- }
- else
- {
- stopRevCounterSpeed++;
- }
- if (stopRevCounterSpeed >= 5)
- {
- stopRevCounterSpeed = 5;
- sensorData.Speed = 0;
- }
- prevMeasTimeSpeed = measTime_speed;
- prevRevCountSpeed = revCount_speed;
- //DISTANCE
- if (revCountZero == 0)
- revCountZero = revCount_speed;
- sensorData.Distance = Config.wheelCircumference * (revCount_speed - revCountZero);
- }
- private void ChannelResponse(ANT_Response response)
- {
- }
- }
- }
|