lf-drone simulation and hardware implementation files.

lf-drone/.gitignore

@@ -0,0 +1,8 @@
+# Lingua Franca generated/build directories
+bin/
+include/
+src-gen/
+.venv/
+**/bin/
+**/include/
+**/src-gen/

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.

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`.

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;
+    }
+  =}
+}

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);
+  =}
+}