Skip to content

Commit

Permalink
fix: Correct a timing issue with Fredorch initialisation
Browse files Browse the repository at this point in the history
  • Loading branch information
@blackspherefollower
blackspherefollower committed Aug 11, 2022
1 parent de28143 commit 7a634ce
Show file tree
Hide file tree
Showing 4 changed files with 135 additions and 49 deletions.
2 changes: 1 addition & 1 deletion buttplug/buttplug-device-config/buttplug-device-config.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3175,7 +3175,7 @@
"services": {
"0000ffb0-0000-1000-8000-00805f9b34fb": {
"tx": "0000ffb1-0000-1000-8000-00805f9b34fb",
"rx": "0000ffb1-0000-1000-8000-00805f9b34fb"
"rx": "0000ffb2-0000-1000-8000-00805f9b34fb"
}
}
},
Expand Down
2 changes: 1 addition & 1 deletion buttplug/buttplug-device-config/buttplug-device-config.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1963,7 +1963,7 @@ protocols:
services:
0000ffb0-0000-1000-8000-00805f9b34fb:
tx: 0000ffb1-0000-1000-8000-00805f9b34fb
rx: 0000ffb1-0000-1000-8000-00805f9b34fb
rx: 0000ffb2-0000-1000-8000-00805f9b34fb
defaults:
name: Fredorch Device
messages:
Expand Down
137 changes: 91 additions & 46 deletions buttplug/src/server/device/protocol/fredorch.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ use crate::{
},
server::device::{
configuration::ProtocolAttributesType,
hardware::{Hardware, HardwareCommand, HardwareWriteCmd},
hardware::{Hardware, HardwareCommand, HardwareWriteCmd, HardwareSubscribeCmd, HardwareEvent},
protocol::{
fleshlight_launch_helper::calculate_speed,
generic_protocol_initializer_setup,
Expand All @@ -24,11 +24,18 @@ use crate::{
},
};
use async_trait::async_trait;
use std::sync::{
atomic::{AtomicU8, Ordering},
Arc,
use futures::FutureExt;
use futures_timer::Delay;
use std::{
sync::{
atomic::{AtomicU8, Ordering},
Arc,
},
time::Duration,
};

const FREDORCH_COMMAND_TIMEOUT_MS: u64 = 500;

const CRC_HI: [u8; 256] = [
0, 193, 129, 64, 1, 192, 128, 65, 1, 192, 128, 65, 0, 193, 129, 64, 1, 192, 128, 65, 0, 193, 129,
64, 0, 193, 129, 64, 1, 192, 128, 65, 1, 192, 128, 65, 0, 193, 129, 64, 0, 193, 129, 64, 1, 192,
Expand Down Expand Up @@ -80,53 +87,91 @@ impl ProtocolInitializer for FredorchInitializer {
&mut self,
hardware: Arc<Hardware>,
) -> Result<Arc<dyn ProtocolHandler>, ButtplugDeviceError> {
// Set the device to program mode
let mut data: Vec<u8> = vec![0x01, 0x06, 0x00, 0x64, 0x00, 0x01];
let mut crc = crc16(&data);
data.push(crc[0]);
data.push(crc[1]);
let mut event_receiver = hardware.event_stream();
hardware
.write_value(&HardwareWriteCmd::new(Endpoint::Tx, data.clone(), false))
.await?;
.subscribe(&HardwareSubscribeCmd::new(Endpoint::Rx))
.await?;

// Set the program mode to record
data = vec![0x01, 0x06, 0x00, 0x69, 0x00, 0x00];
crc = crc16(&data);
data.push(crc[0]);
data.push(crc[1]);
hardware
.write_value(&HardwareWriteCmd::new(Endpoint::Tx, data.clone(), false))
.await?;

// Program the device to move to position 0 at speed 5
data = vec![
0x01, 0x10, 0x00, 0x6b, 0x00, 0x05, 0x0a, 0x00, 0x05, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00,
0x00, 0x01,
let init: Vec<(String, Vec<u8>)> = vec![
(
"Set the device to program mode".to_owned(),
vec![0x01, 0x06, 0x00, 0x64, 0x00, 0x01],
),
(
"Set the program mode to record".to_owned(),
vec![0x01, 0x06, 0x00, 0x69, 0x00, 0x00],
),
(
"Program the device to move to position 0 at speed 5".to_owned(),
vec![
0x01, 0x10, 0x00, 0x6b, 0x00, 0x05, 0x0a, 0x00, 0x05, 0x00, 0x05, 0x00, 0x00, 0x00,
0x00, 0x00, 0x01,
],
),
(
"Run the program".to_owned(),
vec![0x01, 0x06, 0x00, 0x69, 0x00, 0x01],
),
(
"Set the program to repeat".to_owned(),
vec![0x01, 0x06, 0x00, 0x6a, 0x00, 0x01],
),
];
crc = crc16(&data);
data.push(crc[0]);
data.push(crc[1]);
hardware
.write_value(&HardwareWriteCmd::new(Endpoint::Tx, data.clone(), false))
.await?;

// Run the program
data = vec![0x01, 0x06, 0x00, 0x69, 0x00, 0x01];
crc = crc16(&data);
data.push(crc[0]);
data.push(crc[1]);
hardware
.write_value(&HardwareWriteCmd::new(Endpoint::Tx, data.clone(), false))
.await?;
// expect 0, 1, 0, 1, 1 on connect
select! {
event = event_receiver.recv().fuse() => {
if let Ok(HardwareEvent::Notification(_, _, n)) = event {
debug!("Fredorch: wake up - received {:?}", n);
} else {
return Err(
ButtplugDeviceError::ProtocolSpecificError(
"Fredorch".to_owned(),
"Fredorch Device disconnected while initialising.".to_owned(),
)
.into(),
);
}
}
_ = Delay::new(Duration::from_millis(FREDORCH_COMMAND_TIMEOUT_MS)).fuse() => {
// Or not?
}
}

// Set the program to repeat
data = vec![0x01, 0x06, 0x00, 0x6a, 0x00, 0x01];
crc = crc16(&data);
data.push(crc[0]);
data.push(crc[1]);
hardware
.write_value(&HardwareWriteCmd::new(Endpoint::Tx, data.clone(), false))
.await?;
for mut data in init {
let crc = crc16(&data.1);
data.1.push(crc[0]);
data.1.push(crc[1]);
debug!("Fredorch: {} - sent {:?}", data.0, data.1);
hardware
.write_value(&HardwareWriteCmd::new(Endpoint::Tx, data.1.clone(), false))
.await?;

select! {
event = event_receiver.recv().fuse() => {
if let Ok(HardwareEvent::Notification(_, _, n)) = event {
debug!("Fredorch: {} - received {:?}", data.0, n);
} else {
return Err(
ButtplugDeviceError::ProtocolSpecificError(
"Fredorch".to_owned(),
"Fredorch Device disconnected while initialising.".to_owned(),
)
.into(),
);
}
}
_ = Delay::new(Duration::from_millis(FREDORCH_COMMAND_TIMEOUT_MS)).fuse() => {
return Err(
ButtplugDeviceError::ProtocolSpecificError(
"Fredorch".to_owned(),
"Fredorch Device timed out while initialising.".to_owned(),
)
.into(),
);
}
}
}

Ok(Arc::new(Fredorch::default()))
}
Expand Down
43 changes: 42 additions & 1 deletion buttplug/tests/device_test_case/test_fredorch_protocol.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,39 +6,80 @@ device_init:
# Initialization
- !Commands
device_index: 0
commands:
commands:
- !Subscribe
endpoint: rx
- !Events
device_index: 0
events:
- !Notifications
- endpoint: rx
data: [0x00, 0x01, 0x00, 0x01, 0x01]
- !Commands
device_index: 0
commands:
- !Write
endpoint: tx
data: [0x01, 0x06, 0x00, 0x64, 0x00, 0x01, 0x09, 0xd5]
write_with_response: false
- !Events
device_index: 0
events:
- !Notifications
- endpoint: rx
data: [0x01, 0x06, 0x00, 0x64, 0x00, 0x01, 0x09, 0xd5]
- !Commands
device_index: 0
commands:
- !Write
endpoint: tx
data: [0x01, 0x06, 0x00, 0x69, 0x00, 0x00, 0x59, 0xd6]
write_with_response: false
- !Events
device_index: 0
events:
- !Notifications
- endpoint: rx
data: [0x01, 0x06, 0x00, 0x69, 0x00, 0x00, 0x59, 0xd6]
- !Commands
device_index: 0
commands:
- !Write
endpoint: tx
data: [0x01, 0x10, 0x00, 0x6b, 0x00, 0x05, 0x0a, 0x00, 0x05, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xc0, 0xc3]
write_with_response: false
- !Events
device_index: 0
events:
- !Notifications
- endpoint: rx
data: [0x01, 0x10, 0x00, 0x6b, 0x00, 0x05, 0x0a, 0x00, 0x05, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xc0, 0xc3]
- !Commands
device_index: 0
commands:
- !Write
endpoint: tx
data: [0x01, 0x06, 0x00, 0x69, 0x00, 0x01, 0x98, 0x16]
write_with_response: false
- !Events
device_index: 0
events:
- !Notifications
- endpoint: rx
data: [0x01, 0x06, 0x00, 0x69, 0x00, 0x01, 0x98, 0x16]
- !Commands
device_index: 0
commands:
- !Write
endpoint: tx
data: [0x01, 0x06, 0x00, 0x6a, 0x00, 0x01, 0x68, 0x16]
write_with_response: false
- !Events
device_index: 0
events:
- !Notifications
- endpoint: rx
data: [0x01, 0x06, 0x00, 0x6a, 0x00, 0x01, 0x68, 0x16]
device_commands:
# Commands
#
Expand Down

0 comments on commit 7a634ce

Please sign in to comment.