Drone flight control

drone/.gitignore

@@ -0,0 +1,4 @@
+# Ignore Lingua Franca generated source files
+src-gen/
+bin/
+include/

drone/README.md

@@ -0,0 +1,25 @@
+# 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).
+
+## Prerequisites
+Fly the drone using the instructions from [Duckietown DD24 assembly and configuration guide](https://docs.duckietown.com/daffy/opmanual-dd24/intro.html)
+
+## Requirements
+Install vl53l1x to read the ToF sensors. 
+```
+pip install vl53l1x 
+```
+
+## Testing
+Clone the repository and move to the drone directory.
+
+You can test the flight of the drone using 
+```
+lfc src/DroneBrdigeC.lf
+./src-gen/DroneBridgeC/build/DroneBridgeC
+```
+To test the ToF sensors, you can do it using
+```
+lfc src/ToFBridgeC.lf
+./src-gen/ToFBridgeC/build/ToFBridgeC
+```

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

drone/src/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 ./src/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
+}