diff --git a/lf-drone/.gitignore b/lf-drone/.gitignore
new file mode 100644
index 0000000..c2e87e3
--- /dev/null
+++ b/lf-drone/.gitignore
@@ -0,0 +1,8 @@
+# Lingua Franca generated/build directories
+bin/
+include/
+src-gen/
+.venv/
+**/bin/
+**/include/
+**/src-gen/
diff --git a/lf-drone/README.md b/lf-drone/README.md
new file mode 100644
index 0000000..d186d88
--- /dev/null
+++ b/lf-drone/README.md
@@ -0,0 +1,29 @@
+# Overview
+This repository contains the working implementation of a Lingua Franca (LF) drone project with two main execution modes:
+
+1. **Drone mode** for running with live ToF sensors on hardware: [Drone](drone/README.md)
+2. **Simulation mode** for replaying ToF sensor CSV files and generating RC outputs and plots: [Simulation](simulation/README.md)
+
+The repository is organized into:
+- `lib/` for shared LF reactors and Python utilities
+- `drone/` for the real drone implementation
+- `simulation/` for the CSV-based simulation and plotting pipeline
+
+# Prerequisites
+You need to download this repository and have the following installed:
+
+Python 3
+pip
+Lingua Franca compiler
+
+## Library Dependencies
+The following dependencies are required to run this project.
+  
+```
+pip install numpy matplotlib pandas vl53l1x
+
+```
+### Notes on Libraries
+1. ```numpy```, ```matplotlib```, and ```pandas``` are used for simulation result processing and plotting.
+2. ```vl53l1x``` is required for live ToF sensor access on the drone.
+3. If you are only running the simulation, you may not need vl53l1x.
\ No newline at end of file
diff --git a/lf-drone/drone/README.md b/lf-drone/drone/README.md
new file mode 100644
index 0000000..391a4e4
--- /dev/null
+++ b/lf-drone/drone/README.md
@@ -0,0 +1,64 @@
+# Lingua Franca Drone Demo
+Drone is purchased from [Duckiedrone DD24](https://get.duckietown.com/products/autonomous-raspberrypi-quadcopter-duckiedrone-dd24), built using the official [Duckietown DD24 assembly and configuration guide](https://docs.duckietown.com/daffy/opmanual-dd24/intro.html).
+
+## Main Files
+
+- `src/test.lf`: main hardware demo
+- `src/DroneBridgeC.lf`: standalone serial RC bridge experiment
+- `../lib/ToFBridgeC.lf`: live ToF sensor bridge
+- `../lib/avoid_planner_modal.lf`: modal avoidance and landing logic
+- `../lib/msp_sender.lf`: MSP RC sender and logger
+- `../lib/UserLandCmd.lf`: keyboard-triggered landing command
+
+# Prerequisites
+
+Fly the drone using the instructions from [Duckietown DD24 assembly and configuration guide](https://docs.duckietown.com/daffy/opmanual-dd24/intro.html)
+
+See the repository root [README.md](../README.md) for the shared software setup and Python dependencies.
+
+In addition, this workflow expects:
+
+- A configured drone platform or compatible test setup
+- Connected ToF sensors
+- Access to the correct serial device, for example `/dev/ttyACM0`
+
+### To Run the Code
+
+```bash
+lfc ./drone/src/test.lf
+./drone/bin/test
+```
+
+### Additional Instructions
+### Additional Instructions
+
+- Before running `lfc`, update the hard-coded absolute import paths in `drone/src/test.lf` so they point to the local files in this repository's `lib/` directory.
+- For example, change:
+
+```
+import PyToF from "/mnt/e/PhD/lf-demos/lf-drone/lib/ToFBridgeC.lf"
+```
+
+to:
+
+```
+import PyToF from "../../lib/ToFBridgeC.lf"
+```
+
+- The full import block in `drone/src/test.lf` should look like this:
+
+```lf
+import PyToF from "../../lib/ToFBridgeC.lf"
+import AvoidPlanner from "../../lib/avoid_planner_modal.lf"
+import MSPSender from "../../lib/msp_sender.lf"
+import UserLandCmd from "../../lib/UserLandCmd.lf"
+```
+
+- Verify the bus numbers and I2C addresses in `src/test.lf` for the `bottom`, `front`, `right`, `left`, and `top` sensors.
+- Verify the `MSPSender(port="/dev/ttyACM0")` setting for your flight controller.
+- Run the executable from the repository root because `ToFBridgeC.lf` launches `python3 ./lib/tof_reader.py`.
+- Press `l` while the program is running to request landing through `UserLandCmd`.
+
+### Notes
+
+`src/DroneBridgeC.lf` is useful as a smaller serial-control experiment, but the main end-to-end hardware demo in this directory is `src/test.lf`.
\ No newline at end of file
diff --git a/lf-drone/drone/src/DroneBridgeC.lf b/lf-drone/drone/src/DroneBridgeC.lf
new file mode 100644
index 0000000..0796823
--- /dev/null
+++ b/lf-drone/drone/src/DroneBridgeC.lf
@@ -0,0 +1,165 @@
+target C { timeout: 10 s }
+
+preamble {=
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <termios.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <time.h>
+
+#define MSP_SET_RAW_RC 200
+
+static uint8_t lf_cksum(uint8_t size, uint8_t cmd, const uint8_t* data) {
+    uint8_t cs = size ^ cmd;
+    for (int i = 0; i < size; i++) cs ^= data[i];
+    return cs;
+}
+
+static int lf_open_serial(const char* dev) {
+    int fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
+    if (fd < 0) return -1;
+    struct termios tio;
+    memset(&tio, 0, sizeof(tio));
+    cfmakeraw(&tio);
+    cfsetspeed(&tio, B115200);
+    tio.c_cflag |= CLOCAL | CREAD;
+    tio.c_cc[VMIN]  = 0;
+    tio.c_cc[VTIME] = 1; // 0.1 s
+    if (tcsetattr(fd, TCSANOW, &tio) != 0) { close(fd); return -1; }
+    return fd;
+}
+
+static int lf_send_rc(int fd, const uint16_t ch[16]) {
+    uint8_t payload[32];
+    for (int i = 0; i < 16; i++) {
+        uint16_t v = ch[i];
+        if (v < 1000) v = 1000;
+        if (v > 2000) v = 2000;
+        payload[2*i+0] = (uint8_t)(v & 0xFF);
+        payload[2*i+1] = (uint8_t)((v >> 8) & 0xFF);
+    }
+    uint8_t size = sizeof(payload);
+    uint8_t header[3] = {'$', 'M', '<'};
+    uint8_t cmd = MSP_SET_RAW_RC;
+    uint8_t cs = lf_cksum(size, cmd, payload);
+    if (write(fd, header, 3) != 3) return -1;
+    if (write(fd, &size, 1) != 1)  return -1;
+    if (write(fd, &cmd, 1) != 1)   return -1;
+    if (write(fd, payload, size) != size) return -1;
+    if (write(fd, &cs, 1) != 1)    return -1;
+    return 0;
+}
+=}
+
+main reactor DroneBridgeC {
+  timer tick(0, 10 ms)
+
+  state fd:int
+  state stage:int
+  state count:int
+
+  // RC channels
+  state rc0:int
+  state rc1:int
+  state rc2:int
+  state rc3:int
+  state rc4:int
+  state rc5:int
+  state rc6:int
+  state rc7:int
+  state rc8:int
+  state rc9:int
+  state rc10:int
+  state rc11:int
+  state rc12:int
+  state rc13:int
+  state rc14:int
+  state rc15:int
+
+  // throttle ramp parameters
+  state thr_start:int = 1200
+  state thr_end:int   = 1600   
+  state thr_step:int  = 5      // +5 per tick = ~50 per second at 100 Hz
+  state thr_hold_ticks:int = 200  // hold final throttle ~2 s
+
+  state pitch_cmd:int = 1560     // forward. try 1550..1600
+  state roll_trim:int  = 1500    // right is >1500 to cancel west drift. try 1510
+
+  reaction(startup) {=
+    const char* port = "/dev/ttyACM0";
+    self->fd = lf_open_serial(port);
+    if (self->fd < 0) {
+        printf("Failed to open %s\n", port);
+        return;
+    }
+    self->rc0 = 1500; self->rc1 = 1500; self->rc2 = 1500; self->rc3 = 1000;
+    self->rc4 = 1000; self->rc5 = 1000; self->rc6 = 1000; self->rc7 = 1000;
+    self->rc8 = 1000; self->rc9 = 1000; self->rc10 = 1000; self->rc11 = 1000;
+    self->rc12 = 1000; self->rc13 = 1000; self->rc14 = 1000; self->rc15 = 1000;
+
+    self->stage = 0;
+    self->count = 0;
+    printf("Opened %s\n", port);
+  =}
+
+  reaction(tick) {=
+    if (self->fd < 0) return;
+
+    uint16_t rc[16] = {
+      (uint16_t)self->rc0,(uint16_t)self->rc1,(uint16_t)self->rc2,(uint16_t)self->rc3,
+      (uint16_t)self->rc4,(uint16_t)self->rc5,(uint16_t)self->rc6,(uint16_t)self->rc7,
+      (uint16_t)self->rc8,(uint16_t)self->rc9,(uint16_t)self->rc10,(uint16_t)self->rc11,
+      (uint16_t)self->rc12,(uint16_t)self->rc13,(uint16_t)self->rc14,(uint16_t)self->rc15
+    };
+
+    switch (self->stage) {
+    case 0: // neutrals ~1.5 s
+      lf_send_rc(self->fd, rc);
+      if (++self->count >= 150) { self->count = 0; self->stage = 1; }
+      break;
+
+    case 1: // arm on AUX1 ~2 s
+      self->rc4 = 1800; rc[4] = (uint16_t)self->rc4;
+      lf_send_rc(self->fd, rc);
+      if (++self->count >= 200) { self->count = 0; self->stage = 2; }
+      break;
+
+    case 2: // throttle ramp up to thr_end
+      if (self->rc3 < self->thr_start) self->rc3 = self->thr_start;
+      else if (self->rc3 < self->thr_end) self->rc3 += self->thr_step;
+      if (self->rc3 > self->thr_end) self->rc3 = self->thr_end;
+      rc[3] = (uint16_t)self->rc3;
+      // neutral attitude while ramping
+      self->rc0 = 1500; self->rc1 = 1500;
+      rc[0] = (uint16_t)self->rc0; rc[1] = (uint16_t)self->rc1;
+      lf_send_rc(self->fd, rc);
+      if (self->rc3 >= self->thr_end) { self->count = 0; self->stage = 3; }
+      break;
+
+    case 3: // translate forward with optional right trim
+      self->rc1 = self->pitch_cmd;     // forward
+      self->rc0 = self->roll_trim;     // small right bias if you drift west
+      rc[1] = (uint16_t)self->rc1;
+      rc[0] = (uint16_t)self->rc0;
+      rc[3] = (uint16_t)self->rc3;     // keep throttle from ramp
+      lf_send_rc(self->fd, rc);
+      if (++self->count >= self->thr_hold_ticks) { self->count = 0; self->stage = 4; }
+      break;
+
+    case 4: // disarm
+      self->rc1 = 1500; self->rc0 = 1500;
+      self->rc3 = 1000; self->rc4 = 1000;
+      rc[1] = (uint16_t)self->rc1; rc[0] = (uint16_t)self->rc0;
+      rc[3] = (uint16_t)self->rc3; rc[4] = (uint16_t)self->rc4;
+      lf_send_rc(self->fd, rc);
+      if (++self->count >= 100) { self->count = 0; self->stage = 5; }
+      break;
+
+    default:
+      lf_send_rc(self->fd, rc);
+      break;
+    }
+  =}
+}
diff --git a/lf-drone/drone/src/avoid_planner.lf b/lf-drone/drone/src/avoid_planner.lf
new file mode 100644
index 0000000..5c397b2
--- /dev/null
+++ b/lf-drone/drone/src/avoid_planner.lf
@@ -0,0 +1,126 @@
+target C
+
+reactor AvoidPlanner {
+  // timer tick(0, 20 ms) // 50 Hz
+
+  input front:double; input left:double; input right:double; input bottom:double; input top:double
+  output roll:int; output pitch:int; output yaw:int; output throttle:int; output aux1:int; output aux2:int
+
+  state height_sp:double = 0.80
+  state thr_base:int = 1450
+  state thr_min:int = 1200
+  state thr_max:int = 1700
+
+  state k_h_p:double = 950.0
+  state k_h_i:double = 180.0
+  state i_h:double = 0.0
+  state i_h_min:double = -250.0
+  state i_h_max:double =  250.0
+
+  state ceil_min_clear:double = 0.35
+  state ceil_soft_band:double = 0.20
+
+  state front_thresh:double = 0.70
+  state side_target:double  = 0.50
+  state k_side:double = 250.0
+  state k_yaw:double  = 45.0
+  state pitch_fwd:int  = 1538
+  state pitch_slow:int = 1500
+
+  state stage:int = 0
+  state count:int = 0
+  state t_neutral:int = 75    // 1.5 s
+  state t_arm:int     = 100   // 2.0 s
+  state t_bump:int    = 75    // 1.5 s
+  state hover_ok:int  = 0
+
+  state initialized:bool = false
+
+  // state f:double=-1.0; state l:double=-1.0; state r:double=-1.0; state b:double=-1.0; state t:double=-1.0
+  // reaction(front){= self->f = front->value; =}
+  // reaction(left){=  self->l = left->value;  =}
+  // reaction(right){= self->r = right->value; =}
+  // reaction(bottom){=self->b = bottom->value;=}
+  // reaction(top){=   self->t = top->value;   =}
+
+  reaction(startup, front, left, right, bottom, top) -> roll, pitch, yaw, throttle, aux1, aux2 {=
+    if (!self->initialized){
+      self->initialized = true;
+      lf_set(roll,1500); lf_set(pitch,1500); lf_set(yaw,1500);
+      lf_set(throttle,1000); lf_set(aux1,1000);
+      lf_set(aux2,1800); // ANGLE ON
+      self->stage=0; self->count=0; self->i_h=0.0; self->hover_ok=0;
+      return;
+    }
+
+    int rc_roll=1500, rc_pitch=1500, rc_yaw=1500, rc_thr=1000;
+    int rc_aux1=1000, rc_aux2=1800;
+
+    // TODO(Pawan): Make this modal model.
+    // TODO(Pawan): Try using values from input ports directly instead of using state variables.
+    switch(self->stage){
+      case 0: rc_thr=1000; rc_aux1=1000; if(++self->count>=self->t_neutral){self->count=0; self->stage=1;} break;
+      case 1: rc_thr=1000; rc_aux1=1800; if(++self->count>=self->t_arm){self->count=0; self->stage=2;} break;
+      case 2: rc_thr=1200; rc_aux1=1800; if(++self->count>=self->t_bump){self->count=0; self->stage=3;} break;
+      case 3: {
+        rc_aux1=1800;
+        static int ramp=1300;
+        if (ramp < self->thr_base) ramp += 5;
+        rc_thr = ramp;
+        if (self->b>0 && self->b>0.35){ self->stage=4; self->count=0; }
+      } break;
+      default: {
+        rc_aux1=1800;
+        int thr_cmd = self->thr_base;
+        if (self->b>0){
+          double eh = self->height_sp - self->b;
+          double ui = self->i_h + eh*0.02*self->k_h_i;
+          if(ui<self->i_h_min) ui=self->i_h_min; if(ui>self->i_h_max) ui=self->i_h_max;
+          self->i_h = ui;
+          thr_cmd = self->thr_base + (int)(self->k_h_p*eh + ui);
+        }
+        if (self->t>0){
+          if (self->t < self->ceil_min_clear){
+            if (thr_cmd > 1250) thr_cmd = 1250;
+            rc_pitch = 1500; rc_yaw = 1500;
+          } else if (self->t < self->ceil_min_clear + self->ceil_soft_band){
+            double frac = (self->t - self->ceil_min_clear) / self->ceil_soft_band;
+            if (frac<0) frac=0; if (frac>1) frac=1;
+            int clamp = 1350 + (int)(250.0*frac);
+            if (thr_cmd > clamp) thr_cmd = clamp;
+          }
+        }
+        if (thr_cmd < self->thr_min) thr_cmd = self->thr_min;
+        if (thr_cmd > self->thr_max) thr_cmd = self->thr_max;
+        rc_thr = thr_cmd;
+
+        if (!self->hover_ok){
+          if (self->b>0 && self->b>0.6 && self->b<1.2){
+            if (++self->count >= 50) self->hover_ok = 1;
+          } else self->count = 0;
+        }
+
+        if (self->hover_ok){
+          rc_pitch = self->pitch_fwd;
+          if (self->f>0 && self->f < self->front_thresh){
+            rc_pitch = self->pitch_slow;
+            double e = self->front_thresh - self->f;
+            int yaw_cmd = 1500 + (int)(self->k_yaw * e);
+            if (yaw_cmd > 1700) yaw_cmd = 1700;
+            rc_yaw = yaw_cmd;
+          }
+          double roll_bias = 0.0;
+          if (self->l>0) roll_bias += (self->side_target - self->l);
+          if (self->r>0) roll_bias -= (self->side_target - self->r);
+          int roll_cmd = 1500 + (int)(self->k_side * roll_bias);
+          if (roll_cmd < 1400) roll_cmd = 1400;
+          if (roll_cmd > 1600) roll_cmd = 1600;
+          rc_roll = roll_cmd;
+        }
+      } break;
+    }
+
+    lf_set(roll,rc_roll); lf_set(pitch,rc_pitch); lf_set(yaw,rc_yaw);
+    lf_set(throttle,rc_thr); lf_set(aux1,rc_aux1); lf_set(aux2,rc_aux2);
+  =}
+}
\ No newline at end of file
diff --git a/lf-drone/drone/src/avoid_planner_2.lf b/lf-drone/drone/src/avoid_planner_2.lf
new file mode 100644
index 0000000..223fe35
--- /dev/null
+++ b/lf-drone/drone/src/avoid_planner_2.lf
@@ -0,0 +1,173 @@
+target C
+
+preamble {=
+  #include <math.h>
+  static inline int clampi(int v, int lo, int hi){ if(v<lo) return lo; if(v>hi) return hi; return v; }
+  static inline int isnan_d(double x){ return x != x; }
+=}
+
+reactor AvoidPlanner {
+  // ToF inputs in meters
+  input front:  double
+  input left:   double
+  input right:  double
+  input bottom: double
+  input top:    double
+
+  // RC outputs (Betaflight MSP microseconds)
+  output roll: int
+  output pitch: int
+  output yaw: int
+  output throttle: int
+  output aux1: int
+  output aux2: int
+
+  // 50 Hz control loop
+  timer tick(0, 20 ms)
+
+  // --- “parameters” (read-only state so C target parses it reliably) ---
+  state height_sp:      double = 0.80
+  state thr_base:       int    = 1450
+  state thr_min:        int    = 1200
+  state thr_max:        int    = 1700
+  state k_h_p:          double = 950.0
+  state k_h_i:          double = 180.0
+  state i_h_min:        double = -250.0
+  state i_h_max:        double =  250.0
+  state ceil_min_clear: double = 0.35
+  state ceil_soft_band: double = 0.20
+  state front_thresh:   double = 0.70
+  state side_target:    double = 0.50
+  state k_side:         double = 250.0
+  state k_yaw:          double = 45.0
+  state pitch_fwd:      int    = 1538
+  state pitch_slow:     int    = 1500
+
+  // --- modal-style phase machine (no 'mode' keyword) ---
+  state phase:int = 0           // 0=NEUTRAL, 1=ARM, 2=RAMP, 3=HOVER
+  state cnt:int   = 0
+  state T_NEU:int = 75          // 1.5 s at 50 Hz
+  state T_ARM:int = 100         // 2.0 s
+  state T_BUMP:int= 75          // 1.5 s
+
+  // controller memory
+  state i_h:double = 0.0
+  state hover_ok:int = 0
+  state ramp:int = 1300
+
+  reaction(startup) -> roll, pitch, yaw, throttle, aux1, aux2 {=
+    lf_set(roll, 1500);
+    lf_set(pitch,1500);
+    lf_set(yaw,  1500);
+    lf_set(throttle,1000);
+    lf_set(aux1, 1000);    // disarmed
+    lf_set(aux2, 1800);    // ANGLE mode on AUX2
+    self->phase = 0;
+    self->cnt = 0;
+    self->i_h = 0.0;
+    self->hover_ok = 0;
+    self->ramp = 1300;
+  =}
+
+  // --- NEUTRAL phase ---
+  reaction(tick) -> roll, pitch, yaw, throttle, aux1, aux2 {=
+    if (self->phase != 0) return;
+    int rc_roll=1500, rc_pitch=1500, rc_yaw=1500, rc_thr=1000, rc_aux1=1000, rc_aux2=1800;
+
+    // stay disarmed a bit so FC settles
+    if (++self->cnt >= self->T_NEU) { self->cnt = 0; self->phase = 1; }
+
+    lf_set(roll,rc_roll); lf_set(pitch,rc_pitch); lf_set(yaw,rc_yaw);
+    lf_set(throttle,rc_thr); lf_set(aux1,rc_aux1); lf_set(aux2,rc_aux2);
+  =}
+
+  // --- ARM phase ---
+  reaction(tick) -> roll, pitch, yaw, throttle, aux1, aux2 {=
+    if (self->phase != 1) return;
+    int rc_roll=1500, rc_pitch=1500, rc_yaw=1500, rc_thr=1000, rc_aux1=1800, rc_aux2=1800;
+
+    if (++self->cnt >= self->T_ARM) { self->cnt = 0; self->phase = 2; }
+
+    lf_set(roll,rc_roll); lf_set(pitch,rc_pitch); lf_set(yaw,rc_yaw);
+    lf_set(throttle,rc_thr); lf_set(aux1,rc_aux1); lf_set(aux2,rc_aux2);
+  =}
+
+  // --- TAKEOFF phase ---
+  reaction(tick, bottom) -> roll, pitch, yaw, throttle, aux1, aux2 {=
+    if (self->phase != 2) return;
+    int rc_roll=1500, rc_pitch=1500, rc_yaw=1500, rc_thr=self->ramp, rc_aux1=1800, rc_aux2=1800;
+
+    if (self->ramp < self->thr_base) self->ramp += 5;
+    rc_thr = self->ramp;
+
+    double b = bottom->is_present ? bottom->value : (0.0/0.0);
+    if (!isnan_d(b) && b > 0.35) { self->phase = 3; self->cnt = 0; } // sensed lift
+
+    lf_set(roll,rc_roll); lf_set(pitch,rc_pitch); lf_set(yaw,rc_yaw);
+    lf_set(throttle,rc_thr); lf_set(aux1,rc_aux1); lf_set(aux2,rc_aux2);
+  =}
+
+  // --- HOVER/CRUISE phase ---
+  reaction(tick, front, left, right, bottom, top) -> roll, pitch, yaw, throttle, aux1, aux2 {=
+    if (self->phase != 3) return;
+
+    double f = front->is_present  ? front->value  : (0.0/0.0);
+    double l = left->is_present   ? left->value   : (0.0/0.0);
+    double r = right->is_present  ? right->value  : (0.0/0.0);
+    double b = bottom->is_present ? bottom->value : (0.0/0.0);
+    double t = top->is_present    ? top->value    : (0.0/0.0);
+
+    int rc_roll=1500, rc_pitch=1500, rc_yaw=1500, rc_thr=self->thr_base, rc_aux1=1800, rc_aux2=1800;
+
+    // altitude PI about bottom ToF
+    if (!isnan_d(b)) {
+      double eh = self->height_sp - b;
+      self->i_h += eh * 0.02 * self->k_h_i;        // 20 ms
+      if (self->i_h < self->i_h_min) self->i_h = self->i_h_min;
+      if (self->i_h > self->i_h_max) self->i_h = self->i_h_max;
+      rc_thr = self->thr_base + (int)(self->k_h_p * eh + self->i_h);
+    }
+
+    // ceiling clamp using top ToF
+    if (!isnan_d(t)) {
+      if (t < self->ceil_min_clear) {
+        if (rc_thr > 1250) rc_thr = 1250;
+        rc_pitch = 1500; rc_yaw = 1500;
+      } else if (t < self->ceil_min_clear + self->ceil_soft_band) {
+        double frac = (t - self->ceil_min_clear) / self->ceil_soft_band;
+        if (frac < 0.0) frac = 0.0; if (frac > 1.0) frac = 1.0;
+        int clamp = 1350 + (int)(250.0 * frac);
+        if (rc_thr > clamp) rc_thr = clamp;
+      }
+    }
+    rc_thr = clampi(rc_thr, self->thr_min, self->thr_max);
+
+    // confirm stable hover window before forward motion
+    if (!self->hover_ok) {
+      if (!isnan_d(b) && b > 0.6 && b < 1.2) {
+        if (++self->cnt >= 50) self->hover_ok = 1;
+      } else self->cnt = 0;
+    }
+
+    if (self->hover_ok) {
+      // forward pitch; slow down if obstacle in front
+      rc_pitch = self->pitch_fwd;
+      if (!isnan_d(f) && f < self->front_thresh) {
+        rc_pitch = self->pitch_slow;
+        double e = self->front_thresh - f;
+        int yaw_cmd = 1500 + (int)(self->k_yaw * e);
+        rc_yaw = clampi(yaw_cmd, 1400, 1700);
+      }
+
+      // center between side walls
+      double roll_bias = 0.0;
+      if (!isnan_d(l)) roll_bias += (self->side_target - l);
+      if (!isnan_d(r)) roll_bias -= (self->side_target - r);
+      int roll_cmd = 1500 + (int)(self->k_side * roll_bias);
+      rc_roll = clampi(roll_cmd, 1400, 1600);
+    }
+
+    lf_set(roll,rc_roll); lf_set(pitch,rc_pitch); lf_set(yaw,rc_yaw);
+    lf_set(throttle,rc_thr); lf_set(aux1,rc_aux1); lf_set(aux2,rc_aux2);
+  =}
+}
diff --git a/lf-drone/drone/src/avoid_planner_modal.lf b/lf-drone/drone/src/avoid_planner_modal.lf
new file mode 100644
index 0000000..e159114
--- /dev/null
+++ b/lf-drone/drone/src/avoid_planner_modal.lf
@@ -0,0 +1,283 @@
+target C
+
+preamble {=
+  #include <math.h>
+  #include <stdio.h>
+=}
+
+reactor AvoidPlanner {
+  // ---------------- Ports ----------------
+  input front: double
+  input left: double
+  input right: double
+  input bottom: double
+  input top: double
+  // Optional command to initiate landing (present & nonzero -> land)
+  input land: int
+
+  output roll: int
+  output pitch: int
+  output yaw: int
+  output throttle: int
+  output aux1: int
+  output aux2: int
+
+  // Single global timer (used by all modes)
+  timer tick(0, 20 ms)
+
+  // ---------------- Tunables ----------------
+  state height_sp: double = 0.80
+  state thr_base:  int = 1450
+  state thr_min:   int = 1200
+  state thr_max:   int = 1700
+
+  state k_h_p: double = 950.0
+  state k_h_i: double = 180.0
+  state i_h_min: double = -250.0
+  state i_h_max: double =  250.0
+
+  state ceil_min_clear: double = 0.35
+  state ceil_soft_band: double = 0.20
+
+  state front_thresh: double = 0.70
+  state side_target:  double = 0.50
+  state k_side: double = 250.0
+  state k_yaw:  double = 45.0
+  state pitch_fwd: int = 1538
+  state pitch_slow: int = 1500
+
+  // ---------------- Per-mode scratch (reactor-level; reset on entry) ----------------
+  state ticks:int = 0
+  state takeoff:int  = 1300
+  state i_h:double = 0.0
+  state hover_ok:int = 0
+  state ok_count:int = 0
+
+  // Landing scratch
+  state land_thr:int = 1300
+  state land_touch:int = 0      // consecutive ticks on ground
+  state land_min_thr:int = 1080 // don't drop below this while descending
+  state land_step:int = 6       // throttle decrement per tick (≈300 us/s)
+
+  // Stage durations (ticks @ 50 Hz)
+  state t_neutral:int = 75
+  state t_arm:int     = 100
+  state t_takeoff:int = 75
+
+  // ---------------- Modes ----------------
+
+  initial mode Neutral {
+    reaction(reset) {=
+      self->ticks = 0;
+      self->takeoff = 1300;
+      self->i_h = 0.0;
+      self->hover_ok = 0;
+      self->ok_count = 0;
+      self->land_thr = 1300;
+      self->land_touch = 0;
+      printf("[AvoidPlanner] Neutral\n");
+    =}
+
+    reaction(tick) -> roll, pitch, yaw, throttle, aux1, aux2, reset(Arm) {=
+      lf_set(roll, 1500);
+      lf_set(pitch, 1500);
+      lf_set(yaw, 1500);
+      lf_set(throttle, 1000);
+      lf_set(aux1, 1000);   // disarmed
+      lf_set(aux2, 1800);   // ANGLE
+      if (++self->ticks >= self->t_neutral) {
+        lf_set_mode(Arm);
+      }
+    =}
+  }
+
+  mode Arm {
+    reaction(reset) {=
+      self->ticks = 0;
+      printf("[AvoidPlanner] Arm\n");
+    =}
+
+    reaction(tick) -> roll, pitch, yaw, throttle, aux1, aux2, reset(takeoff) {=
+      lf_set(roll, 1500);
+      lf_set(pitch, 1500);
+      lf_set(yaw, 1500);
+      lf_set(throttle, 1000);
+      lf_set(aux1, 1800);   // arm
+      lf_set(aux2, 1800);
+      if (++self->ticks >= self->t_arm) {
+        lf_set_mode(takeoff);
+        printf("[AvoidPlanner] Arm -> takeoff\n");
+      }
+    =}
+  }
+
+  mode takeoff {
+    reaction(reset) {=
+      self->ticks = 0;
+      self->takeoff  = 1300;
+      printf("[AvoidPlanner] takeoff\n");
+    =}
+
+    reaction(tick) -> roll, pitch, yaw, throttle, aux1, aux2, reset(Cruise) {=
+      if (self->takeoff < self->thr_base) self->takeoff += 5;
+      int rc_thr = self->takeoff;
+      if (rc_thr < self->thr_min) rc_thr = self->thr_min;
+      if (rc_thr > self->thr_max) rc_thr = self->thr_max;
+
+      lf_set(roll, 1500);
+      lf_set(pitch, 1500);
+      lf_set(yaw, 1500);
+      lf_set(throttle, rc_thr);
+      lf_set(aux1, 1800);
+      lf_set(aux2, 1800);
+
+      if (++self->ticks >= self->t_takeoff) {
+        lf_set_mode(Cruise);
+        printf("[AvoidPlanner] takeoff -> Cruise\n");
+      }
+    =}
+  }
+
+  mode Cruise {
+    reaction(reset) {=
+      self->i_h = 0.0;
+      self->hover_ok = 0;
+      self->ok_count = 0;
+      printf("[AvoidPlanner] Cruise\n");
+    =}
+
+    reaction(tick)
+             -> roll, pitch, yaw, throttle, aux1, aux2, reset(Landing) {=
+      // Landing request?
+      int land_req = (land->is_present && land->value != 0) ? 1 : 0;
+
+      double f = front->is_present  ? front->value  : NAN;
+      double l = left->is_present   ? left->value   : NAN;
+      double r = right->is_present  ? right->value  : NAN;
+      double b = bottom->is_present ? bottom->value : NAN;
+      double t = top->is_present    ? top->value    : NAN;
+
+      int rc_roll  = 1500;
+      int rc_pitch = 1500;
+      int rc_yaw   = 1500;
+      int rc_thr   = self->thr_base;
+      int rc_aux1  = 1800;
+      int rc_aux2  = 1800;
+
+      // Altitude PI
+      if (!isnan(b)) {
+        double eh = self->height_sp - b;
+        self->i_h += eh * 0.02 * self->k_h_i; // 20 ms tick
+        if (self->i_h < self->i_h_min) self->i_h = self->i_h_min;
+        if (self->i_h > self->i_h_max) self->i_h = self->i_h_max;
+        rc_thr = self->thr_base + (int)(self->k_h_p * eh + self->i_h);
+      }
+
+      // Ceiling clamp
+      if (!isnan(t)) {
+        if (t < self->ceil_min_clear) {
+          if (rc_thr > 1250) rc_thr = 1250;
+          rc_pitch = 1500; rc_yaw = 1500;
+        } else if (t < self->ceil_min_clear + self->ceil_soft_band) {
+          double frac = (t - self->ceil_min_clear) / self->ceil_soft_band;
+          if (frac < 0.0) frac = 0.0;
+          if (frac > 1.0) frac = 1.0;
+          int clamp = 1350 + (int)(250.0 * frac);
+          if (rc_thr > clamp) rc_thr = clamp;
+        }
+      }
+
+      if (rc_thr < self->thr_min) rc_thr = self->thr_min;
+      if (rc_thr > self->thr_max) rc_thr = self->thr_max;
+
+      // Confirm stable hover band once
+      if (!self->hover_ok) {
+        if (!isnan(b) && b > 0.6 && b < 1.2) {
+          if (++self->ok_count >= 50) self->hover_ok = 1;
+        } else {
+          self->ok_count = 0;
+        }
+      }
+
+      // Forward + avoidance
+      if (self->hover_ok) {
+        rc_pitch = self->pitch_fwd;
+
+        if (!isnan(f) && f < self->front_thresh) {
+          rc_pitch = self->pitch_slow;
+          double e = self->front_thresh - f;
+          int yaw_cmd = 1500 + (int)(self->k_yaw * e);
+          if (yaw_cmd < 1400) yaw_cmd = 1400;
+          if (yaw_cmd > 1700) yaw_cmd = 1700;
+          rc_yaw = yaw_cmd;
+        }
+
+        double roll_bias = 0.0;
+        if (!isnan(l)) roll_bias += (self->side_target - l);
+        if (!isnan(r)) roll_bias -= (self->side_target - r);
+        int roll_cmd = 1500 + (int)(self->k_side * roll_bias);
+        if (roll_cmd < 1400) roll_cmd = 1400;
+        if (roll_cmd > 1600) roll_cmd = 1600;
+        rc_roll = roll_cmd;
+      }
+
+      // Apply outputs
+      lf_set(roll, rc_roll);
+      lf_set(pitch, rc_pitch);
+      lf_set(yaw, rc_yaw);
+      lf_set(throttle, rc_thr);
+      lf_set(aux1, rc_aux1);
+      lf_set(aux2, rc_aux2);
+
+      // Transition to Landing if requested
+      if (land_req) {
+        lf_set_mode(Landing);
+        printf("[AvoidPlanner] Cruise -> Landing\n");
+      }
+    =}
+  }
+
+  mode Landing {
+    reaction(reset) {=
+      // Start landing from a safe throttle near hover
+      self->land_thr = (self->thr_base > 1350 ? 1350 : self->thr_base);
+      self->land_touch = 0;
+      printf("[AvoidPlanner] Landing\n");
+    =}
+
+    reaction(tick, bottom)
+             -> roll, pitch, yaw, throttle, aux1, aux2, reset(Neutral) {=
+      double b = bottom->is_present ? bottom->value : NAN;
+
+      // Neutral attitude, keep ANGLE, armed during descent
+      lf_set(roll, 1500);
+      lf_set(pitch, 1500);
+      lf_set(yaw, 1500);
+      lf_set(aux1, 1800);
+      lf_set(aux2, 1800);
+
+      // Ramp down throttle smoothly
+      if (self->land_thr > self->land_min_thr) self->land_thr -= self->land_step;
+
+      // If we see ground close, allow lower throttle to settle
+      if (!isnan(b) && b < 0.20 && self->land_thr > 1100) {
+        self->land_thr = 1100;
+      }
+
+      lf_set(throttle, self->land_thr);
+
+      // Touchdown detection: bottom distance < 8 cm for 0.3 s
+      if (!isnan(b) && b < 0.08) {
+        if (++self->land_touch >= 15) {
+          // Disarm and return to Neutral
+          lf_set(aux1, 1000);
+          lf_set(throttle, 1000);
+          lf_set_mode(Neutral);
+          printf("[AvoidPlanner] Landing -> Neutral (touchdown)\n");
+        }
+      } else {
+        self->land_touch = 0;
+      }
+    =}
+  }
+}
diff --git a/lf-drone/drone/src/test.lf b/lf-drone/drone/src/test.lf
new file mode 100644
index 0000000..2996f73
--- /dev/null
+++ b/lf-drone/drone/src/test.lf
@@ -0,0 +1,39 @@
+target C   // runs until Ctrl+C
+
+import PyToF from "/mnt/e/PhD/lf-demos/lf-drone/lib/ToFBridgeC.lf"
+import AvoidPlanner from "/mnt/e/PhD/lf-demos/lf-drone/lib/avoid_planner_modal.lf"
+import MSPSender from "/mnt/e/PhD/lf-demos/lf-drone/lib/msp_sender.lf"
+import UserLandCmd from "/mnt/e/PhD/lf-demos/lf-drone/lib/UserLandCmd.lf"
+
+main reactor test {
+  // ToF sensors (use the exact buses/addresses that work for you)
+  bottom = new PyToF(bus=22, addr=41, rate=20, timing_ms=100)
+  front  = new PyToF(bus=24, addr=41, rate=20, timing_ms=100)
+  right  = new PyToF(bus=26, addr=41, rate=20, timing_ms=100)
+  left   = new PyToF(bus=23, addr=41, rate=20, timing_ms=100)
+  top    = new PyToF(bus=25, addr=41, rate=20, timing_ms=100)
+
+  avoid  = new AvoidPlanner()
+  msp    = new MSPSender(port="/dev/ttyACM0")
+
+  // Keyboard landing trigger
+  land_cmd = new UserLandCmd()
+
+  // sensors -> planner
+  front.value  -> avoid.front
+  left.value   -> avoid.left
+  right.value  -> avoid.right
+  bottom.value -> avoid.bottom
+  top.value    -> avoid.top
+
+  // landing command
+  land_cmd.out -> avoid.land
+
+  // planner -> MSP
+  avoid.roll     -> msp.roll
+  avoid.pitch    -> msp.pitch
+  avoid.yaw      -> msp.yaw
+  avoid.throttle -> msp.throttle
+  avoid.aux1     -> msp.aux1
+  avoid.aux2     -> msp.aux2
+}
\ No newline at end of file
diff --git a/lf-drone/lib/README.md b/lf-drone/lib/README.md
new file mode 100644
index 0000000..40fe5a4
--- /dev/null
+++ b/lf-drone/lib/README.md
@@ -0,0 +1,30 @@
+# Overview
+
+This directory contains the shared Lingua Franca reactors and helper scripts used by both the hardware and simulation workflows.
+
+## Contents
+
+- `ToFBridgeC.lf`: spawns `lib/tof_reader.py` and publishes live ToF distance readings
+- `UserLandCmd.lf`: emits a landing command when the user presses `l`
+- `avoid_planner_modal.lf`: modal takeoff, cruise, avoidance, and landing controller
+- `msp_sender.lf`: sends MSP RC commands or logs them to CSV
+- `tof_reader.py`: Python interface to the VL53L1X sensor
+- `tof_logger.py`: helper script to log one or more ToF streams into CSV files
+
+# Prerequisites
+
+See the repository root [README.md](../README.md) for the required toolchain and Python libraries.
+
+### To Run the Code
+
+These LF files are imported by the programs in:
+
+- `drone/src/test.lf`
+- `simulation/src/test.lf`
+
+### Additional Instructions
+
+- Before running `lfc`, update the old absolute imports in `drone/src/test.lf` and `simulation/src/test.lf` so they point to this local `lib/` directory.
+- `ToFBridgeC.lf` launches `python3 ./lib/tof_reader.py`, so executables that depend on it should be started from the repository root.
+- `msp_sender.lf` supports both serial mode and log-only mode. Setting `port=""` disables serial transmission and keeps only the logging behavior.
+- `tof_logger.py` can be used to record live ToF sensor data into CSV files for later replay in the simulation workflow.
\ No newline at end of file
diff --git a/lf-drone/lib/ToFBridgeC.lf b/lf-drone/lib/ToFBridgeC.lf
new file mode 100644
index 0000000..279912c
--- /dev/null
+++ b/lf-drone/lib/ToFBridgeC.lf
@@ -0,0 +1,174 @@
+target C
+
+preamble {=
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+typedef struct {
+    FILE* pipe;
+    int   bus;
+    int   addr;
+    int   rate;
+    int   timing_ms;
+} tof_proc_t;
+
+static int tof_start(tof_proc_t* tp, int bus, int addr, int rate, int timing_ms) {
+    tp->bus = bus; tp->addr = addr; tp->rate = rate; tp->timing_ms = timing_ms;
+    char cmd[256];
+    // safer: absolute path so we never depend on cwd
+    snprintf(cmd, sizeof(cmd),
+             "python3 ./lib/tof_reader.py --bus %d --addr 0x%02X --rate %d --timing_ms %d 2>/dev/null",
+             bus, addr, rate, timing_ms);
+    tp->pipe = popen(cmd, "r");
+    if (!tp->pipe) return -1;
+    int fd = fileno(tp->pipe);
+    int flags = fcntl(fd, F_GETFL, 0);
+    fcntl(fd, F_SETFL, flags | O_NONBLOCK);
+    return 0;
+}
+
+static int tof_read_latest(tof_proc_t* tp, double* out_m) {
+    if (!tp || !tp->pipe) return -2;
+    char buf[256];
+    size_t n = fread(buf, 1, sizeof(buf) - 1, tp->pipe);
+    if (n == 0) return -1; // nothing yet
+    buf[n] = 0;
+
+    double v = 0.0;
+    int got = 0;
+    char* p = buf;
+    while (*p) {
+        char* end = NULL;
+        double x = strtod(p, &end);
+        if (end != p) { v = x; got = 1; p = end; } else { p++; }
+    }
+    if (got) { *out_m = v; return 0; }
+    return -3;
+}
+
+static void tof_stop(tof_proc_t* tp) {
+    if (tp && tp->pipe) { pclose(tp->pipe); tp->pipe = NULL; }
+}
+=}
+
+reactor PyToF(id:int=0, label:string="sensor", bus:int=22, 
+              addr:int=41, rate:int=20, timing_ms:int=100, 
+              start_ms:time=100 ms, verbose:int=0) {
+  // start after 100 ms so startup finishes before first poll
+  timer poll(start_ms, 20 ms)
+  output value: double
+
+  // initialize pointer container to 0
+  state tp: long = 0
+  state last_val: double
+
+  reaction(startup) {=
+    tof_proc_t* proc = (tof_proc_t*)malloc(sizeof(tof_proc_t));
+    if (!proc) { fprintf(stderr, "ToF malloc failed\n"); return; }
+    if (tof_start(proc, self->bus, self->addr, self->rate, self->timing_ms) != 0) {
+        fprintf(stderr, "ToF start failed\n"); free(proc); return;
+    }
+    self->tp = (long)(intptr_t)proc; // store pointer safely
+    self->last_val = 0.0;
+    if (self->verbose) {
+      printf("ToF[%d:%s] started on /dev/i2c-%d addr 0x%02X rate %dHz timing %dms\n",
+            self->id, self->label, self->bus, self->addr, self->rate, self->timing_ms);
+    }
+  =}
+
+  reaction(poll) -> value {=
+    tof_proc_t* proc = (tof_proc_t*)(uintptr_t)self->tp;
+    if (!proc) return;
+    double val = 0.0;
+    int r = tof_read_latest(proc, &val);
+    if (r == 0) {
+        self->last_val = val;
+        lf_set(value, val);
+        if (self->verbose) {
+          printf("ToF[%d:%s] value: %.3f\n", self->id, self->label, val);
+        }
+    }
+  =}
+
+  reaction(shutdown) {=
+    tof_proc_t* proc = (tof_proc_t*)(uintptr_t)self->tp;
+    if (proc) { tof_stop(proc); free(proc); self->tp = 0; }
+  =}
+}
+
+reactor TablePrinter(period: time=200 ms) {
+  timer beat(0, period)
+
+  input bottom: double
+  input front:  double
+  input right:  double
+  input left:   double
+  input top:    double
+
+  state b_val: double = -1.0
+  state f_val: double = -1.0
+  state r_val: double = -1.0
+  state l_val: double = -1.0
+  state t_val: double = -1.0
+
+  // Each of these reactions is triggered only when the input is present,
+  // so reading port->value is correct.
+  reaction(bottom) {=
+    self->b_val = bottom->value;
+  =}
+  reaction(front)  {=
+    self->f_val = front->value;
+  =}
+  reaction(right)  {=
+    self->r_val = right->value;
+  =}
+  reaction(left)   {=
+    self->l_val = left->value;
+  =}
+  reaction(top)    {=
+    self->t_val = top->value;
+  =}
+
+  reaction(beat) {=
+    char b[16], f[16], r[16], l[16], t[16];
+    if (self->b_val < 0) strcpy(b, "N/A"); else snprintf(b, sizeof(b), "%.3f", self->b_val);
+    if (self->f_val < 0) strcpy(f, "N/A"); else snprintf(f, sizeof(f), "%.3f", self->f_val);
+    if (self->r_val < 0) strcpy(r, "N/A"); else snprintf(r, sizeof(r), "%.3f", self->r_val);
+    if (self->l_val < 0) strcpy(l, "N/A"); else snprintf(l, sizeof(l), "%.3f", self->l_val);
+    if (self->t_val < 0) strcpy(t, "N/A"); else snprintf(t, sizeof(t), "%.3f", self->t_val);
+
+    // clear + home, then draw one table in place
+    printf("\033[2J\033[H");
+    printf("+---------+-----------+\n");
+    printf("| Sensor  | Value (m) |\n");
+    printf("+---------+-----------+\n");
+    printf("| bottom  | %9s |\n", b);
+    printf("| front   | %9s |\n", f);
+    printf("| right   | %9s |\n", r);
+    printf("| left    | %9s |\n", l);
+    printf("| top     | %9s |\n", t);
+    printf("+---------+-----------+\n");
+    fflush(stdout);
+  =}
+} 
+
+main reactor ToFBridgeC {
+  // set bus=1 if your HAT exposes /dev/i2c-1
+  bottom = new PyToF(id=0, label="bottom", bus=22, addr=41, rate=20, timing_ms=100, start_ms=100 ms, verbose=0)
+  front  = new PyToF(id=1, label="front", bus=24, addr=41, rate=20, timing_ms=100, start_ms=120 ms, verbose=0)
+  right  = new PyToF(id=2, label="right", bus=26, addr=41, rate=20, timing_ms=100, start_ms=140 ms, verbose=0)
+  left   = new PyToF(id=3, label="left", bus=23, addr=41, rate=20, timing_ms=100, start_ms=160 ms, verbose=0)
+  top    = new PyToF(id=4, label="top", bus=25, addr=41, rate=20, timing_ms=100, start_ms=180 ms, verbose=0)
+
+  table  = new TablePrinter(period=200 ms)
+
+  bottom.value -> table.bottom
+  front.value  -> table.front
+  right.value  -> table.right
+  left.value   -> table.left
+  top.value    -> table.top
+}
diff --git a/lf-drone/lib/UserLandCmd.lf b/lf-drone/lib/UserLandCmd.lf
new file mode 100644
index 0000000..5c3a047
--- /dev/null
+++ b/lf-drone/lib/UserLandCmd.lf
@@ -0,0 +1,40 @@
+target C
+
+preamble {=
+  #include <termios.h>
+  #include <unistd.h>
+  #include <fcntl.h>
+  #include <stdio.h>
+
+  static struct termios _old_tio;
+  static void kb_setup(void){
+    struct termios tio;
+    tcgetattr(STDIN_FILENO, &_old_tio);
+    tio = _old_tio;
+    cfmakeraw(&tio);
+    tcsetattr(STDIN_FILENO, TCSANOW, &tio);
+    // non-blocking stdin
+    int fl = fcntl(STDIN_FILENO, F_GETFL, 0);
+    fcntl(STDIN_FILENO, F_SETFL, fl | O_NONBLOCK);
+    fprintf(stderr, "[UserLandCmd] Press 'l' or 'L' to land.\n");
+  }
+  static void kb_restore(void){ tcsetattr(STDIN_FILENO, TCSANOW, &_old_tio); }
+=}
+
+reactor UserLandCmd {
+  output out:int
+  timer poll(0, 50 ms)
+
+  reaction(startup) {= kb_setup(); =}
+  reaction(shutdown) {= kb_restore(); =}
+
+  reaction(poll) -> out {=
+    char c;
+    if (read(STDIN_FILENO, &c, 1) == 1) {
+      if (c=='l' || c=='L') {
+        lf_set(out, 1);       // emit once per keypress
+        fprintf(stderr, "[UserLandCmd] Land requested.\n");
+      }
+    }
+  =}
+}
diff --git a/lf-drone/lib/avoid_planner_modal.lf b/lf-drone/lib/avoid_planner_modal.lf
new file mode 100644
index 0000000..6793b18
--- /dev/null
+++ b/lf-drone/lib/avoid_planner_modal.lf
@@ -0,0 +1,319 @@
+target C
+
+preamble {=
+  #include <math.h>
+  #include <stdio.h>
+=}
+
+reactor AvoidPlanner {
+  // ---------------- Ports ----------------
+  input front: double
+  input left: double
+  input right: double
+  input bottom: double
+  input top: double
+  // Optional command to initiate landing (present & nonzero -> land)
+  input land: int
+
+  output roll: int
+  output pitch: int
+  output yaw: int
+  output throttle: int
+  output aux1: int
+  output aux2: int
+
+  // Single global timer (used by all modes)
+  timer tick(0, 20 ms)
+
+  // ---------------- Tunables ----------------
+  state height_sp: double = 0.80
+  state thr_base:  int = 1450
+  state thr_min:   int = 1200
+  state thr_max:   int = 1700
+
+  state k_h_p: double = 950.0
+  state k_h_i: double = 180.0
+  state i_h_min: double = -250.0
+  state i_h_max: double =  250.0
+
+  state ceil_min_clear: double = 0.35
+  state ceil_soft_band: double = 0.20
+
+  state front_thresh: double = 0.35
+  state front_hard: double = 0.20
+
+  state side_target:  double = 0.50
+  state k_side: double = 250.0
+  state k_yaw:  double = 120.0
+
+  state pitch_fwd:  int = 1538
+  state pitch_slow: int = 1506
+  state pitch_back: int = 1460
+  state hover_confirm_ticks:int = 10
+
+  // ---------------- Per-mode scratch (reactor-level; reset on entry) ----------------
+  state ticks:int = 0
+  state takeoff:int  = 1300
+  state i_h:double = 0.0
+  state hover_ok:int = 0
+  state ok_count:int = 0
+
+  // Landing scratch
+  state land_thr:int = 1300
+  state land_touch:int = 0      // consecutive ticks on ground
+  state land_min_thr:int = 1080 // don't drop below this while descending
+  state land_step:int = 6       // throttle decrement per tick (≈300 us/s)
+
+  // Stage durations (ticks @ 50 Hz)
+  state t_neutral:int = 5
+  state t_arm:int     = 10
+  state t_takeoff:int = 15
+
+  // ---------------- Modes ----------------
+
+  initial mode Neutral {
+    reaction(reset) {=
+      self->ticks = 0;
+      self->takeoff = 1300;
+      self->i_h = 0.0;
+      self->hover_ok = 0;
+      self->ok_count = 0;
+      self->land_thr = 1300;
+      self->land_touch = 0;
+      printf("[AvoidPlanner] Neutral\n");
+    =}
+
+    reaction(tick) -> roll, pitch, yaw, throttle, aux1, aux2, reset(Arm) {=
+      lf_set(roll, 1500);
+      lf_set(pitch, 1500);
+      lf_set(yaw, 1500);
+      lf_set(throttle, 1000);
+      lf_set(aux1, 1000);   // disarmed
+      lf_set(aux2, 1800);   // ANGLE
+      if (++self->ticks >= self->t_neutral) {
+        lf_set_mode(Arm);
+      }
+    =}
+  }
+
+  mode Arm {
+    reaction(reset) {=
+      self->ticks = 0;
+      printf("[AvoidPlanner] Arm\n");
+    =}
+
+    reaction(tick) -> roll, pitch, yaw, throttle, aux1, aux2, reset(takeoff) {=
+      lf_set(roll, 1500);
+      lf_set(pitch, 1500);
+      lf_set(yaw, 1500);
+      lf_set(throttle, 1000);
+      lf_set(aux1, 1800);   // arm
+      lf_set(aux2, 1800);
+      if (++self->ticks >= self->t_arm) {
+        lf_set_mode(takeoff);
+        printf("[AvoidPlanner] Arm -> takeoff\n");
+      }
+    =}
+  }
+
+  mode takeoff {
+    reaction(reset) {=
+      self->ticks = 0;
+      self->takeoff  = 1300;
+      printf("[AvoidPlanner] takeoff\n");
+    =}
+
+    reaction(tick) -> roll, pitch, yaw, throttle, aux1, aux2, reset(Cruise) {=
+      if (self->takeoff < self->thr_base) self->takeoff += 5;
+      int rc_thr = self->takeoff;
+      if (rc_thr < self->thr_min) rc_thr = self->thr_min;
+      if (rc_thr > self->thr_max) rc_thr = self->thr_max;
+
+      lf_set(roll, 1500);
+      lf_set(pitch, 1500);
+      lf_set(yaw, 1500);
+      lf_set(throttle, rc_thr);
+      lf_set(aux1, 1800);
+      lf_set(aux2, 1800);
+
+      if (++self->ticks >= self->t_takeoff) {
+        lf_set_mode(Cruise);
+        printf("[AvoidPlanner] takeoff -> Cruise\n");
+      }
+    =}
+  }
+
+  mode Cruise {
+    reaction(reset) {=
+      self->i_h = 0.0;
+      self->hover_ok = 0;
+      self->ok_count = 0;
+      printf("[AvoidPlanner] Cruise\n");
+    =}
+
+    reaction(tick, land, front, left, right, bottom, top)
+             -> roll, pitch, yaw, throttle, aux1, aux2, reset(Landing) {=
+      // Landing request?
+      int land_req = (land->is_present && land->value != 0) ? 1 : 0;
+
+      double f = front->is_present  ? front->value  : NAN;
+      double l = left->is_present   ? left->value   : NAN;
+      double r = right->is_present  ? right->value  : NAN;
+      double b = bottom->is_present ? bottom->value : NAN;
+      double t = top->is_present    ? top->value    : NAN;
+
+      int rc_roll  = 1500;
+      int rc_pitch = 1500;
+      int rc_yaw   = 1500;
+      int rc_thr   = self->thr_base;
+      int rc_aux1  = 1800;
+      int rc_aux2  = 1800;
+
+      // Altitude PI
+      if (!isnan(b)) {
+        double eh = self->height_sp - b;
+        self->i_h += eh * 0.02 * self->k_h_i; // 20 ms tick
+        if (self->i_h < self->i_h_min) self->i_h = self->i_h_min;
+        if (self->i_h > self->i_h_max) self->i_h = self->i_h_max;
+        rc_thr = self->thr_base + (int)(self->k_h_p * eh + self->i_h);
+      }
+
+      // Ceiling clamp
+      if (!isnan(t)) {
+        if (t < self->ceil_min_clear) {
+          if (rc_thr > 1250) rc_thr = 1250;
+          rc_pitch = 1500; rc_yaw = 1500;
+        } else if (t < self->ceil_min_clear + self->ceil_soft_band) {
+          double frac = (t - self->ceil_min_clear) / self->ceil_soft_band;
+          if (frac < 0.0) frac = 0.0;
+          if (frac > 1.0) frac = 1.0;
+          int clamp = 1350 + (int)(250.0 * frac);
+          if (rc_thr > clamp) rc_thr = clamp;
+        }
+      }
+
+      if (rc_thr < self->thr_min) rc_thr = self->thr_min;
+      if (rc_thr > self->thr_max) rc_thr = self->thr_max;
+
+      // Confirm stable hover band once
+      if (!self->hover_ok) {
+        if (!isnan(b) && b > 0.6 && b < 1.2) {
+          if (++self->ok_count >= self->hover_confirm_ticks) {
+            self->hover_ok = 1;
+          }
+        } else {
+          self->ok_count = 0;
+        }
+      }
+
+      // Forward + avoidance
+      if (self->hover_ok) {
+        rc_pitch = self->pitch_fwd;
+
+        // Corridor-centering from side sensors
+        double roll_bias = 0.0;
+        if (!isnan(l)) roll_bias += (self->side_target - l);
+        if (!isnan(r)) roll_bias -= (self->side_target - r);
+
+        // If obstacle is ahead, slow down first.
+        // If it is very close, back away.
+        if (!isnan(f) && f < self->front_thresh) {
+          rc_pitch = self->pitch_slow;
+
+          if (f < self->front_hard) {
+            rc_pitch = self->pitch_back;
+          }
+
+          // Choose the side with more room.
+          // Positive => more room on the right.
+          double side_pref = 0.0;
+          if (!isnan(l) && !isnan(r)) {
+            side_pref = r - l;
+          } else if (!isnan(r)) {
+            side_pref = r - self->side_target;
+          } else if (!isnan(l)) {
+            side_pref = self->side_target - l;
+          }
+
+          // Add sidestep toward the open side
+          roll_bias += 0.8 * side_pref;
+
+          // Yaw toward the open side too
+          double closeness = self->front_thresh - f;
+          int yaw_delta = (int)(self->k_yaw * closeness);
+          if (yaw_delta < 25) yaw_delta = 25;
+          if (yaw_delta > 180) yaw_delta = 180;
+
+          if (side_pref >= 0.0) {
+            rc_yaw = 1500 + yaw_delta;   // turn right
+          } else {
+            rc_yaw = 1500 - yaw_delta;   // turn left
+          }
+        }
+
+        int roll_cmd = 1500 + (int)(self->k_side * roll_bias);
+        if (roll_cmd < 1380) roll_cmd = 1380;
+        if (roll_cmd > 1620) roll_cmd = 1620;
+        rc_roll = roll_cmd;
+      }
+
+      // Apply outputs
+      lf_set(roll, rc_roll);
+      lf_set(pitch, rc_pitch);
+      lf_set(yaw, rc_yaw);
+      lf_set(throttle, rc_thr);
+      lf_set(aux1, rc_aux1);
+      lf_set(aux2, rc_aux2);
+
+      // Transition to Landing if requested
+      if (land_req) {
+        lf_set_mode(Landing);
+        printf("[AvoidPlanner] Cruise -> Landing\n");
+      }
+    =}
+  }
+
+  mode Landing {
+    reaction(reset) {=
+      // Start landing from a safe throttle near hover
+      self->land_thr = (self->thr_base > 1350 ? 1350 : self->thr_base);
+      self->land_touch = 0;
+      printf("[AvoidPlanner] Landing\n");
+    =}
+
+    reaction(tick, bottom)
+             -> roll, pitch, yaw, throttle, aux1, aux2, reset(Neutral) {=
+      double b = bottom->is_present ? bottom->value : NAN;
+
+      // Neutral attitude, keep ANGLE, armed during descent
+      lf_set(roll, 1500);
+      lf_set(pitch, 1500);
+      lf_set(yaw, 1500);
+      lf_set(aux1, 1800);
+      lf_set(aux2, 1800);
+
+      // Ramp down throttle smoothly
+      if (self->land_thr > self->land_min_thr) self->land_thr -= self->land_step;
+
+      // If we see ground close, allow lower throttle to settle
+      if (!isnan(b) && b < 0.20 && self->land_thr > 1100) {
+        self->land_thr = 1100;
+      }
+
+      lf_set(throttle, self->land_thr);
+
+      // Touchdown detection: bottom distance < 8 cm for 0.3 s
+      if (!isnan(b) && b < 0.08) {
+        if (++self->land_touch >= 15) {
+          // Disarm and return to Neutral
+          lf_set(aux1, 1000);
+          lf_set(throttle, 1000);
+          lf_set_mode(Neutral);
+          printf("[AvoidPlanner] Landing -> Neutral (touchdown)\n");
+        }
+      } else {
+        self->land_touch = 0;
+      }
+    =}
+  }
+}
diff --git a/lf-drone/lib/msp_sender.lf b/lf-drone/lib/msp_sender.lf
new file mode 100644
index 0000000..58b0db9
--- /dev/null
+++ b/lf-drone/lib/msp_sender.lf
@@ -0,0 +1,193 @@
+target C
+
+preamble {=
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <termios.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+
+#define MSP_SET_RAW_RC 200
+
+static void ensure_parent_dir(const char* path) {
+  char dir[512];
+  strncpy(dir, path, sizeof(dir) - 1);
+  dir[sizeof(dir) - 1] = '\0';
+
+  char* slash = strrchr(dir, '/');
+  if (!slash) return;
+
+  *slash = '\0';
+
+  char cmd[600];
+  snprintf(cmd, sizeof(cmd), "mkdir -p \"%s\"", dir);
+  system(cmd);
+}
+
+static uint8_t cs(uint8_t size, uint8_t cmd, const uint8_t* data) {
+  uint8_t s = size ^ cmd;
+  for (int i = 0; i < size; i++) s ^= data[i];
+  return s;
+}
+
+static int open_serial(const char* dev) {
+  if (!dev || dev[0] == '\0') return -2;
+
+  int fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
+  if (fd < 0) return -1;
+
+  struct termios tio;
+  memset(&tio, 0, sizeof(tio));
+  cfmakeraw(&tio);
+  cfsetspeed(&tio, B115200);
+  tio.c_cflag |= CLOCAL | CREAD;
+  tio.c_cc[VMIN] = 0;
+  tio.c_cc[VTIME] = 1;
+
+  if (tcsetattr(fd, TCSANOW, &tio) != 0) {
+    close(fd);
+    return -1;
+  }
+  return fd;
+}
+
+static int send_rc(int fd, const uint16_t ch[16]) {
+  uint8_t payload[32];
+
+  for (int i = 0; i < 16; i++) {
+    uint16_t v = ch[i];
+    if (v < 1000) v = 1000;
+    if (v > 2000) v = 2000;
+    payload[2 * i]     = v & 0xFF;
+    payload[2 * i + 1] = (v >> 8) & 0xFF;
+  }
+
+  uint8_t header[3] = {'$', 'M', '<'};
+  uint8_t size = sizeof(payload);
+  uint8_t cmd  = MSP_SET_RAW_RC;
+  uint8_t csum = cs(size, cmd, payload);
+
+  if (write(fd, header, 3) != 3) return -1;
+  if (write(fd, &size, 1) != 1) return -1;
+  if (write(fd, &cmd, 1) != 1) return -1;
+  if (write(fd, payload, size) != size) return -1;
+  if (write(fd, &csum, 1) != 1) return -1;
+
+  return 0;
+}
+=}
+
+reactor MSPSender(
+  port:string="/dev/ttyACM0",
+  period:time=10 ms,
+  verbose:int=0,
+  print_every:int=1,
+  log_path:string=""
+) {
+  input roll:int
+  input pitch:int
+  input yaw:int
+  input throttle:int
+  input aux1:int
+  input aux2:int
+
+  timer tx(0, period)
+
+  state fd:int = -1
+  state fp: FILE*
+  state seq:int = 0
+
+  // Latched RC values
+  state rr:int  = 1500
+  state pp:int  = 1500
+  state yy:int  = 1500
+  state thr:int = 1000
+  state a1:int  = 1000
+  state a2:int  = 1000
+
+  reaction(startup) {=
+    self->fd = open_serial(self->port);
+    self->fp = NULL;
+    self->seq = 0;
+
+    if (self->fd >= 0) {
+      printf("[MSPSender] opened %s\n", self->port);
+    } else if (self->fd == -2) {
+      printf("[MSPSender] serial disabled; running in print/log-only mode.\n");
+    } else {
+      fprintf(stderr, "[MSPSender] FAILED to open %s\n", self->port);
+    }
+
+    if (self->log_path[0] != '\0') {
+      ensure_parent_dir(self->log_path);
+      self->fp = fopen(self->log_path, "w");
+      if (!self->fp) {
+        fprintf(stderr, "[MSPSender] Failed to open log file %s\n", self->log_path);
+      } else {
+        fprintf(self->fp, "step,roll,pitch,yaw,throttle,aux1,aux2\n");
+        fflush(self->fp);
+      }
+    }
+  =}
+
+  reaction(shutdown) {=
+    if (self->fp) {
+      fclose(self->fp);
+      self->fp = NULL;
+    }
+
+    if (self->fd >= 0) {
+      close(self->fd);
+      self->fd = -1;
+    }
+  =}
+
+  reaction(roll)     {= self->rr  = roll->value; =}
+  reaction(pitch)    {= self->pp  = pitch->value; =}
+  reaction(yaw)      {= self->yy  = yaw->value; =}
+  reaction(throttle) {= self->thr = throttle->value; =}
+  reaction(aux1)     {= self->a1  = aux1->value; =}
+  reaction(aux2)     {= self->a2  = aux2->value; =}
+
+  reaction(tx) {=
+    uint16_t rc[16] = {
+      1500,1500,1500,1000,1000,1000,1000,1000,
+      1000,1000,1000,1000,1000,1000,1000,1000
+    };
+
+    rc[0] = (uint16_t)self->rr;
+    rc[1] = (uint16_t)self->pp;
+    rc[2] = (uint16_t)self->yy;
+    rc[3] = (uint16_t)self->thr;
+    rc[4] = (uint16_t)self->a1;
+    rc[5] = (uint16_t)self->a2;
+
+    self->seq += 1;
+
+    if (self->fd >= 0) {
+      if (send_rc(self->fd, rc) != 0) {
+        fprintf(stderr, "[MSPSender] send_rc failed. Closing serial.\n");
+        close(self->fd);
+        self->fd = -1;
+      }
+    }
+
+    if (self->fp) {
+      fprintf(self->fp, "%d,%d,%d,%d,%d,%d,%d\n",
+              self->seq,
+              self->rr, self->pp, self->yy, self->thr, self->a1, self->a2);
+      fflush(self->fp);
+    }
+
+    if (self->verbose && self->print_every > 0 &&
+        (self->seq % self->print_every) == 0) {
+      printf("[MSPSender] step=%d roll=%d pitch=%d yaw=%d throttle=%d aux1=%d aux2=%d%s\n",
+             self->seq,
+             self->rr, self->pp, self->yy, self->thr, self->a1, self->a2,
+             (self->fd >= 0) ? "" : " [serial not active]");
+      fflush(stdout);
+    }
+  =}
+}
\ No newline at end of file
diff --git a/lf-drone/lib/tof_logger.py b/lf-drone/lib/tof_logger.py
new file mode 100644
index 0000000..46bf806
--- /dev/null
+++ b/lf-drone/lib/tof_logger.py
@@ -0,0 +1,273 @@
+#!/usr/bin/env python3
+
+from __future__ import annotations
+
+import argparse
+import csv
+import datetime as dt
+import os
+import queue
+import signal
+import subprocess
+import sys
+import threading
+import time
+from dataclasses import dataclass
+from typing import Dict, Tuple, List, Optional
+
+try:
+    from zoneinfo import ZoneInfo
+except Exception:
+    ZoneInfo = None  # type: ignore
+
+PHX_TZ_NAME = "America/Phoenix"
+
+# CSV column order you asked for
+CSV_ORDER = ["front", "bottom", "left", "right", "top"]
+
+
+@dataclass
+class SensorCfg:
+    name: str
+    bus: int
+    addr: int
+    mode: int
+    rate: float
+    timing_ms: int
+    max_m: float
+
+
+def now_local_filename(prefix: str = "tof", ext: str = ".csv") -> str:
+    if ZoneInfo is not None:
+        t = dt.datetime.now(tz=ZoneInfo(PHX_TZ_NAME))
+    else:
+        t = dt.datetime.now()
+    return f"{prefix}_{t.strftime('%Y%m%d_%H%M%S')}{ext}"
+
+
+def ensure_parent_dir(path: str) -> None:
+    parent = os.path.dirname(os.path.abspath(path))
+    if parent and not os.path.exists(parent):
+        os.makedirs(parent, exist_ok=True)
+
+
+def parse_sensor_arg(s: str, defaults: dict) -> SensorCfg:
+    """
+    Format:
+      name,bus=24,addr=0x29,mode=1,rate=20,timing_ms=100,max_m=4.0
+    Only name is required. Everything else falls back to defaults.
+    """
+    parts = [p.strip() for p in s.split(",") if p.strip()]
+    if not parts:
+        raise ValueError("Empty --sensor entry")
+
+    name = parts[0]
+    kv = {}
+    for p in parts[1:]:
+        if "=" not in p:
+            raise ValueError(f"Bad sensor field '{p}'. Use key=value.")
+        k, v = p.split("=", 1)
+        kv[k.strip()] = v.strip()
+
+    bus = int(kv.get("bus", defaults["bus"]))
+    addr_raw = kv.get("addr", defaults["addr"])
+    addr = int(addr_raw, 0) if isinstance(addr_raw, str) else int(addr_raw)
+    mode = int(kv.get("mode", defaults["mode"]))
+    rate = float(kv.get("rate", defaults["rate"]))
+    timing_ms = int(kv.get("timing_ms", defaults["timing_ms"]))
+    max_m = float(kv.get("max_m", defaults["max_m"]))
+
+    return SensorCfg(name=name, bus=bus, addr=addr, mode=mode, rate=rate, timing_ms=timing_ms, max_m=max_m)
+
+
+def default_sensor_set(rate: float, timing_ms: int, max_m: float) -> List[SensorCfg]:
+    # Your exact mapping (same addr 0x29 on different buses)
+    mapping = [
+        ("bottom", 22, 0x29, 1),
+        ("left",   23, 0x29, 1),
+        ("front",  24, 0x29, 1),
+        ("top",    25, 0x29, 1),
+        ("right",  26, 0x29, 1),
+    ]
+    return [
+        SensorCfg(name=n, bus=b, addr=a, mode=m, rate=rate, timing_ms=timing_ms, max_m=max_m)
+        for (n, b, a, m) in mapping
+    ]
+
+
+def read_lines_to_queue(
+    proc: subprocess.Popen,
+    sensor_name: str,
+    out_q: "queue.Queue[Tuple[str, float]]",
+    stop_evt: threading.Event,
+) -> None:
+    """
+    Expects one float (meters) per line from tof_reader.py stdout.
+    Pushes (sensor_name, distance_m).
+    """
+    assert proc.stdout is not None
+    while not stop_evt.is_set():
+        line = proc.stdout.readline()
+        if not line:
+            break
+        line = line.strip()
+        if not line:
+            continue
+        try:
+            d_m = float(line)
+        except ValueError:
+            continue
+        out_q.put((sensor_name, d_m))
+
+
+def obstacle_direction_within(latest_vals: Dict[str, float], threshold_m: float) -> str:
+    # Closest among the five, if <= threshold
+    candidates: List[Tuple[float, str]] = []
+    for name in CSV_ORDER:
+        if name in latest_vals:
+            candidates.append((latest_vals[name], name))
+    if not candidates:
+        return "none"
+    d_min, name_min = min(candidates, key=lambda x: x[0])
+    return name_min if d_min <= threshold_m else "none"
+
+
+def main() -> int:
+    ap = argparse.ArgumentParser()
+
+    ap.add_argument("--reader", default="./tof_reader.py", help="Path to your tof_reader.py")
+    ap.add_argument("--log", default=None, help="CSV path. Default: tof_YYYYMMDD_HHMMSS.csv")
+    ap.add_argument("--print", dest="do_print", action="store_true", help="Print live values")
+
+    ap.add_argument("--sensor", action="append", default=[],
+                    help="Repeatable sensor spec: name,bus=24,addr=0x29,mode=1,rate=20,timing_ms=100,max_m=4.0")
+
+    ap.add_argument("--rate", type=float, default=20.0)
+    ap.add_argument("--timing_ms", type=int, default=100)
+    ap.add_argument("--max_m", type=float, default=4.0)
+
+    ap.add_argument("--threshold_m", type=float, default=0.20,
+                    help="Obstacle threshold in meters (default 0.20)")
+
+    args = ap.parse_args()
+
+    log_path = args.log if args.log else now_local_filename("tof", ".csv")
+    ensure_parent_dir(log_path)
+
+    stop_evt = threading.Event()
+    out_q: "queue.Queue[Tuple[str, float]]" = queue.Queue()
+    procs: List[subprocess.Popen] = []
+    threads: List[threading.Thread] = []
+
+    latest_vals: Dict[str, float] = {}
+
+    def shutdown() -> None:
+        stop_evt.set()
+        for p in procs:
+            try:
+                p.terminate()
+            except Exception:
+                pass
+
+    def on_signal(signum, frame):  # noqa: ANN001
+        shutdown()
+
+    signal.signal(signal.SIGINT, on_signal)
+    signal.signal(signal.SIGTERM, on_signal)
+
+    # Decide sensors:
+    # - If --sensor provided, use only those
+    # - Else use your default mapping (all five)
+    if args.sensor:
+        defaults = {"bus": 24, "addr": 0x29, "mode": 1, "rate": args.rate, "timing_ms": args.timing_ms, "max_m": args.max_m}
+        sensor_cfgs = [parse_sensor_arg(s, defaults) for s in args.sensor]
+    else:
+        sensor_cfgs = default_sensor_set(rate=args.rate, timing_ms=args.timing_ms, max_m=args.max_m)
+
+    # Launch tof_reader.py per sensor
+    for cfg in sensor_cfgs:
+        err_path = f"tof_reader_{cfg.name}.err"
+        err_f = open(err_path, "w", buffering=1)
+
+        cmd = [
+            sys.executable,
+            args.reader,
+            "--bus", str(cfg.bus),
+            "--addr", hex(cfg.addr),
+            "--rate", str(cfg.rate),
+            "--mode", str(cfg.mode),
+            "--timing_ms", str(cfg.timing_ms),
+            "--max_m", str(cfg.max_m),
+        ]
+
+        p = subprocess.Popen(
+            cmd,
+            stdout=subprocess.PIPE,
+            stderr=err_f,      # IMPORTANT: keep errors per-sensor
+            text=True,
+            bufsize=1,
+        )
+        procs.append(p)
+
+        t = threading.Thread(target=read_lines_to_queue, args=(p, cfg.name, out_q, stop_evt), daemon=True)
+        t.start()
+        threads.append(t)
+
+    # Write CSV
+    new_file = not os.path.exists(log_path) or os.path.getsize(log_path) == 0
+    with open(log_path, "a", newline="") as f:
+        w = csv.writer(f)
+        if new_file:
+            w.writerow([
+                "front_m",
+                "bottom_m",
+                "left_m",
+                "right_m",
+                "top_m",
+                "obstacle_within_0p20m_direction",
+            ])
+            f.flush()
+
+        try:
+            while not stop_evt.is_set():
+                try:
+                    name, d_m = out_q.get(timeout=0.25)
+                except queue.Empty:
+                    continue
+
+                latest_vals[name] = d_m
+                direction = obstacle_direction_within(latest_vals, args.threshold_m)
+
+                row = []
+                for sensor in CSV_ORDER:
+                    row.append(f"{latest_vals[sensor]:.3f}" if sensor in latest_vals else "")
+                row.append(direction)
+
+                w.writerow(row)
+                f.flush()
+
+                if args.do_print:
+                    s = " ".join([f"{k}={latest_vals[k]:.3f}" if k in latest_vals else f"{k}=NA" for k in CSV_ORDER])
+                    print(f"{s} | obstacle<=0.20m: {direction}", flush=True)
+
+        except KeyboardInterrupt:
+            shutdown()
+        finally:
+            shutdown()
+            for p in procs:
+                try:
+                    p.wait(timeout=1.0)
+                except Exception:
+                    try:
+                        p.kill()
+                    except Exception:
+                        pass
+
+    if args.do_print:
+        print(f"saved: {log_path}", flush=True)
+
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())
diff --git a/lf-drone/lib/tof_reader.py b/lf-drone/lib/tof_reader.py
new file mode 100644
index 0000000..ee68138
--- /dev/null
+++ b/lf-drone/lib/tof_reader.py
@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+# Prints one float (meters) per line at the requested rate.
+
+import argparse
+import sys
+import time
+import traceback
+
+def main():
+    ap = argparse.ArgumentParser()
+    ap.add_argument("--bus", type=int, default=22)
+    ap.add_argument("--addr", type=lambda x: int(x, 0), default=0x29)
+    ap.add_argument("--rate", type=float, default=20.0)
+    ap.add_argument("--mode", type=int, default=3, choices=[1,2,3])
+    ap.add_argument("--timing_ms", type=int, default=100)
+    ap.add_argument("--max_m", type=float, default=4.0)
+    args = ap.parse_args()
+
+    try:
+        # Your working import
+        from VL53L1X import VL53L1X
+    except Exception:
+        traceback.print_exc()
+        print("ERR: cannot import VL53L1X", file=sys.stderr)
+        sys.exit(2)
+
+    try:
+        sensor = VL53L1X(i2c_bus=args.bus, i2c_address=args.addr)
+        if hasattr(sensor, "open"):
+            sensor.open()
+        # some builds print device info on start_ranging
+        sensor.start_ranging(args.mode)
+        if hasattr(sensor, "set_timing_budget"):
+            sensor.set_timing_budget(args.timing_ms)
+
+        period = 1.0 / max(1.0, args.rate)
+        while True:
+            d_mm = sensor.get_distance()
+            if d_mm and d_mm > 0:
+                d_m = d_mm / 1000.0
+                if d_m > args.max_m:
+                    d_m = args.max_m
+                print(f"{d_m:.3f}", flush=True)
+            time.sleep(period)
+
+    except KeyboardInterrupt:
+        pass
+    except Exception:
+        traceback.print_exc()
+        sys.exit(3)
+    finally:
+        try:
+            if "sensor" in locals() and hasattr(sensor, "stop_ranging"):
+                sensor.stop_ranging()
+        except Exception:
+            pass
+
+if __name__ == "__main__":
+    main()
diff --git a/lf-drone/simulation/README.md b/lf-drone/simulation/README.md
new file mode 100644
index 0000000..6f70400
--- /dev/null
+++ b/lf-drone/simulation/README.md
@@ -0,0 +1,62 @@
+# Overview
+
+This directory contains the CSV-based simulation workflow for the Lingua Franca drone demo. It replays recorded ToF measurements through the same avoidance logic used by the hardware workflow and logs the resulting RC commands for offline analysis.
+
+## Directory Structure
+
+- `src/test.lf`: main simulation entry point
+- `src/ToFBridgeCSV.lf`: CSV-backed ToF reader used in place of live hardware
+- `data/`: sample ToF input CSV files collected from the drone using ./lib
+- `results/`: RC logs, plots, and plotting utilities
+
+# Prerequisites
+
+See the repository root [README.md](../README.md) for the shared software setup and Python dependencies.
+
+### To Run the Code
+
+```bash
+lfc simulation/src/test.lf
+./simulation/bin/test
+```
+
+### Additional Instructions
+
+- Before running `lfc`, update the hard-coded absolute import paths in `simulation/src/test.lf` so they point to the local files in this repository's `lib/` directory.
+- In this file, `ToFBridgeCSV.lf` is already local to `simulation/src/`, so that import can stay unchanged.
+
+- For example, change:
+
+```lf
+import AvoidPlanner from "/mnt/e/PhD/lf-demos/lf-drone/lib/avoid_planner_modal.lf"
+```
+
+to:
+
+```lf
+import AvoidPlanner from "../../lib/avoid_planner_modal.lf"
+```
+
+- The full import block in `simulation/src/test.lf` should look like this:
+
+```lf
+import PyToF from "ToFBridgeCSV.lf"
+import AvoidPlanner from "../../lib/avoid_planner_modal.lf"
+import MSPSender from "../../lib/msp_sender.lf"
+import UserLandCmd from "../../lib/UserLandCmd.lf"
+```
+
+- The current `src/test.lf` file reads sensor data from `simulation/data/`, `bottom.csv`, `front.csv`, `right.csv`, `left.csv`, and `top.csv`.
+- The simulation sets `port=""` in `MSPSender`, so it runs in log-only mode and does not require a real flight controller.
+- The current RC output log path is `simulation/results/rc-out.csv`. You can change that value in `src/test.lf` if you want to create a new log file.
+- Launch the final executable from the repository root so the configured CSV and log paths resolve correctly.
+
+### Verification Instructions
+
+After the simulation finishes, you can generate a path plot from the RC outputs and the ToF CSV files:
+
+```bash
+python3 simulation/results/plot_drone_path.py simulation/results/rc-out-file simulation/data
+```
+
+The output PDF is written into `simulation/results/`.
\ No newline at end of file
diff --git a/lf-drone/simulation/data/bottom.csv b/lf-drone/simulation/data/bottom.csv
new file mode 100644
index 0000000..2511ae4
--- /dev/null
+++ b/lf-drone/simulation/data/bottom.csv
@@ -0,0 +1,226 @@
+value_m
+0.008
+0.008
+0.008
+0.008
+0.008
+0.051
+0.051
+0.051
+0.051
+0.051
+0.053
+0.053
+0.053
+0.053
+0.053
+0.059
+0.059
+0.059
+0.059
+0.054
+0.054
+0.054
+0.054
+0.121
+0.121
+0.121
+0.121
+0.121
+0.121
+0.280
+0.280
+0.280
+0.280
+0.280
+0.585
+0.585
+0.585
+0.585
+0.585
+0.791
+0.791
+0.791
+0.791
+0.999
+0.999
+0.999
+0.999
+0.999
+0.961
+0.961
+0.961
+0.961
+0.961
+0.922
+0.922
+0.922
+0.922
+0.939
+0.939
+0.939
+0.939
+1.148
+1.148
+1.148
+1.148
+1.148
+1.114
+1.114
+1.114
+1.114
+1.114
+1.080
+1.080
+1.080
+1.080
+1.080
+1.076
+1.076
+1.076
+1.076
+1.076
+1.104
+1.104
+1.104
+1.104
+1.104
+1.093
+1.093
+1.093
+1.093
+1.093
+1.036
+1.036
+1.036
+1.036
+1.036
+1.029
+1.029
+1.029
+1.029
+1.029
+0.914
+0.914
+0.914
+0.914
+0.914
+0.822
+0.822
+0.822
+0.822
+0.822
+0.831
+0.831
+0.831
+0.831
+0.831
+0.858
+0.858
+0.858
+0.858
+0.858
+0.836
+0.836
+0.836
+0.836
+0.836
+0.831
+0.831
+0.831
+0.831
+0.831
+0.789
+0.789
+0.789
+0.789
+0.789
+0.783
+0.783
+0.783
+0.783
+0.783
+0.801
+0.801
+0.801
+0.801
+0.801
+0.819
+0.819
+0.819
+0.819
+0.819
+0.920
+0.920
+0.920
+0.920
+0.920
+1.066
+1.066
+1.066
+1.066
+1.066
+1.187
+1.187
+1.187
+1.187
+1.187
+1.165
+1.165
+1.165
+1.165
+1.165
+0.988
+0.988
+0.988
+0.988
+0.988
+0.833
+0.833
+0.833
+0.833
+0.833
+0.776
+0.776
+0.776
+0.776
+0.776
+0.760
+0.760
+0.760
+0.760
+0.760
+0.739
+0.739
+0.739
+0.739
+0.739
+0.709
+0.709
+0.709
+0.709
+0.709
+0.660
+0.660
+0.660
+0.660
+0.660
+0.574
+0.574
+0.574
+0.574
+0.574
+0.189
+0.189
+0.189
+0.189
+0.189
+0.015
+0.015
+0.015
+0.015
+0.015
+0.048
+0.048
+0.048
+0.048
diff --git a/lf-drone/simulation/data/front.csv b/lf-drone/simulation/data/front.csv
new file mode 100644
index 0000000..fd619be
--- /dev/null
+++ b/lf-drone/simulation/data/front.csv
@@ -0,0 +1,226 @@
+value_m
+0.597
+0.597
+0.597
+0.609
+0.609
+0.609
+0.609
+0.614
+0.614
+0.614
+0.614
+0.614
+0.383
+0.383
+0.383
+0.383
+0.383
+0.618
+0.618
+0.618
+0.618
+0.618
+0.746
+0.746
+0.746
+0.746
+0.746
+0.710
+0.710
+0.710
+0.710
+0.710
+0.785
+0.785
+0.785
+0.785
+0.785
+0.726
+0.726
+0.726
+0.726
+0.726
+0.272
+0.272
+0.272
+0.272
+0.272
+0.192
+0.192
+0.192
+0.192
+0.192
+0.278
+0.278
+0.278
+0.278
+0.278
+0.278
+0.289
+0.289
+0.289
+0.289
+0.289
+0.282
+0.282
+0.282
+0.282
+0.282
+0.265
+0.265
+0.265
+0.265
+0.265
+0.226
+0.226
+0.226
+0.226
+0.226
+0.212
+0.212
+0.212
+0.212
+0.212
+0.197
+0.197
+0.197
+0.197
+0.197
+0.205
+0.205
+0.205
+0.205
+0.205
+0.118
+0.118
+0.118
+0.118
+0.118
+0.023
+0.023
+0.023
+0.023
+0.109
+0.109
+0.109
+0.109
+0.109
+0.228
+0.228
+0.228
+0.228
+0.228
+0.194
+0.194
+0.194
+0.194
+0.194
+0.135
+0.135
+0.135
+0.135
+0.135
+0.086
+0.086
+0.086
+0.086
+0.086
+0.137
+0.137
+0.137
+0.137
+0.137
+0.230
+0.230
+0.230
+0.230
+0.230
+0.078
+0.078
+0.078
+0.078
+0.078
+0.041
+0.041
+0.041
+0.041
+0.041
+0.163
+0.163
+0.163
+0.163
+0.163
+0.251
+0.251
+0.251
+0.251
+0.251
+0.217
+0.217
+0.217
+0.217
+0.217
+0.103
+0.103
+0.103
+0.103
+0.103
+0.150
+0.150
+0.150
+0.150
+0.150
+0.202
+0.202
+0.202
+0.202
+0.202
+0.253
+0.253
+0.253
+0.253
+0.253
+0.270
+0.270
+0.270
+0.270
+0.270
+0.127
+0.127
+0.127
+0.127
+0.127
+0.191
+0.191
+0.191
+0.191
+0.191
+0.311
+0.311
+0.311
+0.311
+0.311
+0.292
+0.292
+0.292
+0.292
+0.292
+0.403
+0.403
+0.403
+0.403
+0.403
+0.628
+0.628
+0.628
+0.628
+0.628
+0.560
+0.560
+0.560
+0.560
+0.560
+0.561
+0.561
+0.561
diff --git a/lf-drone/simulation/data/left.csv b/lf-drone/simulation/data/left.csv
new file mode 100644
index 0000000..e3b89ce
--- /dev/null
+++ b/lf-drone/simulation/data/left.csv
@@ -0,0 +1,226 @@
+value_m
+0.505
+0.505
+0.506
+0.506
+0.506
+0.506
+0.506
+0.506
+0.504
+0.504
+0.504
+0.504
+0.504
+0.510
+0.510
+0.510
+0.510
+0.510
+0.509
+0.509
+0.509
+0.509
+0.509
+0.509
+0.581
+0.581
+0.581
+0.581
+0.581
+0.581
+0.547
+0.547
+0.547
+0.547
+0.547
+0.500
+0.500
+0.500
+0.500
+0.500
+0.396
+0.396
+0.396
+0.396
+0.396
+0.356
+0.356
+0.356
+0.356
+0.356
+0.285
+0.285
+0.285
+0.285
+0.285
+0.414
+0.414
+0.414
+0.414
+0.414
+0.531
+0.531
+0.531
+0.531
+0.531
+0.543
+0.543
+0.543
+0.543
+0.543
+0.473
+0.473
+0.473
+0.473
+0.473
+0.343
+0.343
+0.343
+0.343
+0.343
+0.207
+0.207
+0.207
+0.207
+0.207
+0.114
+0.114
+0.114
+0.114
+0.114
+0.184
+0.184
+0.184
+0.184
+0.184
+0.266
+0.266
+0.266
+0.266
+0.266
+0.365
+0.365
+0.365
+0.365
+0.365
+0.418
+0.418
+0.418
+0.418
+0.418
+0.429
+0.429
+0.429
+0.429
+0.429
+0.371
+0.371
+0.371
+0.371
+0.371
+0.713
+0.713
+0.713
+0.713
+0.713
+0.425
+0.425
+0.425
+0.425
+0.425
+0.231
+0.231
+0.231
+0.231
+0.231
+0.323
+0.323
+0.323
+0.323
+0.323
+0.664
+0.664
+0.664
+0.664
+0.664
+0.454
+0.454
+0.454
+0.454
+0.454
+0.249
+0.249
+0.249
+0.249
+0.249
+0.394
+0.394
+0.394
+0.394
+0.394
+0.294
+0.294
+0.294
+0.294
+0.294
+0.317
+0.317
+0.317
+0.317
+0.317
+0.504
+0.504
+0.504
+0.504
+0.504
+0.462
+0.462
+0.462
+0.462
+0.462
+0.379
+0.379
+0.379
+0.379
+0.379
+0.732
+0.732
+0.732
+0.732
+0.732
+0.483
+0.483
+0.483
+0.483
+0.483
+0.450
+0.450
+0.450
+0.450
+0.450
+0.381
+0.381
+0.381
+0.381
+0.381
+0.597
+0.597
+0.597
+0.597
+0.597
+0.529
+0.529
+0.529
+0.529
+0.529
+0.459
+0.459
+0.459
+0.459
+0.459
+0.478
+0.478
+0.478
+0.478
+0.478
diff --git a/lf-drone/simulation/data/rc_out_2.csv b/lf-drone/simulation/data/rc_out_2.csv
new file mode 100644
index 0000000..f6c8dce
--- /dev/null
+++ b/lf-drone/simulation/data/rc_out_2.csv
@@ -0,0 +1,226 @@
+step,roll,pitch,yaw,throttle,aux1,aux2
+1,1500,1500,1500,1000,1000,1800
+2,1500,1500,1500,1000,1000,1800
+3,1500,1500,1500,1000,1000,1800
+4,1500,1500,1500,1000,1000,1800
+5,1500,1500,1500,1000,1000,1800
+6,1500,1500,1500,1000,1000,1800
+7,1500,1500,1500,1000,1000,1800
+8,1500,1500,1500,1000,1000,1800
+9,1500,1500,1500,1000,1000,1800
+10,1500,1500,1500,1000,1000,1800
+11,1500,1500,1500,1000,1000,1800
+12,1500,1500,1500,1000,1000,1800
+13,1500,1500,1500,1000,1000,1800
+14,1500,1500,1500,1000,1000,1800
+15,1500,1500,1500,1000,1000,1800
+16,1500,1500,1500,1000,1000,1800
+17,1500,1500,1500,1000,1000,1800
+18,1500,1500,1500,1000,1000,1800
+19,1500,1500,1500,1000,1000,1800
+20,1500,1500,1500,1000,1000,1800
+21,1500,1500,1500,1000,1000,1800
+22,1500,1500,1500,1000,1000,1800
+23,1500,1500,1500,1000,1000,1800
+24,1500,1500,1500,1000,1000,1800
+25,1500,1500,1500,1000,1000,1800
+26,1500,1500,1500,1000,1000,1800
+27,1500,1500,1500,1000,1000,1800
+28,1500,1500,1500,1000,1000,1800
+29,1500,1500,1500,1000,1000,1800
+30,1500,1500,1500,1000,1000,1800
+31,1500,1500,1500,1000,1000,1800
+32,1500,1500,1500,1000,1000,1800
+33,1500,1500,1500,1000,1000,1800
+34,1500,1500,1500,1000,1000,1800
+35,1500,1500,1500,1000,1000,1800
+36,1500,1500,1500,1000,1000,1800
+37,1500,1500,1500,1000,1000,1800
+38,1500,1500,1500,1000,1000,1800
+39,1500,1500,1500,1000,1000,1800
+40,1500,1500,1500,1000,1000,1800
+41,1500,1500,1500,1000,1000,1800
+42,1500,1500,1500,1000,1000,1800
+43,1500,1500,1500,1000,1000,1800
+44,1500,1500,1500,1000,1000,1800
+45,1500,1500,1500,1000,1000,1800
+46,1500,1500,1500,1000,1000,1800
+47,1500,1500,1500,1000,1000,1800
+48,1500,1500,1500,1000,1000,1800
+49,1500,1500,1500,1000,1000,1800
+50,1500,1500,1500,1000,1000,1800
+51,1500,1500,1500,1000,1000,1800
+52,1500,1500,1500,1000,1000,1800
+53,1500,1500,1500,1000,1000,1800
+54,1500,1500,1500,1000,1000,1800
+55,1500,1500,1500,1000,1000,1800
+56,1500,1500,1500,1000,1000,1800
+57,1500,1500,1500,1000,1000,1800
+58,1500,1500,1500,1000,1000,1800
+59,1500,1500,1500,1000,1000,1800
+60,1500,1500,1500,1000,1000,1800
+61,1500,1500,1500,1000,1000,1800
+62,1500,1500,1500,1000,1000,1800
+63,1500,1500,1500,1000,1000,1800
+64,1500,1500,1500,1000,1000,1800
+65,1500,1500,1500,1000,1000,1800
+66,1500,1500,1500,1000,1000,1800
+67,1500,1500,1500,1000,1000,1800
+68,1500,1500,1500,1000,1000,1800
+69,1500,1500,1500,1000,1000,1800
+70,1500,1500,1500,1000,1000,1800
+71,1500,1500,1500,1000,1000,1800
+72,1500,1500,1500,1000,1000,1800
+73,1500,1500,1500,1000,1000,1800
+74,1500,1500,1500,1000,1000,1800
+75,1500,1500,1500,1000,1000,1800
+76,1500,1500,1500,1000,1800,1800
+77,1500,1500,1500,1000,1800,1800
+78,1500,1500,1500,1000,1800,1800
+79,1500,1500,1500,1000,1800,1800
+80,1500,1500,1500,1000,1800,1800
+81,1500,1500,1500,1000,1800,1800
+82,1500,1500,1500,1000,1800,1800
+83,1500,1500,1500,1000,1800,1800
+84,1500,1500,1500,1000,1800,1800
+85,1500,1500,1500,1000,1800,1800
+86,1500,1500,1500,1000,1800,1800
+87,1500,1500,1500,1000,1800,1800
+88,1500,1500,1500,1000,1800,1800
+89,1500,1500,1500,1000,1800,1800
+90,1500,1500,1500,1000,1800,1800
+91,1500,1500,1500,1000,1800,1800
+92,1500,1500,1500,1000,1800,1800
+93,1500,1500,1500,1000,1800,1800
+94,1500,1500,1500,1000,1800,1800
+95,1500,1500,1500,1000,1800,1800
+96,1500,1500,1500,1000,1800,1800
+97,1500,1500,1500,1000,1800,1800
+98,1500,1500,1500,1000,1800,1800
+99,1500,1500,1500,1000,1800,1800
+100,1500,1500,1500,1000,1800,1800
+101,1500,1500,1500,1000,1800,1800
+102,1500,1500,1500,1000,1800,1800
+103,1500,1500,1500,1000,1800,1800
+104,1500,1500,1500,1000,1800,1800
+105,1500,1500,1500,1000,1800,1800
+106,1500,1500,1500,1000,1800,1800
+107,1500,1500,1500,1000,1800,1800
+108,1500,1500,1500,1000,1800,1800
+109,1500,1500,1500,1000,1800,1800
+110,1500,1500,1500,1000,1800,1800
+111,1500,1500,1500,1000,1800,1800
+112,1500,1500,1500,1000,1800,1800
+113,1500,1500,1500,1000,1800,1800
+114,1500,1500,1500,1000,1800,1800
+115,1500,1500,1500,1000,1800,1800
+116,1500,1500,1500,1000,1800,1800
+117,1500,1500,1500,1000,1800,1800
+118,1500,1500,1500,1000,1800,1800
+119,1500,1500,1500,1000,1800,1800
+120,1500,1500,1500,1000,1800,1800
+121,1500,1500,1500,1000,1800,1800
+122,1500,1500,1500,1000,1800,1800
+123,1500,1500,1500,1000,1800,1800
+124,1500,1500,1500,1000,1800,1800
+125,1500,1500,1500,1000,1800,1800
+126,1500,1500,1500,1000,1800,1800
+127,1500,1500,1500,1000,1800,1800
+128,1500,1500,1500,1000,1800,1800
+129,1500,1500,1500,1000,1800,1800
+130,1500,1500,1500,1000,1800,1800
+131,1500,1500,1500,1000,1800,1800
+132,1500,1500,1500,1000,1800,1800
+133,1500,1500,1500,1000,1800,1800
+134,1500,1500,1500,1000,1800,1800
+135,1500,1500,1500,1000,1800,1800
+136,1500,1500,1500,1000,1800,1800
+137,1500,1500,1500,1000,1800,1800
+138,1500,1500,1500,1000,1800,1800
+139,1500,1500,1500,1000,1800,1800
+140,1500,1500,1500,1000,1800,1800
+141,1500,1500,1500,1000,1800,1800
+142,1500,1500,1500,1000,1800,1800
+143,1500,1500,1500,1000,1800,1800
+144,1500,1500,1500,1000,1800,1800
+145,1500,1500,1500,1000,1800,1800
+146,1500,1500,1500,1000,1800,1800
+147,1500,1500,1500,1000,1800,1800
+148,1500,1500,1500,1000,1800,1800
+149,1500,1500,1500,1000,1800,1800
+150,1500,1500,1500,1000,1800,1800
+151,1500,1500,1500,1000,1800,1800
+152,1500,1500,1500,1000,1800,1800
+153,1500,1500,1500,1000,1800,1800
+154,1500,1500,1500,1000,1800,1800
+155,1500,1500,1500,1000,1800,1800
+156,1500,1500,1500,1000,1800,1800
+157,1500,1500,1500,1000,1800,1800
+158,1500,1500,1500,1000,1800,1800
+159,1500,1500,1500,1000,1800,1800
+160,1500,1500,1500,1000,1800,1800
+161,1500,1500,1500,1000,1800,1800
+162,1500,1500,1500,1000,1800,1800
+163,1500,1500,1500,1000,1800,1800
+164,1500,1500,1500,1000,1800,1800
+165,1500,1500,1500,1000,1800,1800
+166,1500,1500,1500,1000,1800,1800
+167,1500,1500,1500,1000,1800,1800
+168,1500,1500,1500,1000,1800,1800
+169,1500,1500,1500,1000,1800,1800
+170,1500,1500,1500,1000,1800,1800
+171,1500,1500,1500,1000,1800,1800
+172,1500,1500,1500,1000,1800,1800
+173,1500,1500,1500,1000,1800,1800
+174,1500,1500,1500,1000,1800,1800
+175,1500,1500,1500,1000,1800,1800
+176,1500,1500,1500,1305,1800,1800
+177,1500,1500,1500,1310,1800,1800
+178,1500,1500,1500,1315,1800,1800
+179,1500,1500,1500,1320,1800,1800
+180,1500,1500,1500,1325,1800,1800
+181,1500,1500,1500,1330,1800,1800
+182,1500,1500,1500,1335,1800,1800
+183,1500,1500,1500,1340,1800,1800
+184,1500,1500,1500,1345,1800,1800
+185,1500,1500,1500,1350,1800,1800
+186,1500,1500,1500,1355,1800,1800
+187,1500,1500,1500,1360,1800,1800
+188,1500,1500,1500,1365,1800,1800
+189,1500,1500,1500,1370,1800,1800
+190,1500,1500,1500,1375,1800,1800
+191,1500,1500,1500,1380,1800,1800
+192,1500,1500,1500,1385,1800,1800
+193,1500,1500,1500,1390,1800,1800
+194,1500,1500,1500,1395,1800,1800
+195,1500,1500,1500,1400,1800,1800
+196,1500,1500,1500,1405,1800,1800
+197,1500,1500,1500,1410,1800,1800
+198,1500,1500,1500,1415,1800,1800
+199,1500,1500,1500,1420,1800,1800
+200,1500,1500,1500,1425,1800,1800
+201,1500,1500,1500,1430,1800,1800
+202,1500,1500,1500,1435,1800,1800
+203,1500,1500,1500,1440,1800,1800
+204,1500,1500,1500,1445,1800,1800
+205,1500,1500,1500,1450,1800,1800
+206,1500,1500,1500,1450,1800,1800
+207,1500,1500,1500,1450,1800,1800
+208,1500,1500,1500,1450,1800,1800
+209,1500,1500,1500,1450,1800,1800
+210,1500,1500,1500,1450,1800,1800
+211,1500,1500,1500,1450,1800,1800
+212,1500,1500,1500,1450,1800,1800
+213,1500,1500,1500,1450,1800,1800
+214,1500,1500,1500,1450,1800,1800
+215,1500,1500,1500,1450,1800,1800
+216,1500,1500,1500,1450,1800,1800
+217,1500,1500,1500,1450,1800,1800
+218,1500,1500,1500,1450,1800,1800
+219,1500,1500,1500,1450,1800,1800
+220,1500,1500,1500,1450,1800,1800
+221,1500,1500,1500,1450,1800,1800
+222,1500,1500,1500,1450,1800,1800
+223,1500,1500,1500,1450,1800,1800
+224,1500,1500,1500,1450,1800,1800
+225,1500,1500,1500,1450,1800,1800
diff --git a/lf-drone/simulation/data/right.csv b/lf-drone/simulation/data/right.csv
new file mode 100644
index 0000000..a138501
--- /dev/null
+++ b/lf-drone/simulation/data/right.csv
@@ -0,0 +1,226 @@
+value_m
+0.635
+0.635
+0.635
+0.635
+0.632
+0.632
+0.632
+0.632
+0.632
+0.633
+0.633
+0.633
+0.633
+0.633
+0.631
+0.631
+0.631
+0.631
+0.631
+0.631
+0.632
+0.632
+0.632
+0.632
+0.632
+0.625
+0.625
+0.625
+0.682
+0.682
+0.682
+0.682
+0.682
+0.652
+0.652
+0.652
+0.652
+0.652
+0.627
+0.627
+0.627
+0.627
+0.627
+0.627
+0.555
+0.555
+0.555
+0.555
+0.555
+0.540
+0.540
+0.540
+0.540
+0.540
+0.472
+0.472
+0.472
+0.472
+0.472
+0.283
+0.283
+0.283
+0.283
+0.283
+0.221
+0.221
+0.221
+0.221
+0.221
+0.291
+0.291
+0.291
+0.291
+0.291
+0.514
+0.514
+0.514
+0.514
+0.514
+0.640
+0.640
+0.640
+0.640
+0.640
+0.734
+0.734
+0.734
+0.734
+0.734
+0.735
+0.735
+0.735
+0.735
+0.735
+0.616
+0.616
+0.616
+0.616
+0.616
+0.594
+0.594
+0.594
+0.594
+0.594
+0.655
+0.655
+0.655
+0.655
+0.655
+0.697
+0.697
+0.697
+0.697
+0.697
+0.563
+0.563
+0.563
+0.563
+0.563
+0.272
+0.272
+0.272
+0.272
+0.272
+0.544
+0.544
+0.544
+0.544
+0.544
+0.495
+0.495
+0.495
+0.495
+0.495
+0.247
+0.247
+0.247
+0.247
+0.247
+0.437
+0.437
+0.437
+0.437
+0.437
+0.632
+0.632
+0.632
+0.632
+0.632
+0.666
+0.666
+0.666
+0.666
+0.666
+0.650
+0.650
+0.650
+0.650
+0.650
+0.585
+0.585
+0.585
+0.585
+0.585
+0.386
+0.386
+0.386
+0.386
+0.386
+0.459
+0.459
+0.459
+0.459
+0.459
+0.580
+0.580
+0.580
+0.580
+0.580
+0.490
+0.490
+0.490
+0.490
+0.490
+0.373
+0.373
+0.373
+0.373
+0.373
+0.582
+0.582
+0.582
+0.582
+0.582
+0.375
+0.375
+0.375
+0.375
+0.375
+0.427
+0.427
+0.427
+0.427
+0.427
+0.576
+0.576
+0.576
+0.576
+0.576
+0.700
+0.700
+0.700
+0.700
+0.700
+0.709
+0.709
+0.709
+0.709
+0.709
+0.679
+0.679
+0.679
+0.679
+0.679
+0.678
diff --git a/lf-drone/simulation/data/tof_20260211_123206.csv b/lf-drone/simulation/data/tof_20260211_123206.csv
new file mode 100644
index 0000000..30b8c9b
--- /dev/null
+++ b/lf-drone/simulation/data/tof_20260211_123206.csv
@@ -0,0 +1,72 @@
+front_m,bottom_m,left_m,right_m,top_m,obstacle_within_0p20m_direction
+,,,0.100,,right
+,,0.419,0.100,,right
+,,0.419,0.100,1.933,right
+0.460,,0.419,0.100,1.933,right
+0.460,,0.419,0.099,1.933,right
+0.460,,0.430,0.099,1.933,right
+0.460,0.029,0.430,0.099,1.933,bottom
+0.460,0.029,0.430,0.099,1.946,bottom
+0.460,0.029,0.430,0.099,1.946,bottom
+0.460,0.029,0.430,0.100,1.946,bottom
+0.460,0.029,0.421,0.100,1.946,bottom
+0.460,0.044,0.421,0.100,1.946,bottom
+0.460,0.044,0.421,0.100,1.950,bottom
+0.469,0.044,0.421,0.100,1.950,bottom
+0.469,0.044,0.421,0.099,1.950,bottom
+0.469,0.042,0.421,0.099,1.950,bottom
+0.469,0.042,0.421,0.099,1.953,bottom
+0.463,0.042,0.421,0.099,1.953,bottom
+0.463,0.042,0.421,0.100,1.953,bottom
+0.463,0.043,0.421,0.100,1.953,bottom
+0.463,0.043,0.421,0.100,1.941,bottom
+0.458,0.043,0.421,0.100,1.941,bottom
+0.458,0.043,0.421,0.099,1.941,bottom
+0.458,0.045,0.421,0.099,1.941,bottom
+0.458,0.045,0.421,0.099,1.953,bottom
+0.464,0.045,0.421,0.099,1.953,bottom
+0.464,0.045,0.421,0.099,1.953,bottom
+0.464,0.044,0.421,0.099,1.953,bottom
+0.464,0.044,0.421,0.099,1.944,bottom
+0.463,0.044,0.421,0.099,1.944,bottom
+0.463,0.044,0.421,0.100,1.944,bottom
+0.463,0.045,0.421,0.100,1.944,bottom
+0.464,0.045,0.421,0.100,1.944,bottom
+0.464,0.045,0.421,0.100,1.941,bottom
+0.464,0.045,0.421,0.102,1.941,bottom
+0.464,0.045,0.421,0.102,1.941,bottom
+0.464,0.045,0.421,0.102,1.952,bottom
+0.458,0.045,0.421,0.102,1.952,bottom
+0.458,0.045,0.421,0.097,1.952,bottom
+0.458,0.046,0.421,0.097,1.952,bottom
+0.460,0.046,0.421,0.097,1.952,bottom
+0.460,0.046,0.421,0.097,1.947,bottom
+0.460,0.046,0.421,0.101,1.947,bottom
+0.460,0.044,0.421,0.101,1.947,bottom
+0.463,0.044,0.421,0.101,1.947,bottom
+0.463,0.044,0.421,0.101,1.934,bottom
+0.463,0.044,0.421,0.100,1.934,bottom
+0.463,0.043,0.421,0.100,1.934,bottom
+0.461,0.043,0.421,0.100,1.934,bottom
+0.461,0.043,0.421,0.100,1.946,bottom
+0.461,0.043,0.421,0.100,1.946,bottom
+0.461,0.044,0.421,0.100,1.946,bottom
+0.460,0.044,0.421,0.100,1.946,bottom
+0.460,0.044,0.421,0.100,1.940,bottom
+0.460,0.044,0.421,0.100,1.940,bottom
+0.460,0.044,0.421,0.100,1.940,bottom
+0.462,0.044,0.421,0.100,1.940,bottom
+0.462,0.044,0.421,0.100,1.945,bottom
+0.462,0.044,0.421,0.101,1.945,bottom
+0.462,0.043,0.421,0.101,1.945,bottom
+0.461,0.043,0.421,0.101,1.945,bottom
+0.461,0.043,0.421,0.101,1.948,bottom
+0.461,0.043,0.421,0.101,1.948,bottom
+0.461,0.043,0.421,0.101,1.948,bottom
+0.461,0.043,0.421,0.101,1.948,bottom
+0.461,0.043,0.421,0.101,1.937,bottom
+0.461,0.043,0.421,0.100,1.937,bottom
+0.461,0.043,0.421,0.100,1.937,bottom
+0.462,0.043,0.421,0.100,1.937,bottom
+0.462,0.043,0.421,0.100,1.944,bottom
+0.462,0.043,0.421,0.099,1.944,bottom
diff --git a/lf-drone/simulation/data/tof_20260211_131939.csv b/lf-drone/simulation/data/tof_20260211_131939.csv
new file mode 100644
index 0000000..87bc89c
--- /dev/null
+++ b/lf-drone/simulation/data/tof_20260211_131939.csv
@@ -0,0 +1,151 @@
+front_m,bottom_m,left_m,right_m,top_m,obstacle_within_0p20m_direction
+,,0.417,,,none
+0.020,,0.417,,,front
+0.020,,0.417,0.080,,front
+0.020,0.026,0.417,0.080,,front
+0.020,0.026,0.410,0.080,,front
+0.043,0.026,0.410,0.080,,bottom
+0.043,0.026,0.410,0.081,,bottom
+0.043,0.040,0.410,0.081,,bottom
+0.045,0.040,0.410,0.081,,bottom
+0.045,0.040,0.409,0.081,,bottom
+0.045,0.040,0.409,0.078,,bottom
+0.045,0.040,0.409,0.078,,bottom
+0.045,0.040,0.409,0.078,,bottom
+0.045,0.040,0.413,0.078,,bottom
+0.045,0.040,0.413,0.079,,bottom
+0.045,0.040,0.413,0.079,,bottom
+0.045,0.040,0.413,0.079,,bottom
+0.045,0.040,0.411,0.079,,bottom
+0.045,0.040,0.411,0.079,,bottom
+0.045,0.039,0.411,0.079,,bottom
+0.043,0.039,0.411,0.079,,bottom
+0.043,0.039,0.412,0.079,,bottom
+0.043,0.039,0.412,0.078,,bottom
+0.043,0.040,0.412,0.078,,bottom
+0.043,0.040,0.412,0.078,,bottom
+0.043,0.040,0.413,0.078,,bottom
+0.043,0.040,0.413,0.081,,bottom
+0.043,0.039,0.413,0.081,,bottom
+0.043,0.039,0.413,0.081,,bottom
+0.043,0.039,0.414,0.081,,bottom
+0.043,0.039,0.414,0.080,,bottom
+0.043,0.041,0.414,0.080,,bottom
+0.043,0.041,0.414,0.080,,bottom
+0.043,0.041,0.411,0.080,,bottom
+0.043,0.041,0.411,0.079,,bottom
+0.043,0.042,0.411,0.079,,bottom
+0.043,0.042,0.411,0.079,,bottom
+0.043,0.042,0.409,0.079,,bottom
+0.043,0.042,0.409,0.080,,bottom
+0.043,0.040,0.409,0.080,,bottom
+0.044,0.040,0.409,0.080,,bottom
+0.044,0.040,0.409,0.081,,bottom
+0.044,0.040,0.412,0.081,,bottom
+0.044,0.040,0.412,0.081,,bottom
+0.043,0.040,0.412,0.081,,bottom
+0.043,0.040,0.412,0.080,,bottom
+0.043,0.040,0.411,0.080,,bottom
+0.043,0.040,0.411,0.080,,bottom
+0.045,0.040,0.411,0.080,,bottom
+0.045,0.040,0.411,0.079,,bottom
+0.045,0.040,0.417,0.079,,bottom
+0.045,0.042,0.417,0.079,,bottom
+0.043,0.042,0.417,0.079,,bottom
+0.043,0.042,0.417,0.079,,bottom
+0.043,0.042,0.409,0.079,,bottom
+0.043,0.041,0.409,0.079,,bottom
+0.044,0.041,0.409,0.079,,bottom
+0.044,0.041,0.409,0.080,,bottom
+0.044,0.041,0.410,0.080,,bottom
+0.044,0.042,0.410,0.080,,bottom
+0.045,0.042,0.410,0.080,,bottom
+0.045,0.042,0.410,0.080,,bottom
+0.045,0.042,0.416,0.080,,bottom
+0.045,0.039,0.416,0.080,,bottom
+0.042,0.039,0.416,0.080,,bottom
+0.042,0.039,0.416,0.081,,bottom
+0.042,0.039,0.414,0.081,,bottom
+0.042,0.040,0.414,0.081,,bottom
+0.043,0.040,0.414,0.081,,bottom
+0.043,0.040,0.414,0.082,,bottom
+0.043,0.040,0.413,0.082,,bottom
+0.043,0.040,0.413,0.082,,bottom
+0.044,0.040,0.413,0.082,,bottom
+0.044,0.040,0.413,0.081,,bottom
+0.044,0.040,0.410,0.081,,bottom
+0.044,0.040,0.410,0.081,,bottom
+0.042,0.040,0.410,0.081,,bottom
+0.042,0.040,0.410,0.079,,bottom
+0.042,0.040,0.410,0.079,,bottom
+0.042,0.039,0.410,0.079,,bottom
+0.044,0.039,0.410,0.079,,bottom
+0.044,0.039,0.410,0.080,,bottom
+0.044,0.039,0.408,0.080,,bottom
+0.044,0.042,0.408,0.080,,bottom
+0.044,0.042,0.408,0.080,,bottom
+0.044,0.042,0.408,0.080,,bottom
+0.044,0.042,0.409,0.080,,bottom
+0.044,0.040,0.409,0.080,,bottom
+0.043,0.040,0.409,0.080,,bottom
+0.043,0.040,0.409,0.080,,bottom
+0.043,0.040,0.408,0.080,,bottom
+0.043,0.039,0.408,0.080,,bottom
+0.041,0.039,0.408,0.080,,bottom
+0.041,0.039,0.408,0.142,,bottom
+0.041,0.039,0.407,0.142,,bottom
+0.041,0.041,0.407,0.142,,front
+0.042,0.041,0.407,0.142,,bottom
+0.042,0.041,0.407,0.120,,bottom
+0.042,0.041,0.408,0.120,,bottom
+0.042,0.040,0.408,0.120,,bottom
+0.045,0.040,0.408,0.120,,bottom
+0.045,0.040,0.408,0.142,,bottom
+0.045,0.040,0.408,0.142,,bottom
+0.045,0.040,0.408,0.142,,bottom
+0.044,0.040,0.408,0.142,,bottom
+0.044,0.040,0.408,0.136,,bottom
+0.044,0.040,0.410,0.136,,bottom
+0.044,0.040,0.410,0.136,,bottom
+0.040,0.040,0.410,0.136,,front
+0.040,0.040,0.410,0.135,,front
+0.040,0.040,0.408,0.135,,front
+0.040,0.041,0.408,0.135,,front
+0.044,0.041,0.408,0.135,,bottom
+0.044,0.041,0.408,0.101,,bottom
+0.044,0.041,0.408,0.101,,bottom
+0.044,0.041,0.408,0.101,,bottom
+0.043,0.041,0.408,0.101,,bottom
+0.043,0.041,0.408,0.080,,bottom
+0.043,0.041,0.413,0.080,,bottom
+0.043,0.041,0.413,0.080,,bottom
+0.045,0.041,0.413,0.080,,bottom
+0.045,0.041,0.413,0.081,,bottom
+0.045,0.041,0.411,0.081,,bottom
+0.045,0.040,0.411,0.081,,bottom
+0.045,0.040,0.411,0.081,,bottom
+0.045,0.040,0.411,0.081,,bottom
+0.045,0.040,0.409,0.081,,bottom
+0.045,0.041,0.409,0.081,,bottom
+0.045,0.041,0.409,0.081,,bottom
+0.045,0.041,0.409,0.080,,bottom
+0.045,0.041,0.415,0.080,,bottom
+0.045,0.039,0.415,0.080,,bottom
+0.043,0.039,0.415,0.080,,bottom
+0.043,0.039,0.415,0.079,,bottom
+0.043,0.039,0.408,0.079,,bottom
+0.043,0.042,0.408,0.079,,bottom
+0.044,0.042,0.408,0.079,,bottom
+0.044,0.042,0.408,0.080,,bottom
+0.044,0.042,0.410,0.080,,bottom
+0.044,0.040,0.410,0.080,,bottom
+0.044,0.040,0.410,0.080,,bottom
+0.044,0.040,0.410,0.081,,bottom
+0.044,0.040,0.407,0.081,,bottom
+0.044,0.040,0.407,0.081,,bottom
+0.044,0.040,0.407,0.081,,bottom
+0.044,0.040,0.407,0.079,,bottom
+0.044,0.040,0.409,0.079,,bottom
+0.044,0.041,0.409,0.079,,bottom
+0.044,0.041,0.409,0.079,,bottom
+0.044,0.041,0.409,0.080,,bottom
diff --git a/lf-drone/simulation/data/tof_20260211_132101.csv b/lf-drone/simulation/data/tof_20260211_132101.csv
new file mode 100644
index 0000000..1e5a59f
--- /dev/null
+++ b/lf-drone/simulation/data/tof_20260211_132101.csv
@@ -0,0 +1,99 @@
+front_m,bottom_m,left_m,right_m,top_m,obstacle_within_0p20m_direction
+,,,,1.968,none
+0.310,,,,1.968,none
+0.310,,,0.076,1.968,right
+0.310,,0.371,0.076,1.968,right
+0.310,,0.371,0.076,1.996,right
+0.303,,0.371,0.076,1.996,right
+0.303,,0.371,0.077,1.996,right
+0.303,,0.374,0.077,1.996,right
+0.303,0.025,0.374,0.077,1.996,bottom
+0.303,0.025,0.374,0.077,1.989,bottom
+0.303,0.025,0.374,0.077,1.989,bottom
+0.303,0.025,0.374,0.073,1.989,bottom
+0.303,0.025,0.372,0.073,1.989,bottom
+0.303,0.039,0.372,0.073,1.989,bottom
+0.303,0.039,0.372,0.073,1.997,bottom
+0.302,0.039,0.372,0.073,1.997,bottom
+0.302,0.039,0.372,0.077,1.997,bottom
+0.302,0.039,0.374,0.077,1.997,bottom
+0.302,0.040,0.374,0.077,1.997,bottom
+0.302,0.040,0.374,0.077,1.986,bottom
+0.303,0.040,0.374,0.077,1.986,bottom
+0.303,0.040,0.374,0.077,1.986,bottom
+0.303,0.040,0.375,0.077,1.986,bottom
+0.303,0.037,0.375,0.077,1.986,bottom
+0.303,0.037,0.375,0.077,1.969,bottom
+0.301,0.037,0.375,0.077,1.969,bottom
+0.301,0.037,0.375,0.078,1.969,bottom
+0.301,0.037,0.375,0.078,1.969,bottom
+0.301,0.037,0.375,0.078,1.969,bottom
+0.301,0.037,0.375,0.078,1.977,bottom
+0.299,0.037,0.375,0.078,1.977,bottom
+0.299,0.037,0.375,0.078,1.977,bottom
+0.299,0.037,0.373,0.078,1.977,bottom
+0.299,0.039,0.373,0.078,1.977,bottom
+0.299,0.039,0.373,0.078,1.972,bottom
+0.302,0.039,0.373,0.078,1.972,bottom
+0.302,0.039,0.373,0.079,1.972,bottom
+0.302,0.039,0.374,0.079,1.972,bottom
+0.302,0.040,0.374,0.079,1.972,bottom
+0.302,0.040,0.374,0.079,1.986,bottom
+0.306,0.040,0.374,0.079,1.986,bottom
+0.306,0.040,0.374,0.078,1.986,bottom
+0.306,0.040,0.374,0.078,1.986,bottom
+0.306,0.039,0.374,0.078,1.986,bottom
+0.306,0.039,0.374,0.078,1.978,bottom
+0.303,0.039,0.374,0.078,1.978,bottom
+0.303,0.039,0.374,0.076,1.978,bottom
+0.303,0.039,0.372,0.076,1.978,bottom
+0.303,0.039,0.372,0.076,1.978,bottom
+0.303,0.039,0.372,0.076,1.985,bottom
+0.303,0.039,0.372,0.076,1.985,bottom
+0.303,0.039,0.372,0.074,1.985,bottom
+0.303,0.039,0.374,0.074,1.985,bottom
+0.303,0.037,0.374,0.074,1.985,bottom
+0.303,0.037,0.374,0.074,1.964,bottom
+0.300,0.037,0.374,0.074,1.964,bottom
+0.300,0.037,0.374,0.079,1.964,bottom
+0.300,0.037,0.371,0.079,1.964,bottom
+0.300,0.037,0.371,0.079,1.964,bottom
+0.300,0.037,0.371,0.079,1.977,bottom
+0.301,0.037,0.371,0.079,1.977,bottom
+0.301,0.037,0.371,0.078,1.977,bottom
+0.301,0.037,0.374,0.078,1.977,bottom
+0.301,0.038,0.374,0.078,1.977,bottom
+0.301,0.038,0.374,0.078,1.989,bottom
+0.304,0.038,0.374,0.078,1.989,bottom
+0.304,0.038,0.374,0.075,1.989,bottom
+0.304,0.038,0.374,0.075,1.989,bottom
+0.304,0.038,0.374,0.075,1.989,bottom
+0.304,0.038,0.374,0.075,1.990,bottom
+0.304,0.038,0.374,0.075,1.990,bottom
+0.304,0.038,0.374,0.076,1.990,bottom
+0.304,0.038,0.373,0.076,1.990,bottom
+0.304,0.039,0.373,0.076,1.990,bottom
+0.304,0.039,0.373,0.076,1.978,bottom
+0.304,0.039,0.373,0.076,1.978,bottom
+0.304,0.039,0.373,0.076,1.978,bottom
+0.304,0.038,0.373,0.076,1.978,bottom
+0.304,0.038,0.373,0.076,1.978,bottom
+0.304,0.038,0.373,0.076,1.982,bottom
+0.303,0.038,0.373,0.076,1.982,bottom
+0.303,0.038,0.373,0.076,1.982,bottom
+0.303,0.037,0.373,0.076,1.982,bottom
+0.303,0.037,0.373,0.076,1.982,bottom
+0.303,0.037,0.373,0.076,1.998,bottom
+0.305,0.037,0.373,0.076,1.998,bottom
+0.305,0.037,0.373,0.076,1.998,bottom
+0.305,0.039,0.373,0.076,1.998,bottom
+0.305,0.039,0.374,0.076,1.998,bottom
+0.305,0.039,0.374,0.076,1.984,bottom
+0.302,0.039,0.374,0.076,1.984,bottom
+0.302,0.039,0.374,0.077,1.984,bottom
+0.302,0.039,0.374,0.077,1.984,bottom
+0.302,0.039,0.373,0.077,1.984,bottom
+0.302,0.039,0.373,0.077,1.976,bottom
+0.304,0.039,0.373,0.077,1.976,bottom
+0.304,0.039,0.373,0.077,1.976,bottom
+0.304,0.039,0.373,0.077,1.976,bottom
diff --git a/lf-drone/simulation/data/tof_20260211_133454.csv b/lf-drone/simulation/data/tof_20260211_133454.csv
new file mode 100644
index 0000000..4675b79
--- /dev/null
+++ b/lf-drone/simulation/data/tof_20260211_133454.csv
@@ -0,0 +1,226 @@
+front_m,bottom_m,left_m,right_m,top_m,obstacle_within_0p20m_direction
+0.597,0.008,0.505,0.635,0.929,bottom
+0.597,0.008,0.505,0.635,1.038,bottom
+0.597,0.008,0.506,0.635,1.038,bottom
+0.609,0.008,0.506,0.635,1.038,bottom
+0.609,0.008,0.506,0.632,1.038,bottom
+0.609,0.051,0.506,0.632,1.038,bottom
+0.609,0.051,0.506,0.632,1.047,bottom
+0.614,0.051,0.506,0.632,1.047,bottom
+0.614,0.051,0.504,0.632,1.047,bottom
+0.614,0.051,0.504,0.633,1.047,bottom
+0.614,0.053,0.504,0.633,1.047,bottom
+0.614,0.053,0.504,0.633,1.182,bottom
+0.383,0.053,0.504,0.633,1.182,bottom
+0.383,0.053,0.510,0.633,1.182,bottom
+0.383,0.053,0.510,0.631,1.182,bottom
+0.383,0.059,0.510,0.631,1.182,bottom
+0.383,0.059,0.510,0.631,1.093,bottom
+0.618,0.059,0.510,0.631,1.093,bottom
+0.618,0.059,0.509,0.631,1.093,bottom
+0.618,0.054,0.509,0.631,1.093,bottom
+0.618,0.054,0.509,0.632,1.093,bottom
+0.618,0.054,0.509,0.632,0.675,bottom
+0.746,0.054,0.509,0.632,0.675,bottom
+0.746,0.121,0.509,0.632,0.675,bottom
+0.746,0.121,0.581,0.632,0.675,bottom
+0.746,0.121,0.581,0.625,0.675,bottom
+0.746,0.121,0.581,0.625,1.010,bottom
+0.710,0.121,0.581,0.625,1.010,bottom
+0.710,0.121,0.581,0.682,1.010,bottom
+0.710,0.280,0.581,0.682,1.010,none
+0.710,0.280,0.547,0.682,1.010,none
+0.710,0.280,0.547,0.682,0.734,none
+0.785,0.280,0.547,0.682,0.734,none
+0.785,0.280,0.547,0.652,0.734,none
+0.785,0.585,0.547,0.652,0.734,none
+0.785,0.585,0.500,0.652,0.734,none
+0.785,0.585,0.500,0.652,0.538,none
+0.726,0.585,0.500,0.652,0.538,none
+0.726,0.585,0.500,0.627,0.538,none
+0.726,0.791,0.500,0.627,0.538,none
+0.726,0.791,0.396,0.627,0.538,none
+0.726,0.791,0.396,0.627,0.315,none
+0.272,0.791,0.396,0.627,0.315,none
+0.272,0.999,0.396,0.627,0.315,none
+0.272,0.999,0.396,0.555,0.315,none
+0.272,0.999,0.356,0.555,0.315,none
+0.272,0.999,0.356,0.555,0.218,none
+0.192,0.999,0.356,0.555,0.218,front
+0.192,0.961,0.356,0.555,0.218,front
+0.192,0.961,0.356,0.540,0.218,front
+0.192,0.961,0.285,0.540,0.218,front
+0.192,0.961,0.285,0.540,0.294,front
+0.278,0.961,0.285,0.540,0.294,none
+0.278,0.922,0.285,0.540,0.294,none
+0.278,0.922,0.285,0.472,0.294,none
+0.278,0.922,0.414,0.472,0.294,none
+0.278,0.922,0.414,0.472,0.278,none
+0.278,0.939,0.414,0.472,0.278,none
+0.289,0.939,0.414,0.472,0.278,none
+0.289,0.939,0.414,0.283,0.278,none
+0.289,0.939,0.531,0.283,0.278,none
+0.289,1.148,0.531,0.283,0.278,none
+0.289,1.148,0.531,0.283,0.223,none
+0.282,1.148,0.531,0.283,0.223,none
+0.282,1.148,0.531,0.221,0.223,none
+0.282,1.148,0.543,0.221,0.223,none
+0.282,1.114,0.543,0.221,0.223,none
+0.282,1.114,0.543,0.221,0.252,none
+0.265,1.114,0.543,0.221,0.252,none
+0.265,1.114,0.543,0.291,0.252,none
+0.265,1.114,0.473,0.291,0.252,none
+0.265,1.080,0.473,0.291,0.252,none
+0.265,1.080,0.473,0.291,0.287,none
+0.226,1.080,0.473,0.291,0.287,none
+0.226,1.080,0.473,0.514,0.287,none
+0.226,1.080,0.343,0.514,0.287,none
+0.226,1.076,0.343,0.514,0.287,none
+0.226,1.076,0.343,0.514,0.293,none
+0.212,1.076,0.343,0.514,0.293,none
+0.212,1.076,0.343,0.640,0.293,none
+0.212,1.076,0.207,0.640,0.293,none
+0.212,1.104,0.207,0.640,0.293,none
+0.212,1.104,0.207,0.640,0.281,none
+0.197,1.104,0.207,0.640,0.281,front
+0.197,1.104,0.207,0.734,0.281,front
+0.197,1.104,0.114,0.734,0.281,left
+0.197,1.093,0.114,0.734,0.281,left
+0.197,1.093,0.114,0.734,0.286,left
+0.205,1.093,0.114,0.734,0.286,left
+0.205,1.093,0.114,0.735,0.286,left
+0.205,1.093,0.184,0.735,0.286,left
+0.205,1.036,0.184,0.735,0.286,left
+0.205,1.036,0.184,0.735,0.228,left
+0.118,1.036,0.184,0.735,0.228,front
+0.118,1.036,0.184,0.616,0.228,front
+0.118,1.036,0.266,0.616,0.228,front
+0.118,1.029,0.266,0.616,0.228,front
+0.118,1.029,0.266,0.616,0.253,front
+0.023,1.029,0.266,0.616,0.253,front
+0.023,1.029,0.266,0.594,0.253,front
+0.023,1.029,0.365,0.594,0.253,front
+0.023,0.914,0.365,0.594,0.253,front
+0.109,0.914,0.365,0.594,0.253,front
+0.109,0.914,0.365,0.594,0.357,front
+0.109,0.914,0.365,0.655,0.357,front
+0.109,0.914,0.418,0.655,0.357,front
+0.109,0.822,0.418,0.655,0.357,front
+0.228,0.822,0.418,0.655,0.357,none
+0.228,0.822,0.418,0.655,0.299,none
+0.228,0.822,0.418,0.697,0.299,none
+0.228,0.822,0.429,0.697,0.299,none
+0.228,0.831,0.429,0.697,0.299,none
+0.194,0.831,0.429,0.697,0.299,front
+0.194,0.831,0.429,0.697,0.375,front
+0.194,0.831,0.429,0.563,0.375,front
+0.194,0.831,0.371,0.563,0.375,front
+0.194,0.858,0.371,0.563,0.375,front
+0.135,0.858,0.371,0.563,0.375,front
+0.135,0.858,0.371,0.563,0.397,front
+0.135,0.858,0.371,0.272,0.397,front
+0.135,0.858,0.713,0.272,0.397,front
+0.135,0.836,0.713,0.272,0.397,front
+0.086,0.836,0.713,0.272,0.397,front
+0.086,0.836,0.713,0.272,0.431,front
+0.086,0.836,0.713,0.544,0.431,front
+0.086,0.836,0.425,0.544,0.431,front
+0.086,0.831,0.425,0.544,0.431,front
+0.137,0.831,0.425,0.544,0.431,front
+0.137,0.831,0.425,0.544,0.415,front
+0.137,0.831,0.425,0.495,0.415,front
+0.137,0.831,0.231,0.495,0.415,front
+0.137,0.789,0.231,0.495,0.415,front
+0.230,0.789,0.231,0.495,0.415,none
+0.230,0.789,0.231,0.495,0.424,none
+0.230,0.789,0.231,0.247,0.424,none
+0.230,0.789,0.323,0.247,0.424,none
+0.230,0.783,0.323,0.247,0.424,none
+0.078,0.783,0.323,0.247,0.424,front
+0.078,0.783,0.323,0.247,0.385,front
+0.078,0.783,0.323,0.437,0.385,front
+0.078,0.783,0.664,0.437,0.385,front
+0.078,0.801,0.664,0.437,0.385,front
+0.041,0.801,0.664,0.437,0.385,front
+0.041,0.801,0.664,0.437,0.331,front
+0.041,0.801,0.664,0.632,0.331,front
+0.041,0.801,0.454,0.632,0.331,front
+0.041,0.819,0.454,0.632,0.331,front
+0.163,0.819,0.454,0.632,0.331,front
+0.163,0.819,0.454,0.632,0.423,front
+0.163,0.819,0.454,0.666,0.423,front
+0.163,0.819,0.249,0.666,0.423,front
+0.163,0.920,0.249,0.666,0.423,front
+0.251,0.920,0.249,0.666,0.423,none
+0.251,0.920,0.249,0.666,0.323,none
+0.251,0.920,0.249,0.650,0.323,none
+0.251,0.920,0.394,0.650,0.323,none
+0.251,1.066,0.394,0.650,0.323,none
+0.217,1.066,0.394,0.650,0.323,none
+0.217,1.066,0.394,0.650,0.190,top
+0.217,1.066,0.394,0.585,0.190,top
+0.217,1.066,0.294,0.585,0.190,top
+0.217,1.187,0.294,0.585,0.190,top
+0.103,1.187,0.294,0.585,0.190,front
+0.103,1.187,0.294,0.585,0.074,top
+0.103,1.187,0.294,0.386,0.074,top
+0.103,1.187,0.317,0.386,0.074,top
+0.103,1.165,0.317,0.386,0.074,top
+0.150,1.165,0.317,0.386,0.074,top
+0.150,1.165,0.317,0.386,0.154,front
+0.150,1.165,0.317,0.459,0.154,front
+0.150,1.165,0.504,0.459,0.154,front
+0.150,0.988,0.504,0.459,0.154,front
+0.202,0.988,0.504,0.459,0.154,top
+0.202,0.988,0.504,0.459,0.351,none
+0.202,0.988,0.504,0.580,0.351,none
+0.202,0.988,0.462,0.580,0.351,none
+0.202,0.833,0.462,0.580,0.351,none
+0.253,0.833,0.462,0.580,0.351,none
+0.253,0.833,0.462,0.580,0.457,none
+0.253,0.833,0.462,0.490,0.457,none
+0.253,0.833,0.379,0.490,0.457,none
+0.253,0.776,0.379,0.490,0.457,none
+0.270,0.776,0.379,0.490,0.457,none
+0.270,0.776,0.379,0.490,0.411,none
+0.270,0.776,0.379,0.373,0.411,none
+0.270,0.776,0.732,0.373,0.411,none
+0.270,0.760,0.732,0.373,0.411,none
+0.127,0.760,0.732,0.373,0.411,front
+0.127,0.760,0.732,0.373,0.415,front
+0.127,0.760,0.732,0.582,0.415,front
+0.127,0.760,0.483,0.582,0.415,front
+0.127,0.739,0.483,0.582,0.415,front
+0.191,0.739,0.483,0.582,0.415,front
+0.191,0.739,0.483,0.582,0.346,front
+0.191,0.739,0.483,0.375,0.346,front
+0.191,0.739,0.450,0.375,0.346,front
+0.191,0.709,0.450,0.375,0.346,front
+0.311,0.709,0.450,0.375,0.346,none
+0.311,0.709,0.450,0.375,0.401,none
+0.311,0.709,0.450,0.427,0.401,none
+0.311,0.709,0.381,0.427,0.401,none
+0.311,0.660,0.381,0.427,0.401,none
+0.292,0.660,0.381,0.427,0.401,none
+0.292,0.660,0.381,0.427,0.462,none
+0.292,0.660,0.381,0.576,0.462,none
+0.292,0.660,0.597,0.576,0.462,none
+0.292,0.574,0.597,0.576,0.462,none
+0.403,0.574,0.597,0.576,0.462,none
+0.403,0.574,0.597,0.576,0.656,none
+0.403,0.574,0.597,0.700,0.656,none
+0.403,0.574,0.529,0.700,0.656,none
+0.403,0.189,0.529,0.700,0.656,bottom
+0.628,0.189,0.529,0.700,0.656,bottom
+0.628,0.189,0.529,0.700,0.677,bottom
+0.628,0.189,0.529,0.709,0.677,bottom
+0.628,0.189,0.459,0.709,0.677,bottom
+0.628,0.015,0.459,0.709,0.677,bottom
+0.560,0.015,0.459,0.709,0.677,bottom
+0.560,0.015,0.459,0.709,0.746,bottom
+0.560,0.015,0.459,0.679,0.746,bottom
+0.560,0.015,0.478,0.679,0.746,bottom
+0.560,0.048,0.478,0.679,0.746,bottom
+0.561,0.048,0.478,0.679,0.746,bottom
+0.561,0.048,0.478,0.679,0.861,bottom
+0.561,0.048,0.478,0.678,0.861,bottom
diff --git a/lf-drone/simulation/data/tof_test.csv b/lf-drone/simulation/data/tof_test.csv
new file mode 100644
index 0000000..3eb7638
--- /dev/null
+++ b/lf-drone/simulation/data/tof_test.csv
@@ -0,0 +1,19 @@
+front_m,bottom_m,left_m,right_m,top_m,obstacle_within_0p20m_direction
+0.597,0.008,0.505,0.635,0.929,bottom
+0.597,0.008,0.505,0.635,1.038,bottom
+0.597,0.008,0.506,0.635,1.038,bottom
+0.785,0.585,0.500,0.652,0.734,none
+0.785,0.585,0.500,0.652,0.538,none
+0.726,0.585,0.500,0.652,0.538,none
+0.192,0.961,0.356,0.540,0.218,front
+0.192,0.961,0.285,0.540,0.218,front
+0.192,0.961,0.285,0.540,0.294,front
+0.205,1.093,0.184,0.735,0.286,left
+0.205,1.036,0.184,0.735,0.286,left
+0.205,1.036,0.184,0.735,0.228,left
+0.217,1.066,0.394,0.585,0.190,top
+0.217,1.066,0.294,0.585,0.190,top
+0.217,1.187,0.294,0.585,0.190,top
+0.561,0.048,0.478,0.679,0.746,bottom
+0.561,0.048,0.478,0.679,0.861,bottom
+0.561,0.048,0.478,0.678,0.861,bottom
\ No newline at end of file
diff --git a/lf-drone/simulation/data/top.csv b/lf-drone/simulation/data/top.csv
new file mode 100644
index 0000000..3da7240
--- /dev/null
+++ b/lf-drone/simulation/data/top.csv
@@ -0,0 +1,226 @@
+value_m
+0.929
+1.038
+1.038
+1.038
+1.038
+1.038
+1.047
+1.047
+1.047
+1.047
+1.047
+1.182
+1.182
+1.182
+1.182
+1.182
+1.093
+1.093
+1.093
+1.093
+1.093
+0.675
+0.675
+0.675
+0.675
+0.675
+1.010
+1.010
+1.010
+1.010
+1.010
+0.734
+0.734
+0.734
+0.734
+0.734
+0.538
+0.538
+0.538
+0.538
+0.538
+0.315
+0.315
+0.315
+0.315
+0.315
+0.218
+0.218
+0.218
+0.218
+0.218
+0.294
+0.294
+0.294
+0.294
+0.294
+0.278
+0.278
+0.278
+0.278
+0.278
+0.278
+0.223
+0.223
+0.223
+0.223
+0.223
+0.252
+0.252
+0.252
+0.252
+0.252
+0.287
+0.287
+0.287
+0.287
+0.287
+0.293
+0.293
+0.293
+0.293
+0.293
+0.281
+0.281
+0.281
+0.281
+0.281
+0.286
+0.286
+0.286
+0.286
+0.286
+0.228
+0.228
+0.228
+0.228
+0.228
+0.253
+0.253
+0.253
+0.253
+0.253
+0.253
+0.357
+0.357
+0.357
+0.357
+0.357
+0.299
+0.299
+0.299
+0.299
+0.299
+0.375
+0.375
+0.375
+0.375
+0.375
+0.397
+0.397
+0.397
+0.397
+0.397
+0.431
+0.431
+0.431
+0.431
+0.431
+0.415
+0.415
+0.415
+0.415
+0.415
+0.424
+0.424
+0.424
+0.424
+0.424
+0.385
+0.385
+0.385
+0.385
+0.385
+0.331
+0.331
+0.331
+0.331
+0.331
+0.423
+0.423
+0.423
+0.423
+0.423
+0.323
+0.323
+0.323
+0.323
+0.323
+0.190
+0.190
+0.190
+0.190
+0.190
+0.074
+0.074
+0.074
+0.074
+0.074
+0.154
+0.154
+0.154
+0.154
+0.154
+0.351
+0.351
+0.351
+0.351
+0.351
+0.457
+0.457
+0.457
+0.457
+0.457
+0.411
+0.411
+0.411
+0.411
+0.411
+0.415
+0.415
+0.415
+0.415
+0.415
+0.346
+0.346
+0.346
+0.346
+0.346
+0.401
+0.401
+0.401
+0.401
+0.401
+0.462
+0.462
+0.462
+0.462
+0.462
+0.656
+0.656
+0.656
+0.656
+0.656
+0.677
+0.677
+0.677
+0.677
+0.677
+0.746
+0.746
+0.746
+0.746
+0.746
+0.861
+0.861
diff --git a/lf-drone/simulation/results/README.md b/lf-drone/simulation/results/README.md
new file mode 100644
index 0000000..34c5b7f
--- /dev/null
+++ b/lf-drone/simulation/results/README.md
@@ -0,0 +1,37 @@
+# Overview
+
+This directory stores output logs and plots produced by the simulation workflow. It also contains the plotting script used to reconstruct an approximate drone path from RC outputs and recorded ToF sensor data.
+
+## Contents
+
+- `plot_drone_path.py`: generates a path-and-obstacle PDF from an RC log and a ToF data directory
+- `rc-out.csv`, `rc-out-2.csv`, `rc-out-3.csv`: example RC output logs
+- `*.pdf`: example generated plots
+
+# Prerequisites
+
+See the repository root [README.md](../../README.md) for the required Python dependencies.
+
+### To Run the Code
+
+Run the plotting script with explicit input paths:
+
+```bash
+python3 simulation/results/plot_drone_path.py simulation/results/rc-out-3.csv simulation/data
+```
+
+You can also run it with no arguments. In that case it defaults to:
+
+- RC log: `simulation/results/rc-out.csv`
+- ToF data directory: `simulation/data/`
+
+```bash
+python3 simulation/results/plot_drone_path.py
+```
+
+### Additional Instructions
+
+- Make sure the RC log and the ToF CSV files come from the same run so that the samples line up correctly.
+- The plotting script reads `front`, `left`, `right`, `top`, and `bottom` sensor CSV files from the selected data directory.
+- The script writes a PDF next to the selected RC log using the pattern `<rc-log-name>-path-obstacles.pdf`.
+- If you change the log filename in `simulation/src/test.lf`, pass the new path explicitly to `plot_drone_path.py`.
\ No newline at end of file
diff --git a/lf-drone/simulation/results/plot_drone_path.py b/lf-drone/simulation/results/plot_drone_path.py
new file mode 100644
index 0000000..ca5db0d
--- /dev/null
+++ b/lf-drone/simulation/results/plot_drone_path.py
@@ -0,0 +1,438 @@
+#!/usr/bin/env python3
+import csv
+import math
+import sys
+from pathlib import Path
+
+import matplotlib
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+MID = 1500.0
+SPAN = 500.0
+
+SENSOR_NAMES = ("front", "left", "right", "top", "bottom")
+
+
+def clamp(x, lo, hi):
+    return lo if x < lo else hi if x > hi else x
+
+
+def rc_to_unit(pwm):
+    return clamp((pwm - MID) / SPAN, -1.0, 1.0)
+
+
+def parse_float(text):
+    try:
+        return float(str(text).strip())
+    except (TypeError, ValueError):
+        return None
+
+
+def robust_deadband(pwm_series):
+    x = np.asarray(pwm_series, dtype=float)
+    x = x[np.isfinite(x)]
+    if len(x) == 0:
+        return 12
+
+    dev = np.abs(x - MID)
+    near = dev[dev < 80]
+    if len(near) < 20:
+        return 12
+
+    mad = np.median(np.abs(near - np.median(near)))
+    return int(clamp(3.0 * mad, 6, 30))
+
+
+def infer_dt_from_step(step):
+    step = np.asarray(step, dtype=float)
+    if len(step) < 2:
+        return np.full(len(step), 0.02, dtype=float)
+
+    dstep = np.diff(step)
+    bad = (~np.isfinite(dstep)) | (dstep <= 0)
+    dstep[bad] = 1.0
+
+    dt = 0.02 * dstep
+    dt = np.concatenate([[dt[0]], dt])
+    return dt
+
+
+def auto_scales(pitch_u, roll_u, yaw_u):
+    mag_xy = np.sqrt(pitch_u ** 2 + roll_u ** 2)
+    mag_xy = mag_xy[np.isfinite(mag_xy)]
+
+    yaw_mag = np.abs(yaw_u)
+    yaw_mag = yaw_mag[np.isfinite(yaw_mag)]
+
+    p95_xy = np.percentile(mag_xy, 95) if len(mag_xy) else 0.0
+    p95_yaw = np.percentile(yaw_mag, 95) if len(yaw_mag) else 0.0
+
+    default_vmax = 1.2
+    default_yaw = math.radians(120)
+
+    if p95_xy < 0.05:
+        vmax = default_vmax
+    else:
+        vmax = float(clamp(default_vmax / p95_xy, 0.6, 3.0))
+
+    if p95_yaw < 0.05:
+        yawrate = default_yaw
+    else:
+        yawrate = float(
+            clamp(default_yaw / p95_yaw, math.radians(40), math.radians(240))
+        )
+
+    return vmax, yawrate
+
+
+def read_rc_csv(path):
+    required = ["step", "roll", "pitch", "yaw", "throttle", "aux1", "aux2"]
+    data = {name: [] for name in required}
+
+    with path.open("r", newline="", encoding="utf-8") as f:
+        reader = csv.DictReader(f)
+        if reader.fieldnames is None:
+            raise ValueError(f"Empty or headerless RC CSV: {path}")
+
+        missing = [name for name in required if name not in reader.fieldnames]
+        if missing:
+            raise ValueError(
+                f"rc_out missing columns {missing}. Found: {reader.fieldnames}"
+            )
+
+        for row in reader:
+            for name in required:
+                value = parse_float(row.get(name, ""))
+                data[name].append(np.nan if value is None else value)
+
+    return {name: np.asarray(values, dtype=float) for name, values in data.items()}
+
+
+def read_single_sensor_csv(path, sensor_name):
+    values = []
+    header_index = None
+    started = False
+
+    preferred_names = [
+        f"{sensor_name}_m",
+        "value_m",
+        "distance_m",
+        "range_m",
+        "tof_m",
+        sensor_name,
+        "value",
+        "distance",
+        "range",
+        "tof",
+    ]
+
+    with path.open("r", newline="", encoding="utf-8") as f:
+        reader = csv.reader(f)
+        for row in reader:
+            cells = [cell.strip() for cell in row]
+
+            if not cells or all(cell == "" for cell in cells):
+                if started:
+                    values.append(np.nan)
+                continue
+
+            if cells[0].startswith("#"):
+                continue
+
+            if not started:
+                lowered = [cell.lower() for cell in cells]
+                has_header_text = any(any(ch.isalpha() for ch in cell) for cell in lowered)
+
+                if has_header_text:
+                    for name in preferred_names:
+                        if name in lowered:
+                            header_index = lowered.index(name)
+                            break
+
+                    if header_index is None:
+                        header_index = len(cells) - 1
+
+                    started = True
+                    continue
+
+                started = True
+
+            value = None
+
+            if header_index is not None and header_index < len(cells):
+                value = parse_float(cells[header_index])
+
+            if value is None:
+                for cell in reversed(cells):
+                    value = parse_float(cell)
+                    if value is not None:
+                        break
+
+            values.append(np.nan if value is None else value)
+
+    return np.asarray(values, dtype=float)
+
+
+def load_tof_inputs(data_dir):
+    tof = {}
+    for name in SENSOR_NAMES:
+        path = data_dir / f"{name}.csv"
+        if not path.exists():
+            raise FileNotFoundError(f"Missing ToF CSV for '{name}': {path}")
+        tof[name] = read_single_sensor_csv(path, name)
+    return tof
+
+
+def integrate_from_rc(rc):
+    step = rc["step"]
+    dt = infer_dt_from_step(step)
+
+    roll = rc["roll"].astype(float).copy()
+    pitch = rc["pitch"].astype(float).copy()
+    yaw = rc["yaw"].astype(float).copy()
+
+    roll[~np.isfinite(roll)] = MID
+    pitch[~np.isfinite(pitch)] = MID
+    yaw[~np.isfinite(yaw)] = MID
+
+    db_r = robust_deadband(roll)
+    db_p = robust_deadband(pitch)
+    db_y = robust_deadband(yaw)
+
+    def apply_db(x, db):
+        x = x.copy()
+        x[np.abs(x - MID) < db] = MID
+        return x
+
+    roll = apply_db(roll, db_r)
+    pitch = apply_db(pitch, db_p)
+    yaw = apply_db(yaw, db_y)
+
+    roll_u = np.array([rc_to_unit(v) for v in roll], dtype=float)
+    pitch_u = np.array([rc_to_unit(v) for v in pitch], dtype=float)
+    yaw_u = np.array([rc_to_unit(v) for v in yaw], dtype=float)
+
+    vmax, yawrate_scale = auto_scales(pitch_u, roll_u, yaw_u)
+
+    v_des_fwd = pitch_u * vmax
+    v_des_right = roll_u * vmax
+
+    tau_v = 0.35
+    tau_y = 0.25
+
+    n = len(step)
+    x = np.zeros(n, dtype=float)
+    y = np.zeros(n, dtype=float)
+    psi = np.zeros(n, dtype=float)
+
+    v_fwd = 0.0
+    v_right = 0.0
+    yawrate = 0.0
+
+    for i in range(1, n):
+        dti = float(dt[i])
+
+        yawrate_des = yaw_u[i - 1] * yawrate_scale
+        alpha_v = 1.0 - math.exp(-dti / tau_v)
+        alpha_y = 1.0 - math.exp(-dti / tau_y)
+
+        v_fwd += alpha_v * (v_des_fwd[i - 1] - v_fwd)
+        v_right += alpha_v * (v_des_right[i - 1] - v_right)
+        yawrate += alpha_y * (yawrate_des - yawrate)
+
+        psi[i] = psi[i - 1] + yawrate * dti
+
+        c = math.cos(psi[i])
+        s = math.sin(psi[i])
+
+        vx = c * v_fwd - s * v_right
+        vy = s * v_fwd + c * v_right
+
+        x[i] = x[i - 1] + vx * dti
+        y[i] = y[i - 1] + vy * dti
+
+    return x, y, psi
+
+
+def point_to_segment_distance(px, py, ax, ay, bx, by):
+    abx = bx - ax
+    aby = by - ay
+    apx = px - ax
+    apy = py - ay
+
+    denom = abx * abx + aby * aby
+    if denom <= 1e-12:
+        return math.hypot(px - ax, py - ay)
+
+    t = (apx * abx + apy * aby) / denom
+    t = 0.0 if t < 0.0 else 1.0 if t > 1.0 else t
+
+    cx = ax + t * abx
+    cy = ay + t * aby
+    return math.hypot(px - cx, py - cy)
+
+
+def point_to_polyline_distance(px, py, x, y):
+    best = float("inf")
+    for i in range(1, len(x)):
+        d = point_to_segment_distance(px, py, x[i - 1], y[i - 1], x[i], y[i])
+        if d < best:
+            best = d
+    return best
+
+
+def resolve_paths(argv):
+    results_dir = Path(__file__).resolve().parent
+    simulation_dir = results_dir.parent
+
+    default_rc = results_dir / "rc-out.csv"
+    default_data = simulation_dir / "data"
+
+    if len(argv) == 1:
+        return default_rc, default_data
+
+    if len(argv) == 3:
+        return Path(argv[1]).resolve(), Path(argv[2]).resolve()
+
+    prog = Path(argv[0]).name
+    raise SystemExit(f"Usage: python3 {prog} [rc-out.csv tof-data-dir]")
+
+
+def main():
+    rc_path, data_dir = resolve_paths(sys.argv)
+
+    if not rc_path.exists():
+        raise FileNotFoundError(f"Missing RC log: {rc_path}")
+    if not data_dir.exists():
+        raise FileNotFoundError(f"Missing ToF data directory: {data_dir}")
+
+    rc = read_rc_csv(rc_path)
+    tof = load_tof_inputs(data_dir)
+
+    lengths = [len(rc["step"])] + [len(values) for values in tof.values()]
+    n = min(lengths)
+    if n == 0:
+        raise ValueError("No aligned samples found in rc-out.csv / ToF CSVs")
+
+    rc = {name: values[:n] for name, values in rc.items()}
+    tof = {name: values[:n] for name, values in tof.items()}
+
+    x, y, psi = integrate_from_rc(rc)
+
+    x = x - x[0]
+    y = y - y[0]
+
+    f = tof["front"]
+    l = tof["left"]
+    r = tof["right"]
+    t = tof["top"]
+    b = tof["bottom"]
+
+    obs_thresh = 0.30
+    top_thresh = 0.30
+    cruise_bottom = 0.30
+
+    cap_dist = 0.05
+    max_path_dist = 0.20
+    min_clearance = 0.003
+
+    in_cruise = np.isfinite(b) & (b > cruise_bottom)
+
+    hit_front = in_cruise & np.isfinite(f) & (f < obs_thresh)
+    hit_left = in_cruise & np.isfinite(l) & (l < obs_thresh)
+    hit_right = in_cruise & np.isfinite(r) & (r < obs_thresh)
+    hit_top = in_cruise & np.isfinite(t) & (t < top_thresh)
+
+    hit_any_xy = hit_front | hit_left | hit_right
+
+    ox, oy = [], []
+    for i in np.where(hit_any_xy)[0]:
+        candidates = []
+
+        if hit_front[i]:
+            candidates.append(("front", float(f[i])))
+        if hit_left[i]:
+            candidates.append(("left", float(l[i])))
+        if hit_right[i]:
+            candidates.append(("right", float(r[i])))
+
+        if not candidates:
+            continue
+
+        direction, dist = min(candidates, key=lambda item: item[1])
+        d = clamp(dist, 0.01, cap_dist)
+
+        if direction == "front":
+            bf, br = 1.0, 0.0
+        elif direction == "left":
+            bf, br = 0.0, -1.0
+        else:
+            bf, br = 0.0, 1.0
+
+        c = math.cos(psi[i])
+        s = math.sin(psi[i])
+
+        dx = c * (bf * d) - s * (br * d)
+        dy = s * (bf * d) + c * (br * d)
+
+        px = float(x[i] + dx)
+        py = float(y[i] + dy)
+
+        dist_to_path = point_to_polyline_distance(px, py, x, y)
+        if min_clearance <= dist_to_path <= max_path_dist:
+            ox.append(px)
+            oy.append(py)
+
+    ox = np.asarray(ox, dtype=float)
+    oy = np.asarray(oy, dtype=float)
+
+    print(f"Loaded {n} aligned samples")
+    print(
+        "Obstacle hits: "
+        f"front={int(np.sum(hit_front))} "
+        f"left={int(np.sum(hit_left))} "
+        f"right={int(np.sum(hit_right))} "
+        f"top={int(np.sum(hit_top))}"
+    )
+    if int(np.sum(hit_top)) > 0:
+        print("Note: top.csv is read and counted, but top obstacles are not projected onto the 2D X-Y path plot.")
+
+    fig, ax = plt.subplots(figsize=(8, 8))
+    ax.plot(x, y, linewidth=1.8, label="Trajectory")
+    ax.scatter([0.0], [0.0], marker="o", label="Start")
+    ax.scatter([x[-1]], [y[-1]], marker="x", label="End")
+
+    if len(ox):
+        ax.scatter(ox, oy, marker="s", s=45, label="Obstacles")
+    else:
+        print("No obstacle points survived filtering. Try increasing max_path_dist or cap_dist.")
+
+    ax.set_aspect("equal", adjustable="box")
+    ax.grid(True, linewidth=0.6, alpha=0.6)
+
+    allx = np.concatenate([x, ox]) if len(ox) else x
+    ally = np.concatenate([y, oy]) if len(oy) else y
+
+    padx = max(0.05, 0.15 * float(np.ptp(allx)))
+    pady = max(0.05, 0.15 * float(np.ptp(ally)))
+
+    ax.set_xlim(float(np.min(allx)) - padx, float(np.max(allx)) + padx)
+    ax.set_ylim(float(np.min(ally)) - pady, float(np.max(ally)) + pady)
+
+    ax.set_xlabel("Forward/Back (m)   (+ forward)")
+    ax.set_ylabel("Right/Left (m)     (+ right)")
+    ax.set_title("Drone Path with Obstacles")
+    ax.legend(loc="best")
+
+    out_pdf = rc_path.with_name(f"{rc_path.stem}-path-obstacles.pdf")
+    plt.savefig(out_pdf, bbox_inches="tight")
+    plt.close(fig)
+
+    print(f"Saved: {out_pdf}")
+
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file
diff --git a/lf-drone/simulation/results/rc-out-path-obstacles.pdf b/lf-drone/simulation/results/rc-out-path-obstacles.pdf
new file mode 100644
index 0000000..e653f8d
Binary files /dev/null and b/lf-drone/simulation/results/rc-out-path-obstacles.pdf differ
diff --git a/lf-drone/simulation/results/rc-out.csv b/lf-drone/simulation/results/rc-out.csv
new file mode 100644
index 0000000..9cdba54
--- /dev/null
+++ b/lf-drone/simulation/results/rc-out.csv
@@ -0,0 +1,228 @@
+step,roll,pitch,yaw,throttle,aux1,aux2
+1,1500,1500,1500,1000,1000,1800
+2,1500,1500,1500,1000,1000,1800
+3,1500,1500,1500,1000,1000,1800
+4,1500,1500,1500,1000,1000,1800
+5,1500,1500,1500,1000,1000,1800
+6,1500,1500,1500,1000,1800,1800
+7,1500,1500,1500,1000,1800,1800
+8,1500,1500,1500,1000,1800,1800
+9,1500,1500,1500,1000,1800,1800
+10,1500,1500,1500,1000,1800,1800
+11,1500,1500,1500,1000,1800,1800
+12,1500,1500,1500,1000,1800,1800
+13,1500,1500,1500,1000,1800,1800
+14,1500,1500,1500,1000,1800,1800
+15,1500,1500,1500,1000,1800,1800
+16,1500,1500,1500,1305,1800,1800
+17,1500,1500,1500,1310,1800,1800
+18,1500,1500,1500,1315,1800,1800
+19,1500,1500,1500,1320,1800,1800
+20,1500,1500,1500,1325,1800,1800
+21,1500,1500,1500,1330,1800,1800
+22,1500,1500,1500,1335,1800,1800
+23,1500,1500,1500,1340,1800,1800
+24,1500,1500,1500,1345,1800,1800
+25,1500,1500,1500,1350,1800,1800
+26,1500,1500,1500,1355,1800,1800
+27,1500,1500,1500,1360,1800,1800
+28,1500,1500,1500,1365,1800,1800
+29,1500,1500,1500,1370,1800,1800
+30,1500,1500,1500,1375,1800,1800
+31,1500,1500,1500,1700,1800,1800
+32,1500,1500,1500,1700,1800,1800
+33,1500,1500,1500,1700,1800,1800
+34,1500,1500,1500,1700,1800,1800
+35,1500,1500,1500,1700,1800,1800
+36,1500,1500,1500,1664,1800,1800
+37,1500,1500,1500,1665,1800,1800
+38,1500,1500,1500,1585,1800,1800
+39,1500,1500,1500,1585,1800,1800
+40,1500,1500,1500,1585,1800,1800
+41,1500,1500,1500,1471,1800,1800
+42,1500,1500,1500,1471,1800,1800
+43,1500,1500,1500,1250,1800,1800
+44,1500,1500,1500,1250,1800,1800
+45,1500,1500,1500,1250,1800,1800
+46,1500,1500,1500,1250,1800,1800
+47,1500,1500,1500,1250,1800,1800
+48,1500,1500,1500,1250,1800,1800
+49,1500,1500,1500,1250,1800,1800
+50,1589,1460,1525,1250,1800,1800
+51,1582,1460,1525,1250,1800,1800
+52,1614,1460,1525,1250,1800,1800
+53,1614,1460,1525,1250,1800,1800
+54,1614,1506,1525,1250,1800,1800
+55,1614,1506,1525,1250,1800,1800
+56,1584,1506,1525,1250,1800,1800
+57,1526,1506,1525,1250,1800,1800
+58,1526,1506,1525,1250,1800,1800
+59,1526,1506,1525,1250,1800,1800
+60,1526,1506,1525,1250,1800,1800
+61,1442,1506,1475,1250,1800,1800
+62,1389,1506,1475,1250,1800,1800
+63,1389,1506,1475,1200,1800,1800
+64,1389,1506,1475,1200,1800,1800
+65,1389,1506,1475,1200,1800,1800
+66,1380,1506,1475,1200,1800,1800
+67,1380,1506,1475,1200,1800,1800
+68,1380,1506,1475,1200,1800,1800
+69,1380,1506,1475,1200,1800,1800
+70,1380,1506,1475,1200,1800,1800
+71,1387,1506,1475,1200,1800,1800
+72,1419,1506,1475,1200,1800,1800
+73,1419,1506,1475,1200,1800,1800
+74,1419,1506,1475,1200,1800,1800
+75,1419,1506,1475,1200,1800,1800
+76,1518,1506,1525,1200,1800,1800
+77,1576,1506,1525,1200,1800,1800
+78,1576,1506,1525,1200,1800,1800
+79,1576,1506,1525,1200,1800,1800
+80,1576,1506,1525,1200,1800,1800
+81,1620,1506,1525,1200,1800,1800
+82,1620,1506,1525,1200,1800,1800
+83,1620,1506,1525,1200,1800,1800
+84,1620,1506,1525,1200,1800,1800
+85,1620,1460,1525,1200,1800,1800
+86,1620,1460,1525,1200,1800,1800
+87,1620,1460,1525,1200,1800,1800
+88,1620,1460,1525,1200,1800,1800
+89,1620,1460,1525,1200,1800,1800
+90,1620,1506,1525,1200,1800,1800
+91,1620,1506,1525,1200,1800,1800
+92,1620,1506,1525,1200,1800,1800
+93,1620,1506,1525,1200,1800,1800
+94,1620,1506,1525,1200,1800,1800
+95,1620,1460,1527,1200,1800,1800
+96,1620,1460,1527,1200,1800,1800
+97,1620,1460,1527,1200,1800,1800
+98,1620,1460,1527,1200,1800,1800
+99,1620,1460,1527,1200,1800,1800
+100,1620,1460,1539,1200,1800,1800
+101,1620,1460,1539,1200,1800,1800
+102,1603,1460,1539,1200,1800,1800
+103,1603,1460,1539,1250,1800,1800
+104,1603,1460,1528,1250,1800,1800
+105,1603,1460,1528,1303,1800,1800
+106,1620,1460,1528,1303,1800,1800
+107,1606,1460,1528,1302,1800,1800
+108,1606,1460,1528,1358,1800,1800
+109,1606,1506,1525,1358,1800,1800
+110,1606,1506,1525,1250,1800,1800
+111,1620,1506,1525,1250,1800,1800
+112,1620,1506,1525,1250,1800,1800
+113,1620,1506,1525,1250,1800,1800
+114,1620,1460,1525,1250,1800,1800
+115,1620,1460,1525,1380,1800,1800
+116,1560,1460,1525,1380,1800,1800
+117,1586,1460,1525,1380,1800,1800
+118,1586,1460,1525,1354,1800,1800
+119,1586,1460,1525,1354,1800,1800
+120,1586,1460,1525,1354,1800,1800
+121,1456,1460,1475,1354,1800,1800
+122,1380,1460,1475,1353,1800,1800
+123,1380,1460,1475,1374,1800,1800
+124,1380,1460,1469,1374,1800,1800
+125,1380,1460,1469,1374,1800,1800
+126,1424,1460,1469,1374,1800,1800
+127,1553,1460,1531,1374,1800,1800
+128,1553,1460,1531,1378,1800,1800
+129,1553,1460,1525,1378,1800,1800
+130,1553,1460,1525,1378,1800,1800
+131,1531,1460,1525,1378,1800,1800
+132,1618,1460,1525,1378,1800,1800
+133,1618,1460,1525,1418,1800,1800
+134,1618,1506,1525,1418,1800,1800
+135,1618,1506,1525,1418,1800,1800
+136,1507,1506,1525,1418,1800,1800
+137,1466,1506,1475,1418,1800,1800
+138,1466,1506,1475,1424,1800,1800
+139,1466,1460,1468,1424,1800,1800
+140,1466,1460,1468,1393,1800,1800
+141,1551,1460,1532,1393,1800,1800
+142,1398,1460,1468,1393,1800,1800
+143,1398,1460,1468,1393,1800,1800
+144,1398,1460,1463,1393,1800,1800
+145,1398,1460,1463,1250,1800,1800
+146,1486,1460,1463,1250,1800,1800
+147,1580,1460,1537,1250,1800,1800
+148,1580,1460,1537,1250,1800,1800
+149,1580,1460,1525,1250,1800,1800
+150,1580,1460,1525,1390,1800,1800
+151,1595,1460,1525,1389,1800,1800
+152,1620,1460,1525,1389,1800,1800
+153,1620,1460,1525,1293,1800,1800
+154,1620,1506,1525,1293,1800,1800
+155,1620,1506,1525,1250,1800,1800
+156,1620,1506,1525,1250,1800,1800
+157,1615,1506,1525,1250,1800,1800
+158,1615,1506,1525,1200,1800,1800
+159,1615,1506,1525,1200,1800,1800
+160,1615,1506,1525,1200,1800,1800
+161,1585,1506,1525,1200,1800,1800
+162,1620,1506,1525,1200,1800,1800
+163,1620,1506,1525,1200,1800,1800
+164,1620,1460,1529,1200,1800,1800
+165,1620,1460,1529,1200,1800,1800
+166,1541,1460,1529,1200,1800,1800
+167,1531,1460,1529,1200,1800,1800
+168,1531,1460,1529,1200,1800,1800
+169,1531,1460,1525,1200,1800,1800
+170,1531,1460,1525,1200,1800,1800
+171,1563,1460,1525,1200,1800,1800
+172,1480,1460,1475,1200,1800,1800
+173,1480,1460,1475,1208,1800,1800
+174,1480,1506,1475,1207,1800,1800
+175,1480,1506,1475,1206,1800,1800
+176,1534,1506,1525,1206,1800,1800
+177,1553,1506,1525,1205,1800,1800
+178,1553,1506,1525,1351,1800,1800
+179,1553,1506,1525,1351,1800,1800
+180,1553,1506,1525,1352,1800,1800
+181,1512,1506,1525,1352,1800,1800
+182,1549,1506,1525,1352,1800,1800
+183,1549,1506,1525,1406,1800,1800
+184,1549,1506,1525,1406,1800,1800
+185,1549,1506,1525,1406,1800,1800
+186,1498,1506,1475,1406,1800,1800
+187,1380,1506,1475,1406,1800,1800
+188,1380,1506,1475,1422,1800,1800
+189,1380,1460,1474,1422,1800,1800
+190,1380,1460,1474,1422,1800,1800
+191,1433,1460,1474,1422,1800,1800
+192,1544,1460,1526,1422,1800,1800
+193,1544,1460,1526,1431,1800,1800
+194,1544,1460,1525,1431,1800,1800
+195,1544,1460,1525,1250,1800,1800
+196,1452,1460,1475,1250,1800,1800
+197,1467,1460,1475,1250,1800,1800
+198,1467,1460,1475,1250,1800,1800
+199,1467,1506,1475,1250,1800,1800
+200,1467,1506,1475,1413,1800,1800
+201,1490,1506,1475,1413,1800,1800
+202,1520,1506,1525,1413,1800,1800
+203,1520,1506,1525,1413,1800,1800
+204,1520,1506,1525,1413,1800,1800
+205,1520,1506,1525,1490,1800,1800
+206,1587,1506,1525,1490,1800,1800
+207,1491,1506,1475,1490,1800,1800
+208,1491,1506,1475,1490,1800,1800
+209,1495,1538,1500,1490,1800,1800
+210,1495,1538,1500,1606,1800,1800
+211,1525,1538,1500,1606,1800,1800
+212,1542,1538,1500,1607,1800,1800
+213,1542,1538,1500,1700,1800,1800
+214,1542,1538,1500,1700,1800,1800
+215,1542,1538,1500,1700,1800,1800
+216,1544,1538,1500,1700,1800,1800
+217,1562,1538,1500,1700,1800,1800
+218,1562,1538,1500,1700,1800,1800
+219,1562,1538,1500,1700,1800,1800
+220,1562,1538,1500,1700,1800,1800
+221,1555,1538,1500,1700,1800,1800
+222,1550,1538,1500,1700,1800,1800
+223,1550,1538,1500,1700,1800,1800
+224,1550,1538,1500,1700,1800,1800
+225,1550,1538,1500,1700,1800,1800
+226,1550,1538,1500,1700,1800,1800
+227,1500,1538,1500,1450,1800,1800
diff --git a/lf-drone/simulation/src/ToFBridgeCSV.lf b/lf-drone/simulation/src/ToFBridgeCSV.lf
new file mode 100644
index 0000000..c6c75fe
--- /dev/null
+++ b/lf-drone/simulation/src/ToFBridgeCSV.lf
@@ -0,0 +1,121 @@
+target C
+
+preamble {=
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <ctype.h>
+#include <errno.h>
+
+void lf_request_stop(void);
+
+static int line_has_alpha(const char* s) {
+  while (*s) {
+    if (isalpha((unsigned char)*s)) return 1;
+    s++;
+  }
+  return 0;
+}
+
+static int parse_first_number(const char* s, double* out) {
+  const char* p = s;
+  while (*p != '\0') {
+    char* end = NULL;
+    double v = strtod(p, &end);
+    if (end != p) {
+      *out = v;
+      return 0;
+    }
+    p++;
+  }
+  return -1;
+}
+=}
+
+reactor PyToF(
+  id:int=0,
+  label:string="sensor",
+  path:string="simulation/data/sensor.csv",
+  start_ms:time=20 ms,
+  period:time=20 ms,
+  verbose:int=0
+) {
+  timer poll(start_ms, period)
+  output value: double
+
+  state fp: FILE*
+  state line_no:int = 0
+  state sample_count:int = 0
+
+  reaction(startup) {=
+    self->fp = NULL;
+    self->line_no = 0;
+    self->sample_count = 0;
+
+    printf("[PyToFCSV:%d:%s] opening %s\n", self->id, self->label, self->path);
+    fflush(stdout);
+
+    self->fp = fopen(self->path, "r");
+    if (!self->fp) {
+      fprintf(stderr,
+              "[PyToFCSV:%d:%s] FAILED to open %s : %s\n",
+              self->id, self->label, self->path, strerror(errno));
+      fflush(stderr);
+      lf_request_stop();
+      return;
+    }
+  =}
+
+  reaction(shutdown) {=
+    if (self->fp) fclose(self->fp);
+    self->fp = NULL;
+  =}
+
+  reaction(poll) -> value {=
+    if (!self->fp) return;
+
+    char line[256];
+
+    while (1) {
+      if (!fgets(line, sizeof(line), self->fp)) {
+        printf("[PyToFCSV:%d:%s] EOF on %s after %d samples\n",
+               self->id, self->label, self->path, self->sample_count);
+        fflush(stdout);
+
+        fclose(self->fp);
+        self->fp = NULL;
+        lf_request_stop();
+        return;
+      }
+
+      self->line_no += 1;
+
+      if (line[0] == '#') {
+        continue;
+      }
+
+      double v = 0.0;
+      if (parse_first_number(line, &v) == 0) {
+        self->sample_count += 1;
+        lf_set(value, v);
+
+        if (self->verbose) {
+          printf("[PyToFCSV:%d:%s] sample=%d value=%.3f\n",
+                 self->id, self->label, self->sample_count, v);
+          fflush(stdout);
+        }
+        return;
+      }
+
+      if (self->line_no == 1 && line_has_alpha(line)) {
+        continue;
+      }
+
+      if (self->verbose) {
+        printf("[PyToFCSV:%d:%s] skipped line %d\n",
+               self->id, self->label, self->line_no);
+        fflush(stdout);
+      }
+    }
+  =}
+}
\ No newline at end of file
diff --git a/lf-drone/simulation/src/test.lf b/lf-drone/simulation/src/test.lf
new file mode 100644
index 0000000..65339fc
--- /dev/null
+++ b/lf-drone/simulation/src/test.lf
@@ -0,0 +1,41 @@
+target C   // runs until Ctrl+C
+
+import PyToF from "ToFBridgeCSV.lf"
+import AvoidPlanner from "/mnt/e/PhD/lf-demos/lf-drone/lib/avoid_planner_modal.lf"
+import MSPSender from "/mnt/e/PhD/lf-demos/lf-drone/lib/msp_sender.lf"
+import UserLandCmd from "/mnt/e/PhD/lf-demos/lf-drone/lib/UserLandCmd.lf"
+
+main reactor test {
+  bottom = new PyToF(id=0, label="bottom", path="simulation/data/bottom.csv", start_ms=20 ms, period=20 ms, verbose=1)
+  front  = new PyToF(id=1, label="front",  path="simulation/data/front.csv",  start_ms=20 ms, period=20 ms, verbose=1)
+  right  = new PyToF(id=2, label="right",  path="simulation/data/right.csv",  start_ms=20 ms, period=20 ms, verbose=1)
+  left   = new PyToF(id=3, label="left",   path="simulation/data/left.csv",   start_ms=20 ms, period=20 ms, verbose=1)
+  top    = new PyToF(id=4, label="top",    path="simulation/data/top.csv",    start_ms=20 ms, period=20 ms, verbose=1)
+
+  avoid = new AvoidPlanner()
+
+  msp = new MSPSender(
+    port="",
+    period=20 ms,
+    verbose=1,
+    print_every=1,
+    log_path="simulation/results/rc-out.csv"
+  )
+
+  land_cmd = new UserLandCmd()
+
+  front.value  -> avoid.front
+  left.value   -> avoid.left
+  right.value  -> avoid.right
+  bottom.value -> avoid.bottom
+  top.value    -> avoid.top
+
+  land_cmd.out -> avoid.land
+
+  avoid.roll     -> msp.roll
+  avoid.pitch    -> msp.pitch
+  avoid.yaw      -> msp.yaw
+  avoid.throttle -> msp.throttle
+  avoid.aux1     -> msp.aux1
+  avoid.aux2     -> msp.aux2
+}
\ No newline at end of file