crazyflie_lib/subsystems/
commander.rs

1//! # Low level setpoint subsystem
2//!
3//! This subsystem allows to send low-level setpoint. The setpoints are described as low-level in the sense that they
4//! are setting the instant target state. As such they likely need to be send very often to have the crazyflie
5//! follow the wanted flight profile.
6//!
7//! The Crazyflie has a couple of safety mechanisms that one needs to be aware of in order to send setpoints:
8//!  - When using the [Commander::setpoint_rpyt()] function, a setpoint with thrust=0 must be sent once to unlock the thrust
9//!  - There is a priority for setpoints in the Crazyflie, this allows app and other internal subsystem like the high-level
10//!    commander to set setpoints in parallel, only the higher priority setpoint is taken into account.
11//!  - In no setpoint are received for 1 seconds, the Crazyflie will reset roll/pitch/yawrate to 0/0/0 and after 2 seconds
12//!    will fallback fallback to a lower-priority setpoint which in most case will cut the motors.
13//!
14//! The following example code would drive the motors in a ramp and then stop:
15//! ``` no_run
16//! # use tokio::time::{sleep, Duration};
17//! # async fn ramp(crazyflie: crazyflie_lib::Crazyflie) -> Result<(), Box<dyn std::error::Error>> {
18//! // Unlock the commander
19//! crazyflie.commander.setpoint_rpyt(0.0, 0.0, 0.0, 0).await?;
20//!
21//! // Ramp!
22//! for thrust in (0..20_000).step_by(1_000) {
23//!     crazyflie.commander.setpoint_rpyt(0.0, 0.0, 0.0, thrust).await?;
24//!     sleep(Duration::from_millis(100)).await;
25//! }
26//!
27//! // Stop the motors
28//! crazyflie.commander.setpoint_rpyt(0.0, 0.0, 0.0, 0).await?;
29//! # Ok(())
30//! # }
31//! ```
32
33use crazyflie_link::Packet;
34use flume::Sender;
35
36use crate::{Error, Result};
37
38use crate::crazyflie::COMMANDER_PORT;
39
40const RPYT_CHANNEL: u8 = 0;
41
42const _GENERIC_SETPOINT_CHANNEL: u8 = 0;
43const _GENERIC_CMD_CHANNEL: u8 = 1;
44
45/// # Low level setpoint subsystem
46///
47/// This struct implements methods to send low level setpoints to the Crazyflie.
48/// See the [commander module documentation](crate::subsystems::commander) for more context and information.
49#[derive(Debug)]
50pub struct Commander {
51    uplink: Sender<Packet>,
52}
53
54impl Commander {
55    pub(crate) fn new(uplink: Sender<Packet>) -> Commander {
56        Commander { uplink }
57    }
58}
59
60/// # Legacy RPY+ setpoint
61///
62/// This setpoint was originally the only one present in the Crazyflie and has been (ab)used to
63/// implement the early position control and other assisted and semi-autonomous mode.
64impl Commander {
65    /// Set the Roll Pitch Yaw Thrust setpoint
66    ///
67    /// When not modified by [parameters](crate::subsystems::param::Param), the meaning of the argument are:
68    ///  - **Roll/Pitch** are in degree and represent the absolute angle
69    ///  - **Yaw** is in degree per seconds and represents the rotation rate
70    ///  - **Thrust** is a 16 bit value where 0 maps to 0% thrust and 65535 to 100% thrust
71    ///
72    /// The thrust is blocked by default. The setpoint needs to be set once with thrust = 0 to unlock
73    /// the thrust for example:
74    /// ``` no_run
75    /// # fn spin(cf: crazyflie_lib::Crazyflie) {
76    /// cf.commander.setpoint_rpyt(0.0, 0.0, 0.0, 0);      // Unlocks the thrust
77    /// cf.commander.setpoint_rpyt(0.0, 0.0, 0.0, 1000);   // Thrust set to 1000
78    /// # }
79    /// ```
80    pub async fn setpoint_rpyt(&self, roll: f32, pitch: f32, yaw: f32, thrust: u16) -> Result<()> {
81        let mut payload = Vec::new();
82        payload.append(&mut roll.to_le_bytes().to_vec());
83        payload.append(&mut pitch.to_le_bytes().to_vec());
84        payload.append(&mut yaw.to_le_bytes().to_vec());
85        payload.append(&mut thrust.to_le_bytes().to_vec());
86
87        let pk = Packet::new(COMMANDER_PORT, RPYT_CHANNEL, payload);
88
89        self.uplink
90            .send_async(pk)
91            .await
92            .map_err(|_| Error::Disconnected)?;
93
94        Ok(())
95    }
96}
97
98/// # Generic setpoints
99///
100/// These setpoints are implemented in such a way that they are easy to add in the Crazyflie firmware
101/// and in libs like this one. So if you have a use-case not covered by any of the existing setpoint
102/// do not hesitate to implement and contribute your dream setpoint :-).
103impl Commander {}
crazyflie_lib/subsystems/commander.rs

crazyflie_lib/subsystems/
commander.rs
