core_pb/driving/
data.rs

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
use crate::constants::ROBOT_LOGS_BUFFER;
use crate::driving::peripherals::RobotPeripheralsBehavior;
use crate::driving::RobotBehavior;
use crate::messages::{
    ExtraImuData, ExtraOptsAtomicTypes, ExtraOptsTypes, FrequentServerToRobot, MotorControlStatus,
    NetworkStatus, SensorData,
};
use crate::names::RobotName;
use crate::robot_definition::RobotDefinition;
use array_init::array_init;
use core::sync::atomic::Ordering;
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
use embassy_sync::pipe::Pipe;
use embassy_sync::signal::Signal;
use embassy_sync::watch::Watch;
use portable_atomic::{AtomicBool, AtomicF32, AtomicI32, AtomicI8, AtomicU64};

/// Each robot should have exactly one. Some fields are managed by core_pb, but (when noted)
/// implementations are responsible for updating values
#[allow(clippy::type_complexity)]
pub struct SharedRobotData<R: RobotBehavior + ?Sized> {
    /// Robot's name, to distinguish it from other robots, is provided on startup
    pub name: RobotName,
    /// The robot's physical characteristics
    pub robot_definition: RobotDefinition<3>,
    /// An instant representing the time the shared struct was created
    pub created_at: R::Instant,

    //
    // ----------- ENABLE/DISABLE DEVICES ----------
    // See core_pb/src/constants.rs for initial values
    pub enable_imu: AtomicBool,
    pub enable_extra_imu_data: AtomicBool,
    pub enable_dists: AtomicBool,
    pub enable_battery_monitor: AtomicBool,
    pub enable_display: AtomicBool,
    pub enable_gamepad: AtomicBool,
    pub display_loop_interval: AtomicU64,

    //
    // ------------------- INTER TASK DATA -------------------
    //
    /// Information gathered by the peripherals task will be posted here for network and motors
    pub sensors: Watch<CriticalSectionRawMutex, SensorData, 2>,
    /// The current network status, updated by network task
    pub network_status: Watch<CriticalSectionRawMutex, (NetworkStatus, Option<[u8; 4]>), 2>,
    /// Configuration from the server that may change frequently, updated by network task
    ///
    /// Note: some fields are loaded into other atomic primitives
    pub config: Watch<CriticalSectionRawMutex, FrequentServerToRobot, 2>,
    /// Motor control status, updated by motors task
    pub motor_control: Watch<CriticalSectionRawMutex, MotorControlStatus, 2>,
    /// Utilization percentage for the three tasks
    pub utilization: [AtomicF32; 3],

    //
    // ------------------- ROBOT -> CORE DATA -------------------
    //
    /// Estimated motor speeds
    ///
    /// It is the responsibility of the implementation to update this field.
    pub sig_motor_speeds: [AtomicF32; 3],
    /// An estimation of the absolute orientation of the robot
    ///
    /// It is the responsibility of the implementation to update this field.
    pub sig_angle: Signal<
        CriticalSectionRawMutex,
        Result<f32, <R::Peripherals as RobotPeripheralsBehavior>::Error>,
    >,
    /// Individual IMU sensor information
    ///
    /// It is the responsibility of the implementation to update this field.
    pub sig_extra_imu_data: Signal<CriticalSectionRawMutex, ExtraImuData>,
    /// Readings from the distance sensors, in order of angle 0, 90, 180, 270
    ///
    /// It is the responsibility of the implementation to update this field.
    ///
    /// - Err(_) indicates that something is wrong with the sensor and the reading can't be trusted
    /// - Ok(None) indicates that the sensor is working, but didn't detect any object in its range
    /// - Ok(x) indicates an object x grid units in front of the sensor
    pub sig_distances: [Signal<
        CriticalSectionRawMutex,
        Result<Option<f32>, <R::Peripherals as RobotPeripheralsBehavior>::Error>,
    >; 4],
    /// The battery level of the robot, in volts
    ///
    /// It is the responsibility of the implementation to update this field.
    pub sig_battery: Signal<
        CriticalSectionRawMutex,
        Result<f32, <R::Peripherals as RobotPeripheralsBehavior>::Error>,
    >,
    pub buttons: [AtomicBool; 6],
    /// Logging bytes from defmt
    ///
    /// It is the responsibility of the implementation to update this field.
    pub defmt_logs: Pipe<CriticalSectionRawMutex, ROBOT_LOGS_BUFFER>,

    //
    // ------------------- EXTRA -------------------
    //
    pub extra_opts: ExtraOptsAtomicTypes,
    pub extra_indicators: ExtraOptsAtomicTypes,
}

fn make_extra_atomic_types() -> ExtraOptsAtomicTypes {
    (
        array_init(|_| AtomicBool::new(false)),
        array_init(|_| AtomicF32::new(0.0)),
        array_init(|_| AtomicI8::new(0)),
        array_init(|_| AtomicI32::new(0)),
    )
}

