diff --git a/Cargo.toml b/Cargo.toml index 8270118207d557ee78ad3f5352663d572aca6561..ebe04c5b0e7fd4f3d61f3da4f0f6cb7a659f96a7 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "mavlink" -version = "0.2.0" +version = "0.3.1" authors = ["Tim Ryan <tim@3dr.com>"] build = "build/main.rs" description = "Parses the MAVLink data interchange format for UAVs." diff --git a/README.md b/README.md index 5804f6acdace7f6aad142fa35187da46dac68963..5ae8fac4cfa2251aff8172bd6f86e757fa18681b 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ Add to your Cargo.toml: ``` -mavlink = "0.1.0" +mavlink = "0.3" ``` ## License diff --git a/src/bin/mavlink-dump.rs b/src/bin/mavlink-dump.rs index a71963f8964ecc0eb25d4ac1e1778bf4b00861b9..20aba2a840e0034c79199d88e516ed475ee546b2 100644 --- a/src/bin/mavlink-dump.rs +++ b/src/bin/mavlink-dump.rs @@ -14,13 +14,7 @@ fn main() { } let vlock = Arc::new(RwLock::new(mavlink::connect(&args[1]).unwrap())); - - { - let mut vehicle = vlock.write().unwrap(); - vehicle.send(mavlink::heartbeat_message()); - vehicle.send(mavlink::request_parameters()); - vehicle.send(mavlink::request_stream()); - } + let vehicle_recv = vlock.read().unwrap().wait_recv(); thread::spawn({ let vlock = vlock.clone(); @@ -35,8 +29,16 @@ fn main() { } }); + let mut first = true; loop { - vlock.read().unwrap().wait_recv().sleep(); + vehicle_recv.sleep(); + + if first { + let mut vehicle = vlock.write().unwrap(); + vehicle.send(mavlink::request_parameters()); + vehicle.send(mavlink::request_stream()); + first = false; + } if let Ok(msg) = vlock.write().unwrap().recv() { println!("{:?}", msg); diff --git a/src/connection.rs b/src/connection.rs index f041110bae33815ddffd1ed7325c8bd600bed3a3..6c4b2bc0fdbc72bb3f40cece3df0a3dafcb8b960 100644 --- a/src/connection.rs +++ b/src/connection.rs @@ -347,15 +347,16 @@ pub struct VehicleConnection { } pub struct VehicleAwait { - value: u64, pair: Arc<(Mutex<u64>, Condvar)>, } impl VehicleAwait { - pub fn sleep(self) { + pub fn sleep(&self) { + // TODO vehilce buffer? let &(ref lock, ref cvar) = &*self.pair; let mut started = lock.lock().unwrap(); - while *started == self.value { + let original = *started; + while *started == original { started = cvar.wait(started).unwrap(); } } @@ -389,12 +390,7 @@ impl VehicleConnection { } pub fn wait_recv(&self) -> VehicleAwait { - let count = { - let &(ref lock, _) = &*self.pair; - *lock.lock().unwrap() - }; VehicleAwait { - value: count, pair: self.pair.clone() } }