pamoja_mavlink/protocol/
offboard.rs

1//! Offboard control setpoints: the `type_mask` builder and setpoint constructors.
2//!
3//! An offboard setpoint carries position, velocity, and acceleration fields all at once, and a
4//! `type_mask` says which of them the vehicle should act on and which to ignore. Getting the
5//! mask wrong is the classic offboard bug: a velocity setpoint with the position bits left
6//! active makes the vehicle chase a zero position. [`TypeMask`] builds the mask from the named
7//! [`POSITION_TARGET_TYPEMASK`](crate::dialect::position_target_typemask) bits, starting from
8//! "ignore everything" and enabling only the dimensions a setpoint sets, and the constructors
9//! on [`SetPositionTargetLocalNed`] and [`SetPositionTargetGlobalInt`] use it to produce
10//! ready-to-send position and velocity setpoints.
11
12use crate::dialect::position_target_typemask as bits;
13use crate::dialect::{SetPositionTargetGlobalInt, SetPositionTargetLocalNed};
14
15/// Builds a `type_mask` for a `SET_POSITION_TARGET_*` message.
16///
17/// A set bit tells the vehicle to ignore that dimension, so the builder starts from
18/// [`ignore_all`](TypeMask::ignore_all) and each `use_*` method clears the ignore bits for the
19/// dimensions the setpoint provides.
20#[derive(Clone, Copy, Debug, PartialEq, Eq)]
21pub struct TypeMask(u16);
22
23impl TypeMask {
24    // Every ignorable dimension, i.e. every bit except FORCE_SET, which is a mode selector.
25    const ALL_IGNORE: u16 = bits::X_IGNORE
26        | bits::Y_IGNORE
27        | bits::Z_IGNORE
28        | bits::VX_IGNORE
29        | bits::VY_IGNORE
30        | bits::VZ_IGNORE
31        | bits::AX_IGNORE
32        | bits::AY_IGNORE
33        | bits::AZ_IGNORE
34        | bits::YAW_IGNORE
35        | bits::YAW_RATE_IGNORE;
36
37    /// Starts from a mask that ignores every dimension.
38    ///
39    /// # Returns
40    ///
41    /// A mask with every ignore bit set.
42    pub fn ignore_all() -> Self {
43        TypeMask(Self::ALL_IGNORE)
44    }
45
46    /// Enables the position fields (`x`, `y`, `z`).
47    ///
48    /// # Returns
49    ///
50    /// The mask, for chaining.
51    pub fn use_position(mut self) -> Self {
52        self.0 &= !(bits::X_IGNORE | bits::Y_IGNORE | bits::Z_IGNORE);
53        self
54    }
55
56    /// Enables the velocity fields (`vx`, `vy`, `vz`).
57    ///
58    /// # Returns
59    ///
60    /// The mask, for chaining.
61    pub fn use_velocity(mut self) -> Self {
62        self.0 &= !(bits::VX_IGNORE | bits::VY_IGNORE | bits::VZ_IGNORE);
63        self
64    }
65
66    /// Enables the acceleration fields (`afx`, `afy`, `afz`).
67    ///
68    /// # Returns
69    ///
70    /// The mask, for chaining.
71    pub fn use_acceleration(mut self) -> Self {
72        self.0 &= !(bits::AX_IGNORE | bits::AY_IGNORE | bits::AZ_IGNORE);
73        self
74    }
75
76    /// Enables the `yaw` field.
77    ///
78    /// # Returns
79    ///
80    /// The mask, for chaining.
81    pub fn use_yaw(mut self) -> Self {
82        self.0 &= !bits::YAW_IGNORE;
83        self
84    }
85
86    /// Enables the `yaw_rate` field.
87    ///
88    /// # Returns
89    ///
90    /// The mask, for chaining.
91    pub fn use_yaw_rate(mut self) -> Self {
92        self.0 &= !bits::YAW_RATE_IGNORE;
93        self
94    }
95
96    /// Sets the force flag, so the acceleration fields are interpreted as a force.
97    ///
98    /// # Returns
99    ///
100    /// The mask, for chaining.
101    pub fn force(mut self) -> Self {
102        self.0 |= bits::FORCE_SET;
103        self
104    }
105
106    /// Returns the assembled mask value.
107    ///
108    /// # Returns
109    ///
110    /// The `type_mask` bits.
111    pub fn bits(self) -> u16 {
112        self.0
113    }
114}
115
116impl SetPositionTargetLocalNed {
117    /// Builds a local-frame position setpoint, ignoring velocity, acceleration, and yaw.
118    ///
119    /// # Arguments
120    ///
121    /// * `time_boot_ms` - the sender's boot timestamp, in milliseconds.
122    /// * `coordinate_frame` - the [`MAV_FRAME`](crate::dialect::mav_frame) of the setpoint.
123    /// * `target_system` - the target system id.
124    /// * `target_component` - the target component id.
125    /// * `x`, `y`, `z` - the position, in meters in the chosen frame.
126    ///
127    /// # Returns
128    ///
129    /// The setpoint, with only the position fields active in its `type_mask`.
130    pub fn position(
131        time_boot_ms: u32,
132        coordinate_frame: u8,
133        target_system: u8,
134        target_component: u8,
135        x: f32,
136        y: f32,
137        z: f32,
138    ) -> Self {
139        SetPositionTargetLocalNed {
140            time_boot_ms,
141            x,
142            y,
143            z,
144            type_mask: TypeMask::ignore_all().use_position().bits(),
145            target_system,
146            target_component,
147            coordinate_frame,
148            ..Self::zeroed()
149        }
150    }
151
152    /// Builds a local-frame velocity setpoint, ignoring position, acceleration, and yaw.
153    ///
154    /// # Arguments
155    ///
156    /// * `time_boot_ms` - the sender's boot timestamp, in milliseconds.
157    /// * `coordinate_frame` - the [`MAV_FRAME`](crate::dialect::mav_frame) of the setpoint.
158    /// * `target_system` - the target system id.
159    /// * `target_component` - the target component id.
160    /// * `vx`, `vy`, `vz` - the velocity, in meters per second in the chosen frame.
161    ///
162    /// # Returns
163    ///
164    /// The setpoint, with only the velocity fields active in its `type_mask`.
165    pub fn velocity(
166        time_boot_ms: u32,
167        coordinate_frame: u8,
168        target_system: u8,
169        target_component: u8,
170        vx: f32,
171        vy: f32,
172        vz: f32,
173    ) -> Self {
174        SetPositionTargetLocalNed {
175            time_boot_ms,
176            vx,
177            vy,
178            vz,
179            type_mask: TypeMask::ignore_all().use_velocity().bits(),
180            target_system,
181            target_component,
182            coordinate_frame,
183            ..Self::zeroed()
184        }
185    }
186}
187
188impl SetPositionTargetGlobalInt {
189    /// Builds a global-frame position setpoint, ignoring velocity, acceleration, and yaw.
190    ///
191    /// # Arguments
192    ///
193    /// * `time_boot_ms` - the sender's boot timestamp, in milliseconds.
194    /// * `coordinate_frame` - the [`MAV_FRAME`](crate::dialect::mav_frame) of the setpoint.
195    /// * `target_system` - the target system id.
196    /// * `target_component` - the target component id.
197    /// * `lat_int`, `lon_int` - latitude and longitude, in degrees times 1e7.
198    /// * `alt` - the altitude, in meters in the chosen frame.
199    ///
200    /// # Returns
201    ///
202    /// The setpoint, with only the position fields active in its `type_mask`.
203    pub fn position(
204        time_boot_ms: u32,
205        coordinate_frame: u8,
206        target_system: u8,
207        target_component: u8,
208        lat_int: i32,
209        lon_int: i32,
210        alt: f32,
211    ) -> Self {
212        SetPositionTargetGlobalInt {
213            time_boot_ms,
214            lat_int,
215            lon_int,
216            alt,
217            type_mask: TypeMask::ignore_all().use_position().bits(),
218            target_system,
219            target_component,
220            coordinate_frame,
221            ..Self::zeroed()
222        }
223    }
224}
225
226#[cfg(test)]
227mod tests {
228    use super::*;
229    use crate::dialect::mav_frame;
230
231    #[test]
232    fn ignore_all_sets_every_dimension_but_the_force_flag() {
233        let mask = TypeMask::ignore_all().bits();
234        // Every ignore bit is set.
235        assert_eq!(mask & bits::X_IGNORE, bits::X_IGNORE);
236        assert_eq!(mask & bits::VZ_IGNORE, bits::VZ_IGNORE);
237        assert_eq!(mask & bits::YAW_RATE_IGNORE, bits::YAW_RATE_IGNORE);
238        // FORCE_SET is a mode selector, not an ignore bit, so it stays clear.
239        assert_eq!(mask & bits::FORCE_SET, 0);
240    }
241
242    #[test]
243    fn a_position_mask_enables_only_the_position_bits() {
244        let mask = TypeMask::ignore_all().use_position().bits();
245        // Position is acted on: its ignore bits are clear.
246        assert_eq!(mask & (bits::X_IGNORE | bits::Y_IGNORE | bits::Z_IGNORE), 0);
247        // Everything else is still ignored.
248        assert_eq!(mask & bits::VX_IGNORE, bits::VX_IGNORE);
249        assert_eq!(mask & bits::AX_IGNORE, bits::AX_IGNORE);
250        assert_eq!(mask & bits::YAW_IGNORE, bits::YAW_IGNORE);
251        // The exact value, so a regression in the bit layout is caught.
252        assert_eq!(mask, TypeMask::ALL_IGNORE & !(1 | 2 | 4));
253    }
254
255    #[test]
256    fn a_velocity_setpoint_ignores_position() {
257        let setpoint =
258            SetPositionTargetLocalNed::velocity(1000, mav_frame::LOCAL_NED, 1, 1, 0.5, 0.0, -0.2);
259        assert_eq!(setpoint.vx, 0.5);
260        assert_eq!(setpoint.vz, -0.2);
261        // Position is ignored, velocity is not.
262        assert_eq!(setpoint.type_mask & bits::X_IGNORE, bits::X_IGNORE);
263        assert_eq!(setpoint.type_mask & bits::VX_IGNORE, 0);
264    }
265
266    #[test]
267    fn a_global_position_setpoint_carries_scaled_coordinates() {
268        let setpoint = SetPositionTargetGlobalInt::position(
269            2000,
270            mav_frame::GLOBAL_RELATIVE_ALT_INT,
271            1,
272            1,
273            473_977_418,
274            85_455_939,
275            10.0,
276        );
277        assert_eq!(setpoint.lat_int, 473_977_418);
278        assert_eq!(setpoint.alt, 10.0);
279        assert_eq!(
280            setpoint.type_mask & (bits::X_IGNORE | bits::Y_IGNORE | bits::Z_IGNORE),
281            0
282        );
283    }
284}
pamoja_mavlink/protocol/offboard.rs

pamoja_mavlink/protocol/
offboard.rs
