ados_swarm/controller/types.rs
1//! The controller's public data types: modes, directives, own and neighbour
2//! state, and what one tick produced.
3
4use std::time::Duration;
5
6use crate::precedence::ModePrecedence;
7use crate::setpoint::Setpoint;
8
9/// The operator-commandable behaviour mode (`swarm.mode`).
10///
11/// `hard-separation` and `operator` are deliberately absent: those are precedence
12/// levels the arbitration DERIVES, not modes anybody can ask for.
13#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
14pub enum SwarmMode {
15 #[default]
16 /// No behaviour layer is driving.
17 Hold,
18 /// Flocking drives the vehicle.
19 Flocking,
20 /// A named formation drives the vehicle.
21 Formation,
22}
23
24impl SwarmMode {
25 /// The stable wire string for this mode.
26 pub const fn as_wire(self) -> &'static str {
27 match self {
28 Self::Hold => "hold",
29 Self::Flocking => "flocking",
30 Self::Formation => "formation",
31 }
32 }
33
34 /// Parse `swarm.mode`. Anything unrecognised is `hold` - an unknown mode must
35 /// never fly.
36 pub fn from_wire(s: &str) -> Self {
37 match s.trim().to_ascii_lowercase().as_str() {
38 "flocking" => Self::Flocking,
39 "formation" => Self::Formation,
40 _ => Self::Hold,
41 }
42 }
43}
44
45/// A swarm-level operator directive.
46#[derive(Debug, Clone, Copy, PartialEq)]
47pub enum OperatorDirective {
48 /// Reposition to a geodetic point.
49 Goto {
50 /// Target latitude, degrees.
51 lat_deg: f64,
52 /// Target longitude, degrees.
53 lon_deg: f64,
54 /// Target altitude, metres.
55 alt_m: f64,
56 },
57 /// Stop where you are.
58 Hold,
59}
60
61/// This drone's own state, as the host already holds it.
62#[derive(Debug, Clone, Copy, PartialEq)]
63pub struct OwnState {
64 /// This drone's fleet slot.
65 pub slot: u8,
66 /// Latitude, degrees.
67 pub lat_deg: f64,
68 /// Longitude, degrees.
69 pub lon_deg: f64,
70 /// Altitude relative to home, metres - the frame every setpoint uses.
71 pub alt_rel_m: f64,
72 /// Velocity north, metres/second.
73 pub vn: f64,
74 /// Velocity east, metres/second.
75 pub ve: f64,
76 /// Velocity down, metres/second.
77 pub vd: f64,
78 /// Whether the flight controller reports armed.
79 pub armed: bool,
80 /// Whether the FC reports GUIDED. No setpoint is emitted otherwise.
81 pub guided: bool,
82 /// How long ago the POSITION above was last refreshed by the flight
83 /// controller.
84 ///
85 /// Not the age of the last message of any kind: a heartbeat keeps arriving
86 /// long after a position stops, and that is exactly the case this exists to
87 /// catch. `None` means no position has ever been seen, treated as
88 /// infinitely stale.
89 pub fix_age: Option<Duration>,
90}
91
92impl Default for OwnState {
93 fn default() -> Self {
94 Self {
95 slot: 1,
96 lat_deg: 0.0,
97 lon_deg: 0.0,
98 alt_rel_m: 0.0,
99 vn: 0.0,
100 ve: 0.0,
101 vd: 0.0,
102 armed: false,
103 guided: false,
104 fix_age: None,
105 }
106 }
107}
108
109/// A neighbour straight off the table: geodetic, already dead-reckoned forward to
110/// `now` by the caller.
111#[derive(Debug, Clone, Copy, PartialEq)]
112pub struct NeighborFix {
113 /// The neighbour's fleet slot.
114 pub slot: u8,
115 /// Latitude, degrees.
116 pub lat_deg: f64,
117 /// Longitude, degrees.
118 pub lon_deg: f64,
119 /// Altitude, metres.
120 pub alt_m: f64,
121 /// Velocity north, metres/second.
122 pub vn: f64,
123 /// Velocity east, metres/second.
124 pub ve: f64,
125 /// Down-positive, MAVLink convention: a climbing neighbour is negative.
126 pub vd: f64,
127 /// The beacon status byte verbatim (see [`crate::status`]).
128 pub status: u8,
129 /// This drone's bus sender id compared with the neighbour's
130 /// (`own.cmp(&neighbour)`), `Equal` when either is unknown. Breaks the hard
131 /// override's tie when two drones share a slot.
132 pub sender_order: std::cmp::Ordering,
133}
134
135/// Why a tick emitted nothing.
136#[derive(Debug, Clone, Copy, PartialEq, Eq)]
137pub enum Suppression {
138 /// `swarm.enabled` is false.
139 Disabled,
140 /// The neighbour table has been empty for [`crate::NEIGHBOR_STALE`].
141 NeighborsStale,
142 /// The vehicle is not armed.
143 NotArmed,
144 /// The FC is not in GUIDED.
145 NotGuided,
146 /// This drone's OWN position has not been refreshed inside
147 /// [`crate::OWN_STATE_STALE`].
148 ///
149 /// Distinct from [`Suppression::NeighborsStale`], which is about what the
150 /// fleet is saying. This one is about what we know of ourselves, and it is
151 /// the more dangerous of the two: every neighbour's relative position is
152 /// computed in a frame anchored on our own fix, so a frozen own position
153 /// silently corrupts the whole picture rather than emptying it.
154 OwnStateStale,
155 /// Nothing is driving and the safety term has nothing to say.
156 NothingToDo,
157}
158
159/// One tick's result.
160#[derive(Debug, Clone, Copy, PartialEq)]
161pub struct ControlOutcome {
162 /// The command to send this tick, if any.
163 pub setpoint: Option<Setpoint>,
164 /// The level ACTUALLY driving, which is what rides beacon status bits 5..7.
165 pub precedence: ModePrecedence,
166 /// Whether beacon status bit 2 should be raised on the next beacon.
167 pub emergency: bool,
168 /// Why nothing was sent, when `setpoint` is `None`.
169 pub suppressed: Option<Suppression>,
170}
171
172/// Cheap counters for the status snapshot.
173#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
174pub struct SwarmControlCounters {
175 /// Setpoints emitted since start.
176 pub setpoints_emitted: u64,
177 /// Ticks on which output was suppressed.
178 pub ticks_suppressed: u64,
179 /// Times the hard-separation override engaged.
180 pub hard_engagements: u64,
181}