sim/
ffi.rs

1use std::fs;
2use std::io::{self, BufWriter, Write};
3use std::net::{SocketAddr, UdpSocket};
4use std::path::{Path, PathBuf};
5use std::sync::{
6    Arc, Condvar, Mutex,
7    atomic::{AtomicBool, Ordering},
8};
9use std::thread::{self, JoinHandle};
10use std::time::{Duration, Instant};
11
12use chrono::{Datelike, TimeZone, Timelike, Utc};
13use veloxity_core::{
14    board::BoardIo,
15    controller::quad::QuadController,
16    errors,
17    estimator::quad::QuadEstimator,
18    math::FlightFloat,
19    mixer::{MixerOutputType, matrix::MatrixMixer},
20    packets,
21    params::{PARAM_DEFINITIONS, ParamValue, Params},
22    pwm::{
23        PwmDriver, PwmError, PwmOutputProtocol, effective_output_rate_hz, output_protocol_for_rate,
24        safe_disarmed_command,
25    },
26    sensors::SensorBus,
27    state_machine::StateManager,
28    world::{ControlLoopRates, RealtimeSchedulerStep, RealtimeServicePolicy, World},
29};
30use veloxity_mavlink::MavlinkInterface;
31
32const NUM_PWM_CHANNELS: usize = 14;
33const DEFAULT_MAVLINK_BIND: &str = "127.0.0.1:14525";
34const DEFAULT_MAVLINK_REMOTE: &str = "127.0.0.1:14520";
35const PARAM_DIR_ENV: &str = "VELOXITY_SIM_PARAM_DIR";
36const PARAM_STORE_FILE: &str = "veloxity_sim.params";
37const FIRMWARE_SYNC_TIMEOUT: Duration = Duration::from_millis(5);
38const SIM_CONTROL_LOOP_HZ: u16 = 400;
39const SIM_TELEMETRY_STREAMS_PER_SERVICE_PHASE: usize = 2;
40const SIM_RX_TRACE_ENV: &str = "VELOXITY_SIM_RX_TRACE";
41const MAVLINK_OFFBOARD_CONTROL_MESSAGE_ID: u32 = 180;
42
43#[repr(C)]
44#[derive(Debug, Clone, Copy, Default)]
45pub struct VeloxityFfiVector3 {
46    pub x: f64,
47    pub y: f64,
48    pub z: f64,
49}
50
51#[repr(C)]
52#[derive(Debug, Clone, Copy, Default)]
53pub struct VeloxityFfiImu {
54    pub timestamp_us: u64,
55    pub angular_velocity: VeloxityFfiVector3,
56    pub linear_acceleration: VeloxityFfiVector3,
57    pub temperature_kelvin: f32,
58}
59
60#[repr(C)]
61#[derive(Debug, Clone, Copy, Default)]
62pub struct VeloxityFfiMag {
63    pub timestamp_us: u64,
64    pub magnetic_field: VeloxityFfiVector3,
65}
66
67#[repr(C)]
68#[derive(Debug, Clone, Copy, Default)]
69pub struct VeloxityFfiBaro {
70    pub timestamp_us: u64,
71    pub altitude: f32,
72    pub pressure: f32,
73    pub temperature_kelvin: f32,
74}
75
76#[repr(C)]
77#[derive(Debug, Clone, Copy, Default)]
78pub struct VeloxityFfiGnss {
79    pub timestamp_us: u64,
80    pub fix_type: u8,
81    pub num_sat: u8,
82    pub lat_degrees: f64,
83    pub lon_degrees: f64,
84    pub alt: f32,
85    pub horizontal_accuracy: f32,
86    pub vertical_accuracy: f32,
87    pub vel_n: f32,
88    pub vel_e: f32,
89    pub vel_d: f32,
90    pub speed_accuracy: f32,
91    pub unix_seconds: i64,
92    pub unix_nanos: i32,
93}
94
95#[repr(C)]
96#[derive(Debug, Clone, Copy, Default)]
97pub struct VeloxityFfiAirspeed {
98    pub timestamp_us: u64,
99    pub differential_pressure: f32,
100    pub temperature_kelvin: f32,
101    pub indicated_airspeed: f32,
102}
103
104#[repr(C)]
105#[derive(Debug, Clone, Copy, Default)]
106pub struct VeloxityFfiRange {
107    pub timestamp_us: u64,
108    pub range: f32,
109    pub min_range: f32,
110    pub max_range: f32,
111}
112
113#[repr(C)]
114#[derive(Debug, Clone, Copy, Default)]
115pub struct VeloxityFfiBattery {
116    pub timestamp_us: u64,
117    pub voltage: f32,
118    pub current: f32,
119}
120
121#[repr(C)]
122#[derive(Debug, Clone, Copy)]
123pub struct VeloxityFfiRc {
124    pub timestamp_us: u64,
125    pub values: [u16; 8],
126}
127
128impl Default for VeloxityFfiRc {
129    fn default() -> Self {
130        Self {
131            timestamp_us: 0,
132            values: [1500, 1500, 1000, 1500, 1000, 1000, 1000, 1000],
133        }
134    }
135}
136
137#[repr(C)]
138#[derive(Debug, Clone, Copy, Default)]
139pub struct VeloxityFfiSensorSnapshot {
140    pub has_imu: bool,
141    pub imu: VeloxityFfiImu,
142    pub has_mag: bool,
143    pub mag: VeloxityFfiMag,
144    pub has_baro: bool,
145    pub baro: VeloxityFfiBaro,
146    pub has_gnss: bool,
147    pub gnss: VeloxityFfiGnss,
148    pub has_airspeed: bool,
149    pub airspeed: VeloxityFfiAirspeed,
150    pub has_range: bool,
151    pub range: VeloxityFfiRange,
152    pub has_battery: bool,
153    pub battery: VeloxityFfiBattery,
154    pub has_rc: bool,
155    pub rc: VeloxityFfiRc,
156}
157
158#[derive(Default)]
159struct SharedSensors {
160    // Mirrors the hardware Signal slots: each value remains pending until the board consumes it,
161    // while a newer value replaces an older unconsumed value.
162    pending: VeloxityFfiSensorSnapshot,
163    latest_imu_generation: u64,
164    pending_imu_generation: u64,
165    consumed_imu_generation: u64,
166}
167
168impl SharedSensors {
169    fn merge(&mut self, incoming: VeloxityFfiSensorSnapshot) {
170        macro_rules! replace_pending {
171            ($has:ident, $value:ident) => {
172                if incoming.$has {
173                    self.pending.$has = true;
174                    self.pending.$value = incoming.$value;
175                }
176            };
177        }
178
179        replace_pending!(has_imu, imu);
180        replace_pending!(has_mag, mag);
181        replace_pending!(has_baro, baro);
182        replace_pending!(has_gnss, gnss);
183        replace_pending!(has_airspeed, airspeed);
184        replace_pending!(has_range, range);
185        replace_pending!(has_battery, battery);
186        replace_pending!(has_rc, rc);
187
188        if incoming.has_imu {
189            self.latest_imu_generation = self.latest_imu_generation.wrapping_add(1).max(1);
190            self.pending_imu_generation = self.latest_imu_generation;
191        }
192    }
193
194    fn imu_pending(&self) -> bool {
195        self.pending.has_imu
196    }
197
198    fn take_imu(&mut self) -> Option<VeloxityFfiImu> {
199        if !self.pending.has_imu {
200            return None;
201        }
202        self.pending.has_imu = false;
203        self.consumed_imu_generation = self.pending_imu_generation;
204        Some(self.pending.imu)
205    }
206
207    fn take_snapshot(&mut self, include_imu: bool) -> VeloxityFfiSensorSnapshot {
208        let mut snapshot = self.pending;
209        if include_imu && snapshot.has_imu {
210            self.pending.has_imu = false;
211            self.consumed_imu_generation = self.pending_imu_generation;
212        } else {
213            snapshot.has_imu = false;
214        }
215        self.pending.has_mag = false;
216        self.pending.has_baro = false;
217        self.pending.has_gnss = false;
218        self.pending.has_airspeed = false;
219        self.pending.has_range = false;
220        self.pending.has_battery = false;
221        self.pending.has_rc = false;
222        snapshot
223    }
224}
225
226struct FirmwareProgress {
227    processed_imu_generation: u64,
228    pwm_outputs: [u16; NUM_PWM_CHANNELS],
229    worker_failed: bool,
230}
231
232impl Default for FirmwareProgress {
233    fn default() -> Self {
234        Self {
235            processed_imu_generation: 0,
236            pwm_outputs: [1000; NUM_PWM_CHANNELS],
237            worker_failed: false,
238        }
239    }
240}
241
242#[derive(Clone)]
243struct FfiPwmDriver {
244    outputs: Arc<Mutex<[u16; NUM_PWM_CHANNELS]>>,
245    output_rates_hz: [f64; NUM_PWM_CHANNELS],
246    output_protocols: [PwmOutputProtocol; NUM_PWM_CHANNELS],
247}
248
249impl FfiPwmDriver {
250    fn new(outputs: Arc<Mutex<[u16; NUM_PWM_CHANNELS]>>) -> Self {
251        Self {
252            outputs,
253            output_rates_hz: [50.0; NUM_PWM_CHANNELS],
254            output_protocols: [PwmOutputProtocol::StandardPwm; NUM_PWM_CHANNELS],
255        }
256    }
257
258    fn set_pwm(&mut self, channel: usize, pwm_us: u16) -> Result<(), PwmError> {
259        if channel >= NUM_PWM_CHANNELS {
260            return Err(PwmError::ChannelOutOfRange);
261        }
262        if let Ok(mut outputs) = self.outputs.lock() {
263            outputs[channel] = pwm_us;
264            Ok(())
265        } else {
266            Err(PwmError::GenericError)
267        }
268    }
269}
270
271impl PwmDriver<f64> for FfiPwmDriver {
272    fn len(&self) -> usize {
273        NUM_PWM_CHANNELS
274    }
275
276    fn is_enabled(&self) -> bool {
277        true
278    }
279
280    fn enable(&mut self, channel: usize) -> Result<(), PwmError> {
281        if channel >= NUM_PWM_CHANNELS {
282            Err(PwmError::ChannelOutOfRange)
283        } else {
284            Ok(())
285        }
286    }
287
288    fn disable(&mut self, channel: usize) -> Result<(), PwmError> {
289        self.set_pwm(channel, 1000)
290    }
291
292    fn enable_all(&mut self) -> Result<(), PwmError> {
293        Ok(())
294    }
295
296    fn disable_all(&mut self) {
297        if let Ok(mut outputs) = self.outputs.lock() {
298            outputs.fill(1000);
299        }
300    }
301
302    fn set_duty_cycle(&mut self, channel: usize, duty: u16) -> Result<(), PwmError> {
303        let normalized = duty as f32 / u16::MAX as f32;
304        self.set_pwm(channel, (1000.0 + normalized * 1000.0) as u16)
305    }
306
307    fn flush<B: BoardIo>(&mut self, _board: &mut B) {}
308
309    fn configure_output_rates(&mut self, rates_hz: &[f64]) -> Result<(), PwmError> {
310        for (index, rate) in rates_hz.iter().take(NUM_PWM_CHANNELS).enumerate() {
311            self.output_protocols[index] = output_protocol_for_rate(*rate)?;
312            self.output_rates_hz[index] = effective_output_rate_hz(*rate)?;
313        }
314        Ok(())
315    }
316
317    fn output_protocol(&self, channel: usize) -> Result<PwmOutputProtocol, PwmError> {
318        self.output_protocols
319            .get(channel)
320            .copied()
321            .ok_or(PwmError::ChannelOutOfRange)
322    }
323
324    fn send_commands<B: BoardIo>(
325        &mut self,
326        _board: &mut B,
327        commands: &[f64],
328    ) -> Result<(), PwmError> {
329        for (channel, command) in commands.iter().take(NUM_PWM_CHANNELS).enumerate() {
330            let pwm_us = 1000.0 + command.clamp(0.0, 1.0) * 1000.0;
331            self.set_pwm(channel, pwm_us as u16)?;
332        }
333        Ok(())
334    }
335
336    fn send_disarmed_commands<B: BoardIo>(
337        &mut self,
338        _board: &mut B,
339        output_types: &[MixerOutputType],
340    ) -> Result<(), PwmError> {
341        for (channel, output_type) in output_types.iter().take(NUM_PWM_CHANNELS).enumerate() {
342            let command = safe_disarmed_command::<f64>(*output_type);
343            let output = match self.output_protocols[channel] {
344                PwmOutputProtocol::StandardPwm => 1000.0 + command * 1000.0,
345                PwmOutputProtocol::Dshot if *output_type == MixerOutputType::Motor => 0.0,
346                PwmOutputProtocol::Dshot => command,
347            };
348            self.set_pwm(channel, output as u16)?;
349        }
350        Ok(())
351    }
352}
353
354struct FfiBoard {
355    start_time: Instant,
356    mavlink_socket: UdpSocket,
357    sensors: Arc<Mutex<SharedSensors>>,
358    param_store_path: PathBuf,
359    last_mag_timestamp_us: u64,
360    last_baro_timestamp_us: u64,
361    last_gnss_timestamp_us: u64,
362    last_airspeed_timestamp_us: u64,
363    last_range_timestamp_us: u64,
364    last_battery_timestamp_us: u64,
365    last_rc_timestamp_us: u64,
366    rx_trace: Option<BufWriter<fs::File>>,
367}
368
369impl FfiBoard {
370    fn new(sensors: Arc<Mutex<SharedSensors>>, start_time: Instant) -> io::Result<Self> {
371        let bind_addr: SocketAddr = std::env::var("VELOXITY_MAVLINK_BIND")
372            .unwrap_or_else(|_| DEFAULT_MAVLINK_BIND.into())
373            .parse()
374            .map_err(|err| io::Error::new(io::ErrorKind::InvalidInput, err))?;
375        let remote_addr: SocketAddr = std::env::var("VELOXITY_MAVLINK_REMOTE")
376            .unwrap_or_else(|_| DEFAULT_MAVLINK_REMOTE.into())
377            .parse()
378            .map_err(|err| io::Error::new(io::ErrorKind::InvalidInput, err))?;
379        let mavlink_socket = UdpSocket::bind(bind_addr)?;
380        mavlink_socket.connect(remote_addr)?;
381        mavlink_socket.set_nonblocking(true)?;
382
383        let rx_trace = std::env::var_os(SIM_RX_TRACE_ENV)
384            .and_then(|path| fs::File::create(path).ok())
385            .map(|file| {
386                let mut writer = BufWriter::new(file);
387                let _ = writeln!(
388                    writer,
389                    "firmware_time_us,datagram_bytes,mavlink_frames,offboard_frames"
390                );
391                writer
392            });
393
394        Ok(Self {
395            start_time,
396            mavlink_socket,
397            sensors,
398            param_store_path: param_store_path()?,
399            last_mag_timestamp_us: 0,
400            last_baro_timestamp_us: 0,
401            last_gnss_timestamp_us: 0,
402            last_airspeed_timestamp_us: 0,
403            last_range_timestamp_us: 0,
404            last_battery_timestamp_us: 0,
405            last_rc_timestamp_us: 0,
406            rx_trace,
407        })
408    }
409
410    fn trace_rx_datagram(&mut self, bytes: &[u8]) {
411        let Some(_) = self.rx_trace else {
412            return;
413        };
414        let now_us = self.clock_micros();
415        let (mavlink_frames, offboard_frames) = count_mavlink_frames(bytes);
416        if let Some(trace) = &mut self.rx_trace {
417            let _ = writeln!(
418                trace,
419                "{now_us},{},{mavlink_frames},{offboard_frames}",
420                bytes.len()
421            );
422        }
423    }
424}
425
426fn count_mavlink_frames(bytes: &[u8]) -> (usize, usize) {
427    let mut index = 0;
428    let mut frame_count = 0;
429    let mut offboard_count = 0;
430    while index < bytes.len() {
431        let (frame_len, message_id) = match bytes[index] {
432            0xfe if index + 6 <= bytes.len() => {
433                let payload_len = bytes[index + 1] as usize;
434                (payload_len + 8, bytes[index + 5] as u32)
435            }
436            0xfd if index + 10 <= bytes.len() => {
437                let payload_len = bytes[index + 1] as usize;
438                let signature_len = if bytes[index + 2] & 0x01 != 0 { 13 } else { 0 };
439                let message_id = bytes[index + 7] as u32
440                    | ((bytes[index + 8] as u32) << 8)
441                    | ((bytes[index + 9] as u32) << 16);
442                (payload_len + 12 + signature_len, message_id)
443            }
444            _ => {
445                index += 1;
446                continue;
447            }
448        };
449        if index + frame_len > bytes.len() {
450            break;
451        }
452        frame_count += 1;
453        offboard_count += usize::from(message_id == MAVLINK_OFFBOARD_CONTROL_MESSAGE_ID);
454        index += frame_len;
455    }
456    (frame_count, offboard_count)
457}
458
459impl FfiBoard {
460    fn update_sensor_bus_impl<R: FlightFloat>(
461        &mut self,
462        sensors: &mut SensorBus<R>,
463        include_imu: bool,
464    ) {
465        sensors.clear();
466        let Ok(mut shared) = self.sensors.lock() else {
467            return;
468        };
469        let snapshot = shared.take_snapshot(include_imu);
470
471        if snapshot.has_imu {
472            sensors.imu = Some(Ok(ffi_imu_packet(snapshot.imu)));
473        }
474
475        if snapshot.has_mag && snapshot.mag.timestamp_us > self.last_mag_timestamp_us {
476            self.last_mag_timestamp_us = snapshot.mag.timestamp_us;
477            sensors.mag = Some(Ok(packets::MagPacket {
478                header: packets::RosflightPacketHeader {
479                    timestamp: snapshot.mag.timestamp_us,
480                    status: 0,
481                },
482                flux: [
483                    snapshot.mag.magnetic_field.x as f32,
484                    snapshot.mag.magnetic_field.y as f32,
485                    snapshot.mag.magnetic_field.z as f32,
486                ],
487                temperature: 25.0,
488            }));
489        }
490
491        if snapshot.has_baro && snapshot.baro.timestamp_us > self.last_baro_timestamp_us {
492            self.last_baro_timestamp_us = snapshot.baro.timestamp_us;
493            sensors.baro = Some(Ok(packets::BaroPacket {
494                header: packets::RosflightPacketHeader {
495                    timestamp: snapshot.baro.timestamp_us,
496                    status: 0,
497                },
498                pressure: snapshot.baro.pressure,
499                temperature: snapshot.baro.temperature_kelvin,
500                altitude: snapshot.baro.altitude,
501            }));
502        }
503
504        if snapshot.has_gnss && snapshot.gnss.timestamp_us > self.last_gnss_timestamp_us {
505            self.last_gnss_timestamp_us = snapshot.gnss.timestamp_us;
506            let dt = Utc
507                .timestamp_opt(snapshot.gnss.unix_seconds, snapshot.gnss.unix_nanos as u32)
508                .latest()
509                .unwrap_or_default();
510            sensors.gnss = Some(Ok(packets::GNSSPacket {
511                header: packets::RosflightPacketHeader {
512                    timestamp: snapshot.gnss.timestamp_us,
513                    status: 0,
514                },
515                unix_seconds: snapshot.gnss.unix_seconds,
516                unix_nanos: snapshot.gnss.unix_nanos,
517                lat: snapshot.gnss.lat_degrees,
518                lon: snapshot.gnss.lon_degrees,
519                height: snapshot.gnss.alt,
520                vel_n: snapshot.gnss.vel_n,
521                vel_e: snapshot.gnss.vel_e,
522                vel_d: snapshot.gnss.vel_d,
523                h_acc: snapshot.gnss.horizontal_accuracy,
524                v_acc: snapshot.gnss.vertical_accuracy,
525                s_acc: snapshot.gnss.speed_accuracy,
526                month: dt.month0() as u8,
527                year: dt.year() as u16,
528                day: dt.day() as u8,
529                hour: dt.hour() as u8,
530                min: dt.minute() as u8,
531                sec: dt.second() as u8,
532                nano: dt.nanosecond() as i32,
533                fix_type: packets::GNSSFixType::from_u8(snapshot.gnss.fix_type),
534                num_sats: snapshot.gnss.num_sat,
535                mag_dec: 0.0,
536                time_correction: 0,
537            }));
538        }
539
540        if snapshot.has_airspeed && snapshot.airspeed.timestamp_us > self.last_airspeed_timestamp_us
541        {
542            self.last_airspeed_timestamp_us = snapshot.airspeed.timestamp_us;
543            sensors.pitot = Some(Ok(packets::PitotPacket {
544                header: packets::RosflightPacketHeader {
545                    timestamp: snapshot.airspeed.timestamp_us,
546                    status: 0,
547                },
548                differential_pressure: snapshot.airspeed.differential_pressure,
549                temperature: snapshot.airspeed.temperature_kelvin,
550                indicated_airspeed: snapshot.airspeed.indicated_airspeed,
551            }));
552        }
553
554        if snapshot.has_range && snapshot.range.timestamp_us > self.last_range_timestamp_us {
555            self.last_range_timestamp_us = snapshot.range.timestamp_us;
556            sensors.range = Some(Ok(packets::RangePacket {
557                header: packets::RosflightPacketHeader {
558                    timestamp: snapshot.range.timestamp_us,
559                    status: 0,
560                },
561                range: snapshot.range.range,
562                min_range: snapshot.range.min_range,
563                max_range: snapshot.range.max_range,
564                range_type: packets::RangeType::Sonar,
565            }));
566        }
567
568        if snapshot.has_battery && snapshot.battery.timestamp_us > self.last_battery_timestamp_us {
569            self.last_battery_timestamp_us = snapshot.battery.timestamp_us;
570            sensors.battery = Some(Ok(packets::BatteryPacket {
571                header: packets::RosflightPacketHeader {
572                    timestamp: snapshot.battery.timestamp_us,
573                    status: 0,
574                },
575                voltage: snapshot.battery.voltage,
576                current: snapshot.battery.current,
577            }));
578        }
579
580        if snapshot.has_rc && snapshot.rc.timestamp_us > self.last_rc_timestamp_us {
581            self.last_rc_timestamp_us = snapshot.rc.timestamp_us;
582            let mut channels = [0.0f32; packets::RC_PACKET_CHANNELS];
583            for (index, value) in snapshot.rc.values.iter().enumerate() {
584                channels[index] = (*value as f32 - 1000.0) / 1000.0;
585            }
586            sensors.rc = Some(Ok(packets::RcPacket {
587                header: packets::RosflightPacketHeader {
588                    timestamp: snapshot.rc.timestamp_us,
589                    status: 0,
590                },
591                n_chan: snapshot.rc.values.len() as u32,
592                chan: channels,
593                lol: false,
594            }));
595        }
596    }
597}
598
599impl BoardIo for FfiBoard {
600    fn update_sensor_bus<R: FlightFloat>(&mut self, sensors: &mut SensorBus<R>) {
601        self.update_sensor_bus_impl(sensors, true);
602    }
603
604    fn update_service_sensor_bus<R: FlightFloat>(&mut self, sensors: &mut SensorBus<R>) {
605        self.update_sensor_bus_impl(sensors, false);
606    }
607
608    fn imu_pending(&self) -> bool {
609        self.sensors.lock().is_ok_and(|shared| shared.imu_pending())
610    }
611
612    fn update_imu_sensor<R: FlightFloat>(&mut self, sensors: &mut SensorBus<R>) {
613        sensors.clear();
614        let Ok(mut shared) = self.sensors.lock() else {
615            return;
616        };
617        if let Some(imu) = shared.take_imu() {
618            sensors.imu = Some(Ok(ffi_imu_packet(imu)));
619        }
620    }
621
622    fn serial_rx_read(&mut self, buf: &mut [u8]) -> Option<Result<usize, errors::TelemError>> {
623        match self.mavlink_socket.recv(buf) {
624            Ok(size) => {
625                self.trace_rx_datagram(&buf[..size]);
626                Some(Ok(size))
627            }
628            Err(err) if err.kind() == io::ErrorKind::WouldBlock => None,
629            Err(_) => Some(Err(errors::TelemError::GenericTelemError(
630                "error reading MAVLink UDP socket",
631            ))),
632        }
633    }
634
635    fn serial_tx_write(&mut self, bytes: &[u8]) -> Option<Result<usize, errors::TelemError>> {
636        match self.mavlink_socket.send(bytes) {
637            Ok(size) => Some(Ok(size)),
638            Err(err) if err.kind() == io::ErrorKind::WouldBlock => Some(Err(
639                errors::TelemError::GenericTelemError("MAVLink UDP socket send buffer full"),
640            )),
641            Err(_) => Some(Err(errors::TelemError::GenericTelemError(
642                "error writing MAVLink UDP socket",
643            ))),
644        }
645    }
646
647    fn clock_millis(&self) -> u32 {
648        self.start_time.elapsed().as_millis() as u32
649    }
650
651    fn clock_micros(&self) -> u64 {
652        self.start_time.elapsed().as_micros() as u64
653    }
654
655    fn read_params(&mut self, params: &mut Params) -> bool {
656        read_params_from_path(&self.param_store_path, params).is_ok()
657    }
658
659    fn write_params(&mut self, params: &Params) -> bool {
660        write_params_to_path(&self.param_store_path, params).is_ok()
661    }
662}
663
664fn ffi_imu_packet<R: FlightFloat>(imu: VeloxityFfiImu) -> packets::ImuPacket<R> {
665    packets::ImuPacket {
666        header: packets::RosflightPacketHeader {
667            timestamp: imu.timestamp_us,
668            status: 0,
669        },
670        accel: [
671            <R as FlightFloat>::from_f64(imu.linear_acceleration.x),
672            <R as FlightFloat>::from_f64(imu.linear_acceleration.y),
673            <R as FlightFloat>::from_f64(imu.linear_acceleration.z),
674        ],
675        gyro: [
676            <R as FlightFloat>::from_f64(imu.angular_velocity.x),
677            <R as FlightFloat>::from_f64(imu.angular_velocity.y),
678            <R as FlightFloat>::from_f64(imu.angular_velocity.z),
679        ],
680        temperature: imu.temperature_kelvin,
681        seq: 0,
682    }
683}
684
685type FfiWorld = World<
686    FfiBoard,
687    QuadEstimator<f64>,
688    QuadController<f64>,
689    MatrixMixer<f64>,
690    MavlinkInterface,
691    FfiPwmDriver,
692    f64,
693>;
694
695pub struct VeloxityFfiHandle {
696    sensors: Arc<Mutex<SharedSensors>>,
697    progress: Arc<(Mutex<FirmwareProgress>, Condvar)>,
698    shutdown: Arc<AtomicBool>,
699    start_time: Instant,
700    worker: Option<JoinHandle<()>>,
701}
702
703impl Drop for VeloxityFfiHandle {
704    fn drop(&mut self) {
705        self.shutdown.store(true, Ordering::Release);
706        self.progress.1.notify_all();
707        if let Some(worker) = self.worker.take() {
708            let _ = worker.join();
709        }
710    }
711}
712
713#[unsafe(no_mangle)]
714pub extern "C" fn veloxity_sim_create() -> *mut VeloxityFfiHandle {
715    let sensors = Arc::new(Mutex::new(SharedSensors::default()));
716    let outputs = Arc::new(Mutex::new([1000; NUM_PWM_CHANNELS]));
717    let progress = Arc::new((Mutex::new(FirmwareProgress::default()), Condvar::new()));
718    let shutdown = Arc::new(AtomicBool::new(false));
719    let start_time = Instant::now();
720
721    let Ok(mut board) = FfiBoard::new(Arc::clone(&sensors), start_time) else {
722        return std::ptr::null_mut();
723    };
724
725    let mut params = Params::new();
726    let _ = board.read_params(&mut params);
727    let estimator = QuadEstimator::default();
728    let controller = QuadController::default();
729    let mixer = MatrixMixer::new(&params);
730    let mavlink = MavlinkInterface::new();
731    let state = StateManager::new();
732    let pwm = FfiPwmDriver::new(Arc::clone(&outputs));
733
734    let mut world = FfiWorld::init(
735        board, params, mavlink, state, estimator, controller, mixer, pwm,
736    );
737    world.set_control_loop_rates(ControlLoopRates::fixed_rate_hz(SIM_CONTROL_LOOP_HZ));
738
739    let worker_sensors = Arc::clone(&sensors);
740    let worker_outputs = Arc::clone(&outputs);
741    let worker_progress = Arc::clone(&progress);
742    let worker_shutdown = Arc::clone(&shutdown);
743    let Ok(worker) = thread::Builder::new()
744        .name("veloxity-sim-firmware".into())
745        .spawn(move || {
746            let result = std::panic::catch_unwind(std::panic::AssertUnwindSafe(|| {
747                run_firmware_worker(
748                    world,
749                    worker_sensors,
750                    worker_outputs,
751                    Arc::clone(&worker_progress),
752                    Arc::clone(&worker_shutdown),
753                );
754            }));
755            if result.is_err() {
756                if let Ok(mut progress) = worker_progress.0.lock() {
757                    progress.worker_failed = true;
758                }
759                worker_progress.1.notify_all();
760                worker_shutdown.store(true, Ordering::Release);
761            }
762        })
763    else {
764        return std::ptr::null_mut();
765    };
766
767    Box::into_raw(Box::new(VeloxityFfiHandle {
768        sensors,
769        progress,
770        shutdown,
771        start_time,
772        worker: Some(worker),
773    }))
774}
775
776fn run_firmware_worker(
777    mut world: FfiWorld,
778    sensors: Arc<Mutex<SharedSensors>>,
779    outputs: Arc<Mutex<[u16; NUM_PWM_CHANNELS]>>,
780    progress: Arc<(Mutex<FirmwareProgress>, Condvar)>,
781    shutdown: Arc<AtomicBool>,
782) {
783    while !shutdown.load(Ordering::Acquire) {
784        let processed_imu_generation = match world.realtime_scheduler_step() {
785            RealtimeSchedulerStep::ImuControl => {
786                let _ = world.run_imu_control_tick();
787                sensors
788                    .lock()
789                    .ok()
790                    .map(|sensors| sensors.consumed_imu_generation)
791            }
792            RealtimeSchedulerStep::ControlUpdate => {
793                let _ = world.run_control_update_tick();
794                None
795            }
796            RealtimeSchedulerStep::Service => {
797                let _ = world.run_prioritized_service_steps_with_policy(
798                    RealtimeServicePolicy::continuous_polling(
799                        SIM_TELEMETRY_STREAMS_PER_SERVICE_PHASE,
800                    ),
801                );
802                None
803            }
804            RealtimeSchedulerStep::Idle => {
805                std::hint::spin_loop();
806                continue;
807            }
808        };
809
810        let Some(pwm_outputs) = outputs.lock().ok().map(|outputs| *outputs) else {
811            continue;
812        };
813        let Ok(mut worker_progress) = progress.0.lock() else {
814            continue;
815        };
816        worker_progress.pwm_outputs = pwm_outputs;
817        if let Some(generation) = processed_imu_generation {
818            worker_progress.processed_imu_generation =
819                worker_progress.processed_imu_generation.max(generation);
820        }
821        drop(worker_progress);
822        progress.1.notify_all();
823    }
824}
825
826#[unsafe(no_mangle)]
827pub unsafe extern "C" fn veloxity_sim_destroy(handle: *mut VeloxityFfiHandle) {
828    if !handle.is_null() {
829        drop(unsafe { Box::from_raw(handle) });
830    }
831}
832
833#[unsafe(no_mangle)]
834pub unsafe extern "C" fn veloxity_sim_set_sensors(
835    handle: *const VeloxityFfiHandle,
836    snapshot: *const VeloxityFfiSensorSnapshot,
837) -> bool {
838    if handle.is_null() || snapshot.is_null() {
839        return false;
840    }
841
842    let handle = unsafe { &*handle };
843    let Ok(mut sensors) = handle.sensors.lock() else {
844        return false;
845    };
846    sensors.merge(unsafe { *snapshot });
847    true
848}
849
850#[unsafe(no_mangle)]
851pub unsafe extern "C" fn veloxity_sim_sync_latest_imu(handle: *const VeloxityFfiHandle) -> bool {
852    if handle.is_null() {
853        return false;
854    }
855
856    let handle = unsafe { &*handle };
857    let target_generation = match handle.sensors.lock() {
858        Ok(sensors) => sensors.latest_imu_generation,
859        Err(_) => return false,
860    };
861    if target_generation == 0 {
862        return true;
863    }
864
865    wait_for_imu_generation(
866        &handle.progress,
867        &handle.shutdown,
868        target_generation,
869        FIRMWARE_SYNC_TIMEOUT,
870    )
871}
872
873fn wait_for_imu_generation(
874    progress: &(Mutex<FirmwareProgress>, Condvar),
875    shutdown: &AtomicBool,
876    target_generation: u64,
877    timeout: Duration,
878) -> bool {
879    let Ok(progress_guard) = progress.0.lock() else {
880        return false;
881    };
882    let Ok((progress_guard, _)) =
883        progress
884            .1
885            .wait_timeout_while(progress_guard, timeout, |worker_progress| {
886                worker_progress.processed_imu_generation < target_generation
887                    && !worker_progress.worker_failed
888                    && !shutdown.load(Ordering::Acquire)
889            })
890    else {
891        return false;
892    };
893    !progress_guard.worker_failed
894        && !shutdown.load(Ordering::Acquire)
895        && progress_guard.processed_imu_generation >= target_generation
896}
897
898#[unsafe(no_mangle)]
899pub unsafe extern "C" fn veloxity_sim_get_pwm(
900    handle: *const VeloxityFfiHandle,
901    output: *mut u16,
902    output_len: usize,
903) -> usize {
904    if handle.is_null() || output.is_null() {
905        return 0;
906    }
907
908    let handle = unsafe { &*handle };
909    let Ok(progress) = handle.progress.0.lock() else {
910        return 0;
911    };
912    let copy_len = output_len.min(progress.pwm_outputs.len());
913    unsafe {
914        std::ptr::copy_nonoverlapping(progress.pwm_outputs.as_ptr(), output, copy_len);
915    }
916    copy_len
917}
918
919#[unsafe(no_mangle)]
920pub unsafe extern "C" fn veloxity_sim_clock_micros(handle: *const VeloxityFfiHandle) -> u64 {
921    if handle.is_null() {
922        return 0;
923    }
924    let handle = unsafe { &*handle };
925    handle.start_time.elapsed().as_micros() as u64
926}
927
928fn param_store_path() -> io::Result<PathBuf> {
929    let Some(dir) = std::env::var_os(PARAM_DIR_ENV) else {
930        return Err(io::Error::new(
931            io::ErrorKind::NotFound,
932            "VELOXITY_SIM_PARAM_DIR must point to a writable runtime parameter directory",
933        ));
934    };
935    let dir = PathBuf::from(dir);
936    fs::create_dir_all(&dir)?;
937    Ok(dir.join(PARAM_STORE_FILE))
938}
939
940fn write_params_to_path(path: &Path, params: &Params) -> io::Result<()> {
941    if let Some(parent) = path
942        .parent()
943        .filter(|parent| !parent.as_os_str().is_empty())
944    {
945        fs::create_dir_all(parent)?;
946    }
947
948    let mut contents = Vec::new();
949    for definition in PARAM_DEFINITIONS.iter() {
950        writeln!(
951            contents,
952            "{}={}",
953            definition.name,
954            format_param_value(params.get_by_id(definition.id))
955        )?;
956    }
957
958    let temp_path = path.with_extension("tmp");
959    fs::write(&temp_path, contents)?;
960    fs::rename(temp_path, path)
961}
962
963#[cfg(test)]
964mod tests {
965    use super::*;
966
967    static FFI_ENV_LOCK: Mutex<()> = Mutex::new(());
968
969    fn imu_snapshot(timestamp_us: u64) -> VeloxityFfiSensorSnapshot {
970        VeloxityFfiSensorSnapshot {
971            has_imu: true,
972            imu: VeloxityFfiImu {
973                timestamp_us,
974                ..VeloxityFfiImu::default()
975            },
976            ..VeloxityFfiSensorSnapshot::default()
977        }
978    }
979
980    #[test]
981    fn receive_trace_counts_v1_and_v2_offboard_frames() {
982        let mavlink_v1 = [0xfe, 0, 1, 1, 1, 180, 0, 0];
983        let mavlink_v2 = [0xfd, 0, 0, 0, 2, 1, 1, 180, 0, 0, 0, 0];
984        let other_v1 = [0xfe, 0, 3, 1, 1, 0, 0, 0];
985        let bytes = [
986            mavlink_v1.as_slice(),
987            mavlink_v2.as_slice(),
988            other_v1.as_slice(),
989        ]
990        .concat();
991
992        assert_eq!(count_mavlink_frames(&bytes), (3, 2));
993    }
994
995    #[test]
996    fn receive_trace_ignores_truncated_frames() {
997        assert_eq!(count_mavlink_frames(&[0xfe, 10, 1, 1, 1, 180]), (0, 0));
998    }
999
1000    #[test]
1001    fn imu_remains_pending_until_consumed() {
1002        let mut sensors = SharedSensors::default();
1003        sensors.merge(imu_snapshot(10));
1004        sensors.merge(VeloxityFfiSensorSnapshot::default());
1005
1006        assert!(sensors.imu_pending());
1007        assert_eq!(sensors.latest_imu_generation, 1);
1008        assert_eq!(sensors.take_imu().map(|imu| imu.timestamp_us), Some(10));
1009        assert_eq!(sensors.consumed_imu_generation, 1);
1010        assert!(!sensors.imu_pending());
1011        assert!(sensors.take_imu().is_none());
1012    }
1013
1014    #[test]
1015    fn newer_imu_replaces_unconsumed_sample() {
1016        let mut sensors = SharedSensors::default();
1017        sensors.merge(imu_snapshot(10));
1018        sensors.merge(imu_snapshot(20));
1019
1020        assert_eq!(sensors.take_imu().map(|imu| imu.timestamp_us), Some(20));
1021        assert_eq!(sensors.latest_imu_generation, 2);
1022        assert_eq!(sensors.consumed_imu_generation, 2);
1023    }
1024
1025    #[test]
1026    fn service_snapshot_does_not_consume_pending_imu() {
1027        let mut sensors = SharedSensors::default();
1028        sensors.merge(imu_snapshot(10));
1029        sensors.merge(VeloxityFfiSensorSnapshot {
1030            has_rc: true,
1031            ..VeloxityFfiSensorSnapshot::default()
1032        });
1033
1034        let service_snapshot = sensors.take_snapshot(false);
1035
1036        assert!(!service_snapshot.has_imu);
1037        assert!(service_snapshot.has_rc);
1038        assert!(sensors.imu_pending());
1039        assert_eq!(sensors.consumed_imu_generation, 0);
1040    }
1041
1042    #[test]
1043    fn generation_barrier_accepts_a_newer_processed_replacement() {
1044        let progress = Arc::new((Mutex::new(FirmwareProgress::default()), Condvar::new()));
1045        let shutdown = Arc::new(AtomicBool::new(false));
1046        let worker_progress = Arc::clone(&progress);
1047        let worker = thread::spawn(move || {
1048            let mut progress = worker_progress.0.lock().unwrap();
1049            progress.processed_imu_generation = 2;
1050            drop(progress);
1051            worker_progress.1.notify_all();
1052        });
1053
1054        assert!(wait_for_imu_generation(
1055            &progress,
1056            &shutdown,
1057            1,
1058            Duration::from_millis(100),
1059        ));
1060        worker.join().unwrap();
1061    }
1062
1063    #[test]
1064    fn generation_barrier_returns_on_shutdown() {
1065        let progress = (Mutex::new(FirmwareProgress::default()), Condvar::new());
1066        let shutdown = AtomicBool::new(true);
1067
1068        assert!(!wait_for_imu_generation(
1069            &progress,
1070            &shutdown,
1071            1,
1072            Duration::from_millis(100),
1073        ));
1074    }
1075
1076    #[test]
1077    fn firmware_worker_processes_imu_and_shuts_down_cleanly() {
1078        let _env_guard = FFI_ENV_LOCK.lock().unwrap();
1079        let bind_probe = UdpSocket::bind("127.0.0.1:0").unwrap();
1080        let bind_addr = bind_probe.local_addr().unwrap();
1081        drop(bind_probe);
1082        let remote = UdpSocket::bind("127.0.0.1:0").unwrap();
1083        let param_dir = std::env::temp_dir().join(format!(
1084            "veloxity-ffi-test-{}-{}",
1085            std::process::id(),
1086            bind_addr.port()
1087        ));
1088        fs::create_dir_all(&param_dir).unwrap();
1089        let previous_bind = std::env::var_os("VELOXITY_MAVLINK_BIND");
1090        let previous_remote = std::env::var_os("VELOXITY_MAVLINK_REMOTE");
1091        let previous_param_dir = std::env::var_os(PARAM_DIR_ENV);
1092
1093        unsafe {
1094            std::env::set_var("VELOXITY_MAVLINK_BIND", bind_addr.to_string());
1095            std::env::set_var(
1096                "VELOXITY_MAVLINK_REMOTE",
1097                remote.local_addr().unwrap().to_string(),
1098            );
1099            std::env::set_var(PARAM_DIR_ENV, &param_dir);
1100        }
1101
1102        let handle = veloxity_sim_create();
1103        assert!(!handle.is_null());
1104        let snapshot = imu_snapshot(unsafe { veloxity_sim_clock_micros(handle) }.max(1));
1105        assert!(unsafe { veloxity_sim_set_sensors(handle, &snapshot) });
1106        assert!(unsafe { veloxity_sim_sync_latest_imu(handle) });
1107        let mut pwm = [0_u16; NUM_PWM_CHANNELS];
1108        assert_eq!(
1109            unsafe { veloxity_sim_get_pwm(handle, pwm.as_mut_ptr(), pwm.len()) },
1110            NUM_PWM_CHANNELS
1111        );
1112        unsafe { veloxity_sim_destroy(handle) };
1113
1114        restore_env("VELOXITY_MAVLINK_BIND", previous_bind);
1115        restore_env("VELOXITY_MAVLINK_REMOTE", previous_remote);
1116        restore_env(PARAM_DIR_ENV, previous_param_dir);
1117        fs::remove_dir_all(param_dir).unwrap();
1118    }
1119
1120    fn restore_env(name: &str, value: Option<std::ffi::OsString>) {
1121        unsafe {
1122            if let Some(value) = value {
1123                std::env::set_var(name, value);
1124            } else {
1125                std::env::remove_var(name);
1126            }
1127        }
1128    }
1129}
1130
1131fn read_params_from_path(path: &Path, params: &mut Params) -> io::Result<()> {
1132    let contents = fs::read_to_string(path)?;
1133
1134    for line in contents.lines() {
1135        let Some((name, value)) = line.split_once('=') else {
1136            continue;
1137        };
1138        let Some(definition) = PARAM_DEFINITIONS
1139            .iter()
1140            .find(|definition| definition.name == name)
1141        else {
1142            continue;
1143        };
1144        let Some(parsed) = parse_param_value(value, definition.default) else {
1145            continue;
1146        };
1147        params.set_by_id(definition.id, parsed);
1148    }
1149
1150    Ok(())
1151}
1152
1153fn format_param_value(value: ParamValue) -> String {
1154    match value {
1155        ParamValue::Float(value) => value.to_string(),
1156        ParamValue::Int(value) => value.to_string(),
1157        ParamValue::Uint(value) => value.to_string(),
1158        ParamValue::Bool(value) => value.to_string(),
1159    }
1160}
1161
1162fn parse_param_value(value: &str, default: ParamValue) -> Option<ParamValue> {
1163    match default {
1164        ParamValue::Float(_) => value.parse().ok().map(ParamValue::Float),
1165        ParamValue::Int(_) => value.parse().ok().map(ParamValue::Int),
1166        ParamValue::Uint(_) => value.parse().ok().map(ParamValue::Uint),
1167        ParamValue::Bool(_) => value.parse().ok().map(ParamValue::Bool),
1168    }
1169}
sim/ffi.rs

sim/
ffi.rs