impl ExtraOptsTypes {
    pub fn store_into(&self, atomics: &ExtraOptsAtomicTypes) {
        for (i, x) in self.opts_bool.iter().enumerate() {
            atomics.0[i].store(*x, Ordering::Relaxed);
        }
        for (i, x) in self.opts_f32.iter().enumerate() {
            atomics.1[i].store(*x, Ordering::Relaxed);
        }
        for (i, x) in self.opts_i8.iter().enumerate() {
            atomics.2[i].store(*x, Ordering::Relaxed);
        }
        for (i, x) in self.opts_i32.iter().enumerate() {
            atomics.3[i].store(*x, Ordering::Relaxed);
        }
    }

    pub fn load_from(atomics: &ExtraOptsAtomicTypes) -> Self {
        let mut s = Self::default();
        for (i, x) in s.opts_bool.iter_mut().enumerate() {
            *x = atomics.0[i].load(Ordering::Relaxed);
        }
        for (i, x) in s.opts_f32.iter_mut().enumerate() {
            *x = atomics.1[i].load(Ordering::Relaxed);
        }
        for (i, x) in s.opts_i8.iter_mut().enumerate() {
            *x = atomics.2[i].load(Ordering::Relaxed);
        }
        for (i, x) in s.opts_i32.iter_mut().enumerate() {
            *x = atomics.3[i].load(Ordering::Relaxed);
        }
        s
    }
}

impl<R: RobotBehavior> SharedRobotData<R> {
    pub fn new(name: RobotName) -> Self {
        let config = FrequentServerToRobot::new(name);
        Self {
            name,
            robot_definition: RobotDefinition::new(name),
            created_at: R::Instant::default(),

            enable_imu: AtomicBool::new(config.enable_imu),
            enable_extra_imu_data: AtomicBool::new(config.enable_extra_imu_data),
            enable_dists: AtomicBool::new(config.enable_dists),
            enable_battery_monitor: AtomicBool::new(config.enable_battery_monitor),
            enable_display: AtomicBool::new(config.enable_display),
            enable_gamepad: AtomicBool::new(config.enable_gamepad),
            display_loop_interval: AtomicU64::new(config.display_loop_interval),

            sensors: Watch::new(),
            network_status: Watch::new_with((NetworkStatus::NotConnected, None)),
            config: Watch::new_with(config),
            motor_control: Watch::new(),
            utilization: array_init(|_| AtomicF32::new(0.0)),

            sig_motor_speeds: Default::default(),
            sig_angle: Default::default(),
            sig_extra_imu_data: Default::default(),
            sig_distances: Default::default(),
            sig_battery: Default::default(),
            buttons: array_init(|_| AtomicBool::new(false)),
            defmt_logs: Pipe::new(),

            extra_opts: make_extra_atomic_types(),
            extra_indicators: make_extra_atomic_types(),
        }
    }

    #[deprecated = "Extra options should only be used for temporary testing"]
    pub fn get_extra_bool_opt(&self, index: usize) -> Option<bool> {
        self.extra_opts
            .0
            .get(index)
            .map(|b| b.load(Ordering::Relaxed))
    }

    #[deprecated = "Extra options should only be used for temporary testing"]
    pub fn get_extra_f32_opt(&self, index: usize) -> Option<f32> {
        self.extra_opts
            .1
            .get(index)
            .map(|b| b.load(Ordering::Relaxed))
    }

    #[deprecated = "Extra options should only be used for temporary testing"]
    pub fn get_extra_i8_opt(&self, index: usize) -> Option<i8> {
        self.extra_opts
            .2
            .get(index)
            .map(|b| b.load(Ordering::Relaxed))
    }

    #[deprecated = "Extra options should only be used for temporary testing"]
    pub fn get_extra_i32_opt(&self, index: usize) -> Option<i32> {
        self.extra_opts
            .3
            .get(index)
            .map(|b| b.load(Ordering::Relaxed))
    }

    #[deprecated = "Extra indicators should only be used for temporary testing"]
    pub fn set_extra_bool_indicator(&self, index: usize, value: bool) {
        if let Some(b) = self.extra_indicators.0.get(index) {
            b.store(value, Ordering::Relaxed)
        }
    }

    #[deprecated = "Extra indicators should only be used for temporary testing"]
    pub fn set_extra_f32_indicator(&self, index: usize, value: f32) {
        if let Some(b) = self.extra_indicators.1.get(index) {
            b.store(value, Ordering::Relaxed)
        }
    }

    #[deprecated = "Extra indicators should only be used for temporary testing"]
    pub fn set_extra_i8_indicator(&self, index: usize, value: i8) {
        if let Some(b) = self.extra_indicators.2.get(index) {
            b.store(value, Ordering::Relaxed)
        }
    }

    #[deprecated = "Extra indicators should only be used for temporary testing"]
    pub fn set_extra_i32_indicator(&self, index: usize, value: i32) {
        if let Some(b) = self.extra_indicators.3.get(index) {
            b.store(value, Ordering::Relaxed)
        }
    }
}