mirror of
https://github.com/scottbez1/smartknob.git
synced 2025-09-26 23:09:27 +08:00
BLE read/write fixed
This commit is contained in:
parent
418e20feba
commit
95fc1c23e2
2
.vscode/settings.json
vendored
2
.vscode/settings.json
vendored
@ -7,7 +7,7 @@
|
||||
"editor.defaultFormatter": "rvest.vs-code-prettier-eslint",
|
||||
"editor.formatOnPaste": false,
|
||||
"editor.formatOnType": false,
|
||||
"editor.formatOnSave": true,
|
||||
"editor.formatOnSave": false,
|
||||
"editor.formatOnSaveMode": "file",
|
||||
"files.associations": {
|
||||
"array": "cpp",
|
||||
|
@ -286,7 +286,6 @@
|
||||
console.log('> Profile sent')
|
||||
}
|
||||
else{
|
||||
console.log(res)
|
||||
msg.html("Sending error: "+res.error)
|
||||
console.log('> Command BT error: '+res.error)
|
||||
}
|
||||
|
@ -1,5 +1,4 @@
|
||||
|
||||
'use strict'
|
||||
"use strict";
|
||||
|
||||
// SmartKnob I/O:
|
||||
|
||||
@ -15,8 +14,8 @@
|
||||
|
||||
// Set config / calls
|
||||
// *Motor/Haptic
|
||||
// *Motor calibration
|
||||
// *Push
|
||||
// *Motor calibration
|
||||
// *Push
|
||||
// *Lights (firmare not implemented)
|
||||
|
||||
// Hardware manipulation
|
||||
@ -28,257 +27,227 @@
|
||||
// *Proximity (firmare not implemented)
|
||||
|
||||
const SmartKnob = class {
|
||||
debug = true;
|
||||
|
||||
debug = true
|
||||
ble_nameprefix = "SmartKnob_";
|
||||
|
||||
ble_nameprefix = "SmartKnob_"
|
||||
|
||||
ble_connected = false
|
||||
ble_connected = false;
|
||||
|
||||
debug_messages = {
|
||||
'disconnected': "Disconnected",
|
||||
'connecting': "Connecting",
|
||||
'connected': "Connected",
|
||||
'tryagain': "Try again",
|
||||
'error': "Error"
|
||||
}
|
||||
'error': "Error",
|
||||
};
|
||||
|
||||
serviceUuid = 0x340f // Primary service
|
||||
serviceUuid = 0x0001; // Primary service
|
||||
|
||||
serviceCharacteristics = []
|
||||
TX_UUID = "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"; // Notify
|
||||
RX_UUID = "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"; // Write
|
||||
|
||||
useSingleCharacteristics = true
|
||||
notify_tx_Characteristics = false;
|
||||
write_rx_Characteristics = false;
|
||||
|
||||
useSingleCharacteristics = true;
|
||||
|
||||
characteristicUuids = {
|
||||
'ambient': {
|
||||
'uuid': "0000420f-0000-1000-8000-00805f9b34fb",
|
||||
'functions': ['read', 'write', 'notify', 'indicate']
|
||||
'functions': ["read", "write", "notify", "indicate"],
|
||||
},
|
||||
'scale': {
|
||||
'uuid': "e65dc16e-fb3e-4800-ad86-f78485cc65c2",
|
||||
'functions': ['read', 'notify', 'indicate']
|
||||
'functions': ["read", "notify", "indicate"],
|
||||
},
|
||||
'button': {
|
||||
'uuid': "d769e5b4-601c-4506-b1da-29f64069869d",
|
||||
'functions': ['read', 'notify', 'indicate']
|
||||
'functions': ["read", "notify", "indicate"],
|
||||
},
|
||||
'position': {
|
||||
'uuid': "602f75a0-697d-4690-8dd5-fa52781446d1",
|
||||
'functions': ['read', 'write', 'notify', 'indicate']
|
||||
'functions': ["read", "write", "notify", "indicate"],
|
||||
},
|
||||
'lights': {
|
||||
'uuid': "0000424f-0000-1000-8000-00805f9b34fb",
|
||||
'functions': ['read', 'notify', 'indicate']
|
||||
'functions': ["read", "notify", "indicate"],
|
||||
},
|
||||
'haptic': {
|
||||
'uuid': "0000425f-0000-1000-8000-00805f9b34fb",
|
||||
'functions': ['write']
|
||||
'functions': ["write"],
|
||||
},
|
||||
'config': {
|
||||
'uuid': "0000426f-0000-1000-8000-00805f9b34fb",
|
||||
'functions': ['read', 'write', 'notify', 'indicate']
|
||||
'functions': ["read", "write", "notify", "indicate"],
|
||||
},
|
||||
}
|
||||
};
|
||||
|
||||
constructor() {
|
||||
window.addEventListener('onunload', this.disconnect())
|
||||
}
|
||||
window.addEventListener("onunload", this.disconnect());
|
||||
}
|
||||
|
||||
async connect() {
|
||||
navigator.bluetooth.addEventListener(
|
||||
"availabilitychanged",
|
||||
this.onavailabilitychanged
|
||||
);
|
||||
|
||||
this.notify_tx_Characteristics = false;
|
||||
this.write_rx_Characteristics = false;
|
||||
|
||||
async connect(){
|
||||
|
||||
navigator.bluetooth.addEventListener("availabilitychanged", this.onavailabilitychanged)
|
||||
|
||||
try {
|
||||
|
||||
this.eventDisconnected(this.debug_messages.connecting, "processing")
|
||||
this.log('Requesting Bluetooth Device...');
|
||||
this.eventDisconnected(this.debug_messages.connecting, "processing");
|
||||
this.log("Requesting Bluetooth Device...");
|
||||
const device = await navigator.bluetooth.requestDevice({
|
||||
filters: [{"namePrefix": this.ble_nameprefix}],
|
||||
optionalServices: [this.serviceUuid]
|
||||
filters: [{ namePrefix: this.ble_nameprefix }],
|
||||
optionalServices: [this.serviceUuid],
|
||||
});
|
||||
|
||||
this.log('Connecting to GATT Server...');
|
||||
this.log("Connecting to GATT Server...");
|
||||
const server = await device.gatt.connect();
|
||||
|
||||
this.log('Getting Service...');
|
||||
this.log("Getting Service...");
|
||||
const service = await server.getPrimaryService(this.serviceUuid);
|
||||
|
||||
this.log('Getting List of Characteristics...');
|
||||
let serviceCharacteristicsList = await service.getCharacteristics()
|
||||
this.log(serviceCharacteristicsList)
|
||||
|
||||
if(this.useSingleCharacteristics){
|
||||
let i=0 // Just use the first Characteristics
|
||||
let charac = serviceCharacteristicsList[i]
|
||||
this.log("Getting List of Characteristics...");
|
||||
let serviceCharacteristicsList = await service.getCharacteristics();
|
||||
this.log(serviceCharacteristicsList);
|
||||
|
||||
console.log("startNotifications on "+charac.uuid)
|
||||
this.serviceCharacteristics[i] = charac //await service.getCharacteristic(charac.uuid);
|
||||
for (let i = 0; i < serviceCharacteristicsList.length; i++) {
|
||||
let charac = serviceCharacteristicsList[i];
|
||||
|
||||
await this.serviceCharacteristics[i].startNotifications();
|
||||
this.log('> Notifications started on '+charac.uuid);
|
||||
this.serviceCharacteristics[i].addEventListener('characteristicvaluechanged', this.handleCombinedNotifications);
|
||||
}
|
||||
else{
|
||||
for(let i=0; i<serviceCharacteristicsList.length; i++){
|
||||
let charac = serviceCharacteristicsList[i]
|
||||
if (charac.properties.notify && !this.notify_tx_Characteristics) {
|
||||
console.log("startNotifications on " + charac.uuid);
|
||||
this.notify_tx_Characteristics = charac; //await service.getCharacteristic(charac.uuid);
|
||||
|
||||
console.log("startNotifications on "+charac.uuid)
|
||||
this.serviceCharacteristics[i] = charac //await service.getCharacteristic(charac.uuid);
|
||||
|
||||
console.log(this.serviceCharacteristics[i])
|
||||
console.log(this.notify_tx_Characteristics);
|
||||
// let value = await this.serviceCharacteristics[i].readValue()
|
||||
// console.log("value: "+ value)
|
||||
await this.notify_tx_Characteristics.startNotifications();
|
||||
this.log("> Notifications started on " + charac.uuid);
|
||||
this.notify_tx_Characteristics.addEventListener("characteristicvaluechanged", this.handleCombinedNotifications);
|
||||
}
|
||||
|
||||
await this.serviceCharacteristics[i].startNotifications();
|
||||
this.log('> Notifications started on '+charac.uuid);
|
||||
this.serviceCharacteristics[i].addEventListener('characteristicvaluechanged', this.handleNotifications);
|
||||
if (charac.properties.write && !this.write_rx_Characteristics) {
|
||||
console.log("will Write on " + charac.uuid);
|
||||
this.write_rx_Characteristics = charac; //await service.getCharacteristic(charac.uuid);
|
||||
console.log(this.write_rx_Characteristics);
|
||||
}
|
||||
}
|
||||
|
||||
this.eventConnected()
|
||||
|
||||
|
||||
} catch(error) {
|
||||
this.eventConnected();
|
||||
}
|
||||
catch (error) {
|
||||
// NetworkError: GATT Server is disconnected. Cannot retrieve services. (Re)connect first with `device.gatt.connect`.
|
||||
this.log('! BT Connection error' + error);
|
||||
this.eventDisconnected(this.debug_messages.tryagain, "error")
|
||||
this.log("! BT Connection error" + error);
|
||||
this.eventDisconnected(this.debug_messages.tryagain, "error");
|
||||
}
|
||||
}
|
||||
|
||||
disconnect(){
|
||||
if(this.serviceCharacteristic){
|
||||
|
||||
navigator.bluetooth.removeEventListener("availabilitychanged")
|
||||
disconnect() {
|
||||
if (this.serviceCharacteristic) {
|
||||
navigator.bluetooth.removeEventListener("availabilitychanged");
|
||||
|
||||
this.serviceCharacteristics.forEach(async (charac, i) => {
|
||||
this.charac.stopNotifications()
|
||||
.then(_ => {
|
||||
this.log('> Notifications stopped');
|
||||
this.charac.removeEventListener('characteristicvaluechanged', handleNotifications);
|
||||
})
|
||||
.catch(error => {
|
||||
if(error == "NotFoundError: User cancelled the requestDevice() chooser."){
|
||||
this.eventDisconnected(this.debug_messages.tryagain)
|
||||
this.charac
|
||||
.stopNotifications()
|
||||
.then((_) => {
|
||||
this.log("> Notifications stopped");
|
||||
this.charac.removeEventListener("characteristicvaluechanged", handleNotifications);
|
||||
})
|
||||
.catch((error) => {
|
||||
if (error == "NotFoundError: User cancelled the requestDevice() chooser.") {
|
||||
this.eventDisconnected(this.debug_messages.tryagain);
|
||||
}
|
||||
//NetworkError: GATT operation already in progress.
|
||||
//NotSupportedError: GATT operation failed for unknown reason.
|
||||
//NetworkError: Failed to execute 'writeValue' on 'BluetoothRemoteGATTCharacteristic': GATT Server is disconnected. Cannot perform GATT operations. (Re)connect first with `device.gatt.connect`.
|
||||
//NetworkError: GATT Server is disconnected. Cannot retrieve services. (Re)connect first with `device.gatt.connect`.
|
||||
else{
|
||||
console.log('! BT error: ' + error);
|
||||
this.eventDisconnected(this.debug_messages.error, "error")
|
||||
else {
|
||||
console.log("! BT error: " + error);
|
||||
this.eventDisconnected(this.debug_messages.error, "error");
|
||||
}
|
||||
});
|
||||
})
|
||||
});
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
handleCombinedNotifications(event) {
|
||||
handleCombinedNotifications(event) {
|
||||
let buffer = event.target.value;
|
||||
let response = bufferToString(buffer);
|
||||
|
||||
let buffer = event.target.value;
|
||||
let response = bufferToString(buffer);
|
||||
|
||||
let value = 0
|
||||
for(let i=4; i<buffer.byteLength; i++){
|
||||
value += (i>4) ? buffer.getUint8(i)*(256*(i-4)) : buffer.getUint8(i)
|
||||
}
|
||||
// console.log(buffer, buffer.byteLength, response, value)
|
||||
let value = 0;
|
||||
for (let i = 4; i < buffer.byteLength; i++) {
|
||||
value += i > 4 ? buffer.getUint8(i) * (256 * (i - 4)) : buffer.getUint8(i);
|
||||
}
|
||||
console.log(buffer, buffer.byteLength, response, value);
|
||||
|
||||
switch(buffer.getUint8(0)){
|
||||
case 1:
|
||||
document.dispatchEvent(new CustomEvent('handleButtonNotifications', {detail: {'value': ((buffer.getUint8(1) == 1) ? true : false)}}))
|
||||
break;
|
||||
switch (buffer.getUint8(0)) {
|
||||
case 1:
|
||||
document.dispatchEvent(new CustomEvent("handleButtonNotifications", {
|
||||
detail: { value: buffer.getUint8(1) == 1 ? true : false },
|
||||
}));
|
||||
break;
|
||||
|
||||
case 2:
|
||||
document.dispatchEvent(new CustomEvent('handleScaleNotifications', {detail: {'value': parseFloat(value)}}))
|
||||
break;
|
||||
case 2:
|
||||
document.dispatchEvent(new CustomEvent("handleScaleNotifications", {
|
||||
detail: { value: parseFloat(value) },
|
||||
}));
|
||||
break;
|
||||
|
||||
case 3:
|
||||
document.dispatchEvent(new CustomEvent('handlePushNotifications', {detail: {'value': ((buffer.getUint8(1) == 1) ? true : false)}}))
|
||||
break;
|
||||
|
||||
case 4:
|
||||
|
||||
value = buffer.getUint8(4)+(buffer.getUint8(5)*256)+(buffer.getUint8(6))+(buffer.getUint8(7)*256)
|
||||
if(buffer.getUint8(6) > 0){ // if last two last bits have any value, the value is in negative
|
||||
value = (value-131071)
|
||||
}
|
||||
document.dispatchEvent(new CustomEvent('handlePositionNotifications', {detail: {'value': parseFloat(value)}}))
|
||||
break;
|
||||
|
||||
case 5:
|
||||
document.dispatchEvent(new CustomEvent('handlePositionSetNotifications', {detail: {'value': parseFloat(value-1)}}))
|
||||
break;
|
||||
|
||||
case 6:
|
||||
document.dispatchEvent(new CustomEvent('handleLuxNotifications', {detail: {'value': parseFloat(value-1)}}))
|
||||
break;
|
||||
case 3:
|
||||
document.dispatchEvent(new CustomEvent("handlePushNotifications", {
|
||||
detail: { value: buffer.getUint8(1) == 1 ? true : false },
|
||||
}));
|
||||
break;
|
||||
|
||||
case 4:
|
||||
value =
|
||||
buffer.getUint8(4) +
|
||||
buffer.getUint8(5) * 256 +
|
||||
buffer.getUint8(6) +
|
||||
buffer.getUint8(7) * 256;
|
||||
if (buffer.getUint8(6) > 0) {
|
||||
// if last two last bits have any value, the value is in negative
|
||||
value = value - 131071;
|
||||
}
|
||||
document.dispatchEvent(new CustomEvent("handlePositionNotifications", {
|
||||
detail: { value: parseFloat(value) },
|
||||
}));
|
||||
break;
|
||||
|
||||
case 5:
|
||||
document.dispatchEvent(new CustomEvent("handlePositionSetNotifications", {
|
||||
detail: { value: parseFloat(value - 1) },
|
||||
}));
|
||||
break;
|
||||
|
||||
case 6:
|
||||
document.dispatchEvent(new CustomEvent("handleLuxNotifications", {
|
||||
detail: { value: parseFloat(value - 1) },
|
||||
}));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
async sendProfile(profile){
|
||||
return await this.sendWrite("SP", profile.join(","))
|
||||
async sendProfile(profile) {
|
||||
return await this.sendWrite("SP", profile.join(","));
|
||||
}
|
||||
|
||||
async sendWrite(cmd, value){
|
||||
async sendWrite(cmd, value) {
|
||||
// try{
|
||||
let msg = sk.stringToBuffer(cmd+"+"+value)
|
||||
await this.serviceCharacteristics[0].writeValue(msg)
|
||||
let msg = sk.stringToBuffer(cmd + "+" + value);
|
||||
if (this.write_rx_Characteristics) {
|
||||
await this.write_rx_Characteristics.writeValue(msg);
|
||||
return true;
|
||||
// }
|
||||
// catch(error){
|
||||
// //this.eventDisconnected(this.debug_messages.tryagain, "error")
|
||||
// return {'error': error, 'message': this.debug_messages.tryagain}
|
||||
// }
|
||||
}
|
||||
} else {
|
||||
this.eventDisconnected()
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// handleCombinedNotifications(event) {
|
||||
|
||||
|
||||
// let buffer = event.target.value;
|
||||
// console.log(buffer)
|
||||
|
||||
// // Display raw hex
|
||||
// let a = [];
|
||||
// for (let i = 0; i < buffer.byteLength; i++) {
|
||||
// a.push('0x' + ('00' + buffer.getUint8(i).toString(16)).slice(-2));
|
||||
// }
|
||||
// console.log('>> ' + a.join(' '));
|
||||
|
||||
// // var data = new Int32Array(buffer);
|
||||
// //let data = bufferToString(value)
|
||||
// // let data = new DataView(buffer, 0)
|
||||
// //let data = new Uint8Array(buffer)
|
||||
|
||||
// // bit(0) is for boolean values, like push/button
|
||||
// let bools = buffer.getUint8(0)
|
||||
// let button = ((bools>>0)&1)
|
||||
// let push = ((bools>>1)&1)
|
||||
|
||||
// document.dispatchEvent(new CustomEvent('handleButtonNotifications', {detail: {'value': button}}))
|
||||
// document.dispatchEvent(new CustomEvent('handlePushNotifications', {detail: {'value': push}}))
|
||||
|
||||
// console.log("Bool values: ", button, push)
|
||||
|
||||
// // bit(1) is placeholder, not used now
|
||||
// // buffer.getUint8(1)
|
||||
|
||||
// // bit(2-5) are for current position
|
||||
// let value = buffer.getUint8(2)+(buffer.getUint8(3)*256)+(buffer.getUint8(4)*256)+(buffer.getUint8(5)*256)
|
||||
// if(buffer.getUint8(4) > 0){ // if last two last bits have any value, the value is in negative
|
||||
// value = (value-196096)
|
||||
// }
|
||||
// document.dispatchEvent(new CustomEvent('handlePositionNotifications', {detail: {'value': value}}))
|
||||
|
||||
// // bit(6-7) are for position config
|
||||
// let position_set = buffer.getUint8(6)+(buffer.getUint8(7)*256)
|
||||
// document.dispatchEvent(new CustomEvent('handlePositionSetNotifications', {detail: {'value': position_set}}))
|
||||
|
||||
|
||||
// }
|
||||
handleNotifications(event) {
|
||||
|
||||
console.log(event.currentTarget.uuid)
|
||||
console.log(event.currentTarget.uuid);
|
||||
|
||||
let buffer = event.target.value;
|
||||
|
||||
@ -294,40 +263,51 @@ const SmartKnob = class {
|
||||
// let data = new DataView(buffer, 0)
|
||||
//let data = new Uint8Array(buffer)
|
||||
|
||||
switch(event.currentTarget.uuid){
|
||||
switch (event.currentTarget.uuid) {
|
||||
// Position
|
||||
case SmartKnob.characteristicUuids.position.uuid:{
|
||||
let value = buffer.getUint8(0)+(buffer.getUint8(1)*256)+(buffer.getUint8(2)*256)+(buffer.getUint8(3)*256)
|
||||
if(buffer.getUint8(2) > 0){
|
||||
value = (value-196096)
|
||||
case SmartKnob.characteristicUuids.position.uuid: {
|
||||
let value =
|
||||
buffer.getUint8(0) +
|
||||
buffer.getUint8(1) * 256 +
|
||||
buffer.getUint8(2) * 256 +
|
||||
buffer.getUint8(3) * 256;
|
||||
if (buffer.getUint8(2) > 0) {
|
||||
value = value - 196096;
|
||||
}
|
||||
|
||||
document.dispatchEvent(new CustomEvent('handlePositionNotifications', {detail: {'value': value}}))
|
||||
document.dispatchEvent(
|
||||
new CustomEvent("handlePositionNotifications", {
|
||||
detail: { value: value },
|
||||
})
|
||||
);
|
||||
break;
|
||||
}
|
||||
// Scale
|
||||
case SmartKnob.characteristicUuids.scale.uuid:{
|
||||
let value = buffer.getUint8(0)
|
||||
case SmartKnob.characteristicUuids.scale.uuid: {
|
||||
let value = buffer.getUint8(0);
|
||||
|
||||
document.dispatchEvent(new CustomEvent('handleScaleNotifications', {detail: {'value': value}}))
|
||||
document.dispatchEvent(
|
||||
new CustomEvent("handleScaleNotifications", {
|
||||
detail: { value: value },
|
||||
})
|
||||
);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
onavailabilitychanged(event){
|
||||
this.log("Availabiliy changed: "+event.value)
|
||||
onavailabilitychanged(event) {
|
||||
this.log("Availabiliy changed: " + event.value);
|
||||
}
|
||||
|
||||
eventConnected(){
|
||||
console.log("connected")
|
||||
this.ble_connected = true
|
||||
document.dispatchEvent(new CustomEvent('isConnected', {detail: {'message': this.debug_messages.connected, 'type': "success"}}))
|
||||
eventConnected() {
|
||||
console.log("connected");
|
||||
this.ble_connected = true;
|
||||
document.dispatchEvent(new CustomEvent("isConnected", {detail: { message: this.debug_messages.connected, type: "success" },}));
|
||||
}
|
||||
eventDisconnected(msg=this.debug_messages.disconnected, cls=""){
|
||||
this.ble_connected = false
|
||||
document.dispatchEvent(new CustomEvent('isDisconnected', {detail: {'message': msg, 'type': cls}}))
|
||||
eventDisconnected(msg = this.debug_messages.disconnected, cls = "") {
|
||||
this.ble_connected = false;
|
||||
document.dispatchEvent(new CustomEvent("isDisconnected", { detail: { message: msg, type: cls } }));
|
||||
}
|
||||
|
||||
// Function to add event listener and log who is listening
|
||||
@ -342,73 +322,71 @@ const SmartKnob = class {
|
||||
document.removeEventListener(eventName, handler);
|
||||
// this.log(`${element} is removed from listening for ${eventName}`);
|
||||
}
|
||||
|
||||
|
||||
|
||||
stringToBuffer(str) {
|
||||
let Len = str.length,
|
||||
resPos = -1
|
||||
resPos = -1;
|
||||
// The Uint8Array's length must be at least 3x the length of the string because an invalid UTF-16
|
||||
// takes up the equivalent space of 3 UTF-8 characters to encode it properly.
|
||||
let resArr = new Uint8Array(Len * 3)
|
||||
let resArr = new Uint8Array(Len * 3);
|
||||
for (let point = 0, nextcode = 0, i = 0; i !== Len; ) {
|
||||
point = str.charCodeAt(i)
|
||||
i += 1
|
||||
if (point >= 0xd800 && point <= 0xdbff) {
|
||||
if (i === Len) {
|
||||
resArr[(resPos += 1)] = 0xef /*0b11101111*/
|
||||
resArr[(resPos += 1)] = 0xbf /*0b10111111*/
|
||||
resArr[(resPos += 1)] = 0xbd /*0b10111101*/
|
||||
break
|
||||
}
|
||||
// https://mathiasbynens.be/notes/javascript-encoding#surrogate-formulae
|
||||
nextcode = str.charCodeAt(i)
|
||||
if (nextcode >= 0xdc00 && nextcode <= 0xdfff) {
|
||||
point = (point - 0xd800) * 0x400 + nextcode - 0xdc00 + 0x10000
|
||||
i += 1
|
||||
if (point > 0xffff) {
|
||||
resArr[(resPos += 1)] = (0x1e /*0b11110*/ << 3) | (point >>> 18)
|
||||
resArr[(resPos += 1)] =
|
||||
(0x2 /*0b10*/ << 6) | ((point >>> 12) & 0x3f) /*0b00111111*/
|
||||
resArr[(resPos += 1)] =
|
||||
(0x2 /*0b10*/ << 6) | ((point >>> 6) & 0x3f) /*0b00111111*/
|
||||
resArr[(resPos += 1)] =
|
||||
(0x2 /*0b10*/ << 6) | (point & 0x3f) /*0b00111111*/
|
||||
continue
|
||||
}
|
||||
} else {
|
||||
resArr[(resPos += 1)] = 0xef /*0b11101111*/
|
||||
resArr[(resPos += 1)] = 0xbf /*0b10111111*/
|
||||
resArr[(resPos += 1)] = 0xbd /*0b10111101*/
|
||||
continue
|
||||
}
|
||||
point = str.charCodeAt(i);
|
||||
i += 1;
|
||||
if (point >= 0xd800 && point <= 0xdbff) {
|
||||
if (i === Len) {
|
||||
resArr[(resPos += 1)] = 0xef; /*0b11101111*/
|
||||
resArr[(resPos += 1)] = 0xbf; /*0b10111111*/
|
||||
resArr[(resPos += 1)] = 0xbd; /*0b10111101*/
|
||||
break;
|
||||
}
|
||||
if (point <= 0x007f) {
|
||||
resArr[(resPos += 1)] = (0x0 /*0b0*/ << 7) | point
|
||||
} else if (point <= 0x07ff) {
|
||||
resArr[(resPos += 1)] = (0x6 /*0b110*/ << 5) | (point >>> 6)
|
||||
// https://mathiasbynens.be/notes/javascript-encoding#surrogate-formulae
|
||||
nextcode = str.charCodeAt(i);
|
||||
if (nextcode >= 0xdc00 && nextcode <= 0xdfff) {
|
||||
point = (point - 0xd800) * 0x400 + nextcode - 0xdc00 + 0x10000;
|
||||
i += 1;
|
||||
if (point > 0xffff) {
|
||||
resArr[(resPos += 1)] = (0x1e /*0b11110*/ << 3) | (point >>> 18);
|
||||
resArr[(resPos += 1)] =
|
||||
(0x2 /*0b10*/ << 6) | (point & 0x3f) /*0b00111111*/
|
||||
(0x2 /*0b10*/ << 6) | ((point >>> 12) & 0x3f); /*0b00111111*/
|
||||
resArr[(resPos += 1)] =
|
||||
(0x2 /*0b10*/ << 6) | ((point >>> 6) & 0x3f); /*0b00111111*/
|
||||
resArr[(resPos += 1)] =
|
||||
(0x2 /*0b10*/ << 6) | (point & 0x3f); /*0b00111111*/
|
||||
continue;
|
||||
}
|
||||
} else {
|
||||
resArr[(resPos += 1)] = (0xe /*0b1110*/ << 4) | (point >>> 12)
|
||||
resArr[(resPos += 1)] =
|
||||
(0x2 /*0b10*/ << 6) | ((point >>> 6) & 0x3f) /*0b00111111*/
|
||||
resArr[(resPos += 1)] =
|
||||
(0x2 /*0b10*/ << 6) | (point & 0x3f) /*0b00111111*/
|
||||
resArr[(resPos += 1)] = 0xef; /*0b11101111*/
|
||||
resArr[(resPos += 1)] = 0xbf; /*0b10111111*/
|
||||
resArr[(resPos += 1)] = 0xbd; /*0b10111101*/
|
||||
continue;
|
||||
}
|
||||
}
|
||||
return resArr.subarray(0, resPos + 1)
|
||||
if (point <= 0x007f) {
|
||||
resArr[(resPos += 1)] = (0x0 /*0b0*/ << 7) | point;
|
||||
} else if (point <= 0x07ff) {
|
||||
resArr[(resPos += 1)] = (0x6 /*0b110*/ << 5) | (point >>> 6);
|
||||
resArr[(resPos += 1)] =
|
||||
(0x2 /*0b10*/ << 6) | (point & 0x3f); /*0b00111111*/
|
||||
} else {
|
||||
resArr[(resPos += 1)] = (0xe /*0b1110*/ << 4) | (point >>> 12);
|
||||
resArr[(resPos += 1)] =
|
||||
(0x2 /*0b10*/ << 6) | ((point >>> 6) & 0x3f); /*0b00111111*/
|
||||
resArr[(resPos += 1)] =
|
||||
(0x2 /*0b10*/ << 6) | (point & 0x3f); /*0b00111111*/
|
||||
}
|
||||
}
|
||||
return resArr.subarray(0, resPos + 1);
|
||||
}
|
||||
|
||||
log(){
|
||||
if(this.debug) console.log(...arguments);
|
||||
}
|
||||
}
|
||||
log() {
|
||||
if (this.debug) console.log(...arguments);
|
||||
}
|
||||
};
|
||||
|
||||
function bufferToString(data) {
|
||||
if (!("TextDecoder" in window))
|
||||
alert("Sorry, this browser does not support TextDecoder...");
|
||||
var enc = new TextDecoder("utf-8");
|
||||
var arr = new Uint8Array(data.buffer);
|
||||
return enc.decode(arr);
|
||||
}
|
||||
function bufferToString(data) {
|
||||
if (!("TextDecoder" in window))
|
||||
alert("Sorry, this browser does not support TextDecoder...");
|
||||
var enc = new TextDecoder("utf-8");
|
||||
var arr = new Uint8Array(data.buffer);
|
||||
return enc.decode(arr);
|
||||
}
|
||||
|
@ -150,4 +150,8 @@ void DisplayTask::log(const char* msg) {
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
class DisplayTask {};
|
||||
|
||||
#endif
|
@ -153,8 +153,9 @@ void InterfaceTask::run() {
|
||||
Wire.begin(PIN_SDA, PIN_SCL);
|
||||
Wire.setClock(400000);
|
||||
#endif
|
||||
|
||||
#if SK_STRAIN
|
||||
scale.begin(38, 2);
|
||||
scale.begin(PIN_STRAIN_DO, PIN_STRAIN_SCK);
|
||||
#endif
|
||||
|
||||
#if SK_ALS
|
||||
@ -281,7 +282,6 @@ void InterfaceTask::updateHardware() {
|
||||
#if SK_STRAIN
|
||||
if (scale.wait_ready_timeout(100)) {
|
||||
int32_t reading = scale.read();
|
||||
|
||||
|
||||
static uint32_t last_reading_display;
|
||||
if (millis() - last_reading_display > 1000) {
|
||||
@ -292,8 +292,8 @@ void InterfaceTask::updateHardware() {
|
||||
}
|
||||
|
||||
// TODO: calibrate and track (long term moving average) zero point (lower); allow calibration of set point offset
|
||||
const int32_t lower = 950000;
|
||||
const int32_t upper = 1800000;
|
||||
const int32_t lower = -400000;
|
||||
const int32_t upper = 500000; // original value: 1800000
|
||||
// Ignore readings that are way out of expected bounds
|
||||
if (reading >= lower - (upper - lower) && reading < upper + (upper - lower)*2) {
|
||||
long value = CLAMP(reading, lower, upper);
|
||||
|
@ -9,7 +9,7 @@
|
||||
#endif
|
||||
#include "util.h"
|
||||
|
||||
|
||||
|
||||
// ####
|
||||
// Hardware-specific motor calibration constants.
|
||||
// Run calibration once at startup, then update these constants with the calibration results.
|
||||
|
@ -24,13 +24,13 @@ class KnobCharacteristicCallBack : public BLECharacteristicCallbacks{
|
||||
public:
|
||||
//This method not called
|
||||
void onWrite(BLECharacteristic *pCharacteristic) override{
|
||||
ble_quiet_please = true;
|
||||
// std::string xcc = pCharacteristic->getValue();
|
||||
// // uint8_t* rxValue = pCharacteristic->getData();
|
||||
// Serial.print("value received = ");
|
||||
// Serial.println((char*)&xcc);
|
||||
ble_quiet_please = true;
|
||||
|
||||
char* rxValue = reinterpret_cast<char*>(pCharacteristic->getData());
|
||||
newMotorConfig_ = reinterpret_cast<char*>(pCharacteristic->getData());
|
||||
|
||||
// char buf_[256];
|
||||
// snprintf(buf_, sizeof(buf_), "value received from BT = %s", val);
|
||||
@ -38,15 +38,15 @@ public:
|
||||
|
||||
log_i("data is received");
|
||||
|
||||
// delay(50);
|
||||
hasNewMotorConfig_ = true;
|
||||
newMotorConfigProccessed = false;
|
||||
newMotorConfig_ = rxValue;
|
||||
delay(100);
|
||||
// delay(50);
|
||||
ble_quiet_please = false;
|
||||
}
|
||||
};
|
||||
|
||||
BLETask::BLETask(const uint8_t task_core) : Task("BLE", 2700, 1, task_core) {
|
||||
BLETask::BLETask(const uint8_t task_core) : Task("BLE", 3000, 1, task_core) {
|
||||
queue_ = xQueueCreate(5, sizeof(Message));
|
||||
assert(queue_ != NULL);
|
||||
|
||||
@ -67,8 +67,14 @@ void BLETask::run() {
|
||||
// See the following for generating UUIDs:
|
||||
// https://www.uuidgenerator.net/
|
||||
|
||||
#define SERVICE_UUID "0000340f-0000-1000-8000-00805f9b34fb"
|
||||
#define CHARACTERISTIC_UUID "602f75a0-697d-4690-8dd5-fa52781446d1"
|
||||
// #define SERVICE_UUID "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
|
||||
// #define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"
|
||||
// #define SERVICE_UUID "00001235-0000-1000-8000-00805f9b34fb"
|
||||
// #define CHARACTERISTIC_UUID "00004568-0000-1000-8000-00805f9b34fb"
|
||||
|
||||
#define SERVICE_UUID "00000001-0000-1000-8000-00805f9b34fb" // UART service UUID
|
||||
#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E" // Writes
|
||||
#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E" // Notifications
|
||||
|
||||
// Create the BLE Device
|
||||
BLEDevice::init("SmartKnob_0123");
|
||||
@ -81,59 +87,48 @@ void BLETask::run() {
|
||||
// Create the BLE Service
|
||||
BLEService *pService = pServer->createService(SERVICE_UUID);
|
||||
|
||||
// https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.descriptor.gatt.client_characteristic_configuration.xml
|
||||
|
||||
// Create a BLE Characteristic
|
||||
pCharacteristic = pService->createCharacteristic(
|
||||
CHARACTERISTIC_UUID,
|
||||
BLECharacteristic::PROPERTY_READ |
|
||||
BLECharacteristic::PROPERTY_WRITE |
|
||||
BLECharacteristic::PROPERTY_NOTIFY |
|
||||
BLECharacteristic::PROPERTY_INDICATE
|
||||
);
|
||||
|
||||
pCharacteristic->setCallbacks(new KnobCharacteristicCallBack());
|
||||
// Create a BLE Descriptor
|
||||
pCharacteristic->addDescriptor(new BLE2902());
|
||||
// Create TX BLE Characteristic
|
||||
pTxCharacteristic = pService->createCharacteristic(
|
||||
CHARACTERISTIC_UUID_TX,
|
||||
BLECharacteristic::PROPERTY_NOTIFY
|
||||
);
|
||||
pTxCharacteristic->addDescriptor(new BLE2902());
|
||||
|
||||
|
||||
// Create RX BLE Characteristic
|
||||
BLECharacteristic * pRxCharacteristic = pService->createCharacteristic(
|
||||
CHARACTERISTIC_UUID_RX,
|
||||
BLECharacteristic::PROPERTY_WRITE
|
||||
);
|
||||
pRxCharacteristic->setCallbacks(new KnobCharacteristicCallBack());
|
||||
|
||||
|
||||
// Start the service
|
||||
pService->start();
|
||||
|
||||
|
||||
// Start advertising
|
||||
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
|
||||
pAdvertising->addServiceUUID(SERVICE_UUID);
|
||||
pAdvertising->setScanResponse(false);
|
||||
pAdvertising->setMinPreferred(0x0); // set value to 0x00 to not advertise this parameter
|
||||
BLEDevice::startAdvertising();
|
||||
pServer->getAdvertising()->start();
|
||||
|
||||
// // Start advertising
|
||||
// BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
|
||||
// pAdvertising->addServiceUUID(SERVICE_UUID);
|
||||
// pAdvertising->setScanResponse(true);
|
||||
// pAdvertising->setMinPreferred(0x06); // functions that help with iPhone connections issue
|
||||
// pAdvertising->setMinPreferred(0x12);
|
||||
// BLEDevice::startAdvertising();
|
||||
|
||||
PB_SmartKnobState state;
|
||||
|
||||
while (1) {
|
||||
|
||||
// disconnecting
|
||||
if (!deviceConnected && oldDeviceConnected) {
|
||||
delay(500); // give the bluetooth stack the chance to get things ready
|
||||
pServer->startAdvertising(); // restart advertising
|
||||
log("BLE start advertising");
|
||||
oldDeviceConnected = deviceConnected;
|
||||
}
|
||||
// connecting
|
||||
if (deviceConnected && !oldDeviceConnected) {
|
||||
// do stuff here on connecting
|
||||
oldDeviceConnected = deviceConnected;
|
||||
//delay(500);
|
||||
//pCharacteristic->notify(); // DOES NOT WORK
|
||||
}
|
||||
|
||||
// Old method here, skip everything if no update from knob
|
||||
if (xQueueReceive(knob_state_queue_, &state, portMAX_DELAY) == pdFALSE) {
|
||||
continue;
|
||||
}
|
||||
if (deviceConnected && !ble_quiet_please) {
|
||||
|
||||
// Old method here, skip everything if no update from knob
|
||||
if (xQueueReceive(knob_state_queue_, &state, portMAX_DELAY) == pdFALSE) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// notify changed value
|
||||
if (deviceConnected && !ble_quiet_please) {
|
||||
|
||||
if(button_state_old != button_state_){
|
||||
button_state_old = button_state_;
|
||||
sendNotify(1, button_state_old);
|
||||
@ -163,9 +158,22 @@ void BLETask::run() {
|
||||
// lux_value_old = lux_value_;
|
||||
// sendNotify(6, (uint32_t)&lux_value_old);
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
// disconnecting
|
||||
if (!deviceConnected && oldDeviceConnected) {
|
||||
delay(500); // give the bluetooth stack the chance to get things ready
|
||||
pServer->startAdvertising(); // restart advertising
|
||||
log("BLE start advertising");
|
||||
oldDeviceConnected = deviceConnected;
|
||||
}
|
||||
// connecting
|
||||
if (deviceConnected && !oldDeviceConnected) {
|
||||
// do stuff here on connecting
|
||||
oldDeviceConnected = deviceConnected;
|
||||
}
|
||||
|
||||
|
||||
// // Check queue for pending requests from other tasks
|
||||
// Message message;
|
||||
// if (xQueueReceive(queue_, &message, 0) == pdTRUE) {
|
||||
@ -190,27 +198,25 @@ void BLETask::run() {
|
||||
}
|
||||
|
||||
void BLETask::sendNotify(int key, bool data){
|
||||
uint8_t temp[2];
|
||||
temp[0] = key;
|
||||
if(data) temp[1] = 1;
|
||||
else temp[1] = 0;
|
||||
pCharacteristic->setValue((uint8_t*)&temp, 2);
|
||||
pCharacteristic->notify();
|
||||
delay(3); // bluetooth stack will go into congestion, if too many packets are sent, in 6 hours test i was able to go as low as 3ms
|
||||
temp8[0] = key;
|
||||
if(data) temp8[1] = 1;
|
||||
else temp8[1] = 0;
|
||||
pTxCharacteristic->setValue((uint8_t*)&temp8, 2);
|
||||
pTxCharacteristic->notify();
|
||||
delay(ble_delay); // bluetooth stack will go into congestion, if too many packets are sent, in 6 hours test i was able to go as low as 3ms
|
||||
}
|
||||
void BLETask::sendNotify(int key, uint32_t data32){
|
||||
uint32_t temp[8];
|
||||
temp[0] = key;
|
||||
temp[1] = data32;
|
||||
temp[2] = data32 >> 8;
|
||||
temp[3] = data32 >> 16;
|
||||
temp[4] = data32 >> 24;
|
||||
temp[5] = data32 >> 32;
|
||||
temp[6] = data32 >> 40;
|
||||
temp[7] = data32 >> 48;
|
||||
pCharacteristic->setValue((uint8_t*)&temp,8);
|
||||
pCharacteristic->notify();
|
||||
delay(3); // bluetooth stack will go into congestion, if too many packets are sent, in 6 hours test i was able to go as low as 3ms
|
||||
temp32[0] = key;
|
||||
temp32[1] = data32;
|
||||
temp32[2] = data32 >> 8;
|
||||
temp32[3] = data32 >> 16;
|
||||
temp32[4] = data32 >> 24;
|
||||
temp32[5] = data32 >> 32;
|
||||
temp32[6] = data32 >> 40;
|
||||
temp32[7] = data32 >> 48;
|
||||
pTxCharacteristic->setValue((uint8_t*)&temp32,8);
|
||||
pTxCharacteristic->notify();
|
||||
delay(ble_delay); // bluetooth stack will go into congestion, if too many packets are sent, in 6 hours test i was able to go as low as 3ms
|
||||
}
|
||||
|
||||
void BLETask::updateScale(int32_t new_press_value_unit){
|
||||
|
@ -2,10 +2,7 @@
|
||||
|
||||
#if SK_BLE
|
||||
|
||||
// #include <iostream>
|
||||
// #include <vector>
|
||||
#include <cstring> // Include for std::strncpy, std::strtok, and std::strncmp
|
||||
// #include <cstdlib> // Include for std::atoi and std::atof
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <BLEDevice.h>
|
||||
@ -63,9 +60,12 @@ class BLETask : public Task<BLETask> {
|
||||
Logger* logger_;
|
||||
std::vector<QueueHandle_t> listeners_;
|
||||
|
||||
int ble_delay = 3;
|
||||
|
||||
// BLE Setup
|
||||
BLEServer* pServer = NULL;
|
||||
BLECharacteristic* pCharacteristic = NULL;
|
||||
BLECharacteristic* pTxCharacteristic;
|
||||
BLECharacteristic* pRxCharacteristic;
|
||||
// bool BLE_CONNECTED;
|
||||
bool oldDeviceConnected = false;
|
||||
|
||||
@ -82,6 +82,8 @@ class BLETask : public Task<BLETask> {
|
||||
float lux_value_;
|
||||
float lux_value_old;
|
||||
PB_SmartKnobConfig new_motor_config;
|
||||
uint32_t temp32[8];
|
||||
uint8_t temp8[2];
|
||||
|
||||
QueueHandle_t knob_state_queue_;
|
||||
|
||||
|
@ -52,7 +52,7 @@ lib_deps =
|
||||
build_flags =
|
||||
${base_config.build_flags}
|
||||
-DSK_BLE=1
|
||||
-DSK_DISPLAY=1
|
||||
-DSK_DISPLAY=0
|
||||
-DSK_LEDS=1
|
||||
-DNUM_LEDS=8
|
||||
-DSENSOR_MT6701=1
|
||||
|
Loading…
Reference in New Issue
Block a user