tello_edu/
state.rs

1use tokio::{spawn, task};
2use tokio::sync::mpsc;
3use tokio::net::UdpSocket;
4
5use crate::errors::{Result, TelloError};
6
7const STATE_UDP_PORT:u32 = 8890;
8
9pub type TelloStateSender = mpsc::UnboundedSender<TelloState>;
10pub type TelloStateReceiver = mpsc::UnboundedReceiver<TelloState>;
11
12pub fn make_tello_state_channel() -> (TelloStateSender, TelloStateReceiver) {
13    mpsc::unbounded_channel()
14}
15
16/// The live state of the drone.
17#[derive(Debug, Default)]
18pub struct TelloState {
19    /// Roll angle in degrees.
20    pub roll: i16,
21
22    /// Pitch angle in degrees.
23    pub pitch: i16,
24
25    /// Yaw angle in degrees.
26    pub yaw: i16,
27
28    /// Height above launch point in cm
29    pub height: i16,
30
31    /// Barometer measurement in cm (height?)
32    pub barometer: f32,
33
34    /// Battery level as percentage.
35    pub battery: u8,
36
37    /// Not sure what this actually is - Tello docs says "time of flight distance in cm"
38    pub time_of_flight: u16,
39
40    /// The time the motors have been active for, in seconds.
41    pub motor_time: u16,
42
43    /// Minimum temperature, Celsius.
44    pub temperature_low: i16,
45
46    /// Maximum temperature, Celsius.
47    pub temperature_high: i16,
48
49    /// Velocity, cm⁻¹
50    pub velocity: Vector3<i16>,
51
52    /// Acceleration, cms⁻²
53    pub acceleration: Vector3<f32>
54}
55
56#[derive(Debug, Default)]
57pub struct Vector3<T> {
58    x: T,
59    y: T,
60    z: T
61}
62
63impl TelloState {
64    /// Parses a state string received from the drone.
65    ///
66    /// Example message:
67    /// "mid:-1;x:-100;y:-100;z:-100;mpry:-1,-1,-1;pitch:0;roll:0;yaw:-3;vgx:0;vgy:0;vgz:1;templ:58;temph:60;tof:71;h:50;bat:82;baro:-57.14;time:14;agx:17.00;agy:-4.00;agz:-956.00;"
68    ///
69    pub fn from_message(s: &str) -> Result<TelloState> {
70        let mut state = TelloState::default();
71
72        for f in s.split(";") {
73            if f.is_empty() { continue; }
74
75            let (k,v) = split_key_value(f)?;
76
77            match k.as_str() {
78                "roll" => state.roll = value_as(&v)?,
79                "pitch" => state.pitch = value_as(&v)?,
80                "yaw" => state.yaw = value_as(&v)?,
81                "h" => state.height = value_as(&v)?,
82                "baro" => state.barometer = value_as(&v)?,
83                "bat" => state.battery = value_as(&v)?,
84                "tof" => state.time_of_flight = value_as(&v)?,
85                "time" => state.motor_time = value_as(&v)?,
86                "templ" => state.temperature_low = value_as(&v)?,
87                "temph" => state.temperature_high = value_as(&v)?,
88                "vgx" => state.velocity.x = value_as(&v)?,
89                "vgy" => state.velocity.y = value_as(&v)?,
90                "vgz" => state.velocity.z = value_as(&v)?,
91                "agx" => state.acceleration.x = value_as(&v)?,
92                "agy" => state.acceleration.y = value_as(&v)?,
93                "agz" => state.acceleration.z = value_as(&v)?,
94                _ => {}
95            }
96        }
97
98        Ok(state)
99    }
100}
101
102fn split_key_value(kv: &str) -> Result<(String, String)> {
103    let mut i = kv.split(":");
104    let k = i.next().ok_or_else(|| TelloError::ParseError { msg: kv.to_string() })?;
105    let v = i.next().ok_or_else(|| TelloError::ParseError { msg: kv.to_string() })?;
106    Ok((k.to_string(),v.to_string()))
107}
108
109fn value_as<T: std::str::FromStr>(s: &str) -> Result<T> {
110    s.parse::<T>().map_err(|_| TelloError::ParseError { msg: s.to_string() })
111}
112
113// fn value_as_some<T: std::str::FromStr>(s: &str) -> Result<Option<T>> {
114//     let v = s.parse::<T>().map_err(|_| TelloError::ParseError { msg: s.to_string() })?;
115//     Ok(Some(v))
116// }
117
118#[derive(Debug)]
119pub(crate) struct StateListener {
120    task: task::JoinHandle<()>
121}   
122
123impl StateListener {
124    pub(crate) async fn start_listening(sender:TelloStateSender) -> Result<Self> { 
125        let local_address = format!("0.0.0.0:{STATE_UDP_PORT}");
126        println!("[State] START LISTENING at {local_address}");
127
128        let sock = UdpSocket::bind(&local_address).await?;
129
130        let task = spawn(async move {
131            loop {
132                let s = &sock;
133                let mut buf = vec![0; 1024];        
134                let n = s.recv(&mut buf).await.unwrap();
135
136                buf.truncate(n);
137                let r = String::from_utf8(buf).unwrap();
138                let raw_state = r.trim().to_string();
139
140                let state = TelloState::from_message(&raw_state).unwrap();
141                sender.send(state).unwrap();
142            }
143        });
144
145        Ok(Self { task })
146    }
147
148    pub(crate) async fn stop_listening(&self) -> Result<()> {
149        println!("[State] STOP LISTENING");
150        self.task.abort();
151        // TODO?
152        // let _err = self.task.await;
153        Ok(())
154    }
155 }
tello_edu/state.rs

tello_edu/
state.rs
