From 1180256a4c6958bbb112a2c8d369a118015679d0 Mon Sep 17 00:00:00 2001 From: saipavan4 Date: Tue, 14 Oct 2025 17:03:45 +0100 Subject: [PATCH 1/9] Added files to run drone --- drone/src/DroneBridgeC.lf | 165 ++++++++++++++++++++++++++++++++++++++ drone/src/ToFBridgeC.lf | 100 +++++++++++++++++++++++ drone/src/tof_reader.py | 59 ++++++++++++++ 3 files changed, 324 insertions(+) create mode 100644 drone/src/DroneBridgeC.lf create mode 100644 drone/src/ToFBridgeC.lf create mode 100644 drone/src/tof_reader.py diff --git a/drone/src/DroneBridgeC.lf b/drone/src/DroneBridgeC.lf new file mode 100644 index 0000000..0796823 --- /dev/null +++ b/drone/src/DroneBridgeC.lf @@ -0,0 +1,165 @@ +target C { timeout: 10 s } + +preamble {= +#include +#include +#include +#include +#include +#include +#include + +#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/drone/src/ToFBridgeC.lf b/drone/src/ToFBridgeC.lf new file mode 100644 index 0000000..37c115d --- /dev/null +++ b/drone/src/ToFBridgeC.lf @@ -0,0 +1,100 @@ +target C { timeout: 15 s } + +preamble {= +#include +#include +#include +#include +#include +#include + +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 /home/duckie/drone_msp_env/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(bus:int=22, addr:int=41, rate:int=20, timing_ms:int=100) { + // start after 100 ms so startup finishes before first poll + timer poll(100 ms, 20 ms) + output height: double + + // initialize pointer container to 0 + state tp: long = 0 + state last_h: 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_h = 0.0; + printf("PyToF started on bus %d addr 0x%02X rate %dHz timing %dms\n", + self->bus, self->addr, self->rate, self->timing_ms); + =} + + reaction(poll) -> height {= + tof_proc_t* proc = (tof_proc_t*)(uintptr_t)self->tp; + if (!proc) return; + double h = 0.0; + int r = tof_read_latest(proc, &h); + if (r == 0) { + self->last_h = h; + lf_set(height, h); + printf("ToF height m: %.3f\n", h); + } + =} + + reaction(shutdown) {= + tof_proc_t* proc = (tof_proc_t*)(uintptr_t)self->tp; + if (proc) { tof_stop(proc); free(proc); self->tp = 0; } + =} +} + +main reactor ToFBridgeC { + // set bus=1 if your HAT exposes /dev/i2c-1 + tof = new PyToF(bus=22, addr=41, rate=20, timing_ms=100) +} diff --git a/drone/src/tof_reader.py b/drone/src/tof_reader.py new file mode 100644 index 0000000..ee68138 --- /dev/null +++ b/drone/src/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() From a41a0b7338327dd97ab5612d481331844b6a24a0 Mon Sep 17 00:00:00 2001 From: Pawan Kumar <57193478+saipavan4@users.noreply.github.com> Date: Tue, 14 Oct 2025 10:15:36 -0700 Subject: [PATCH 2/9] Create README.md --- drone/README.md | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 drone/README.md diff --git a/drone/README.md b/drone/README.md new file mode 100644 index 0000000..6e94d93 --- /dev/null +++ b/drone/README.md @@ -0,0 +1,19 @@ +# 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). + +## Requirements +Install vl53l1x to read the ToF sensors. +``` +pip install vl53l1x +``` + +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 +``` From c9ce6abc0aaa3fcc1ef9e5ccd041898254f4f204 Mon Sep 17 00:00:00 2001 From: saipavan4 Date: Tue, 14 Oct 2025 18:19:27 +0100 Subject: [PATCH 3/9] Added git ignore files --- drone/.gitignore | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 drone/.gitignore diff --git a/drone/.gitignore b/drone/.gitignore new file mode 100644 index 0000000..a09f54e --- /dev/null +++ b/drone/.gitignore @@ -0,0 +1,4 @@ +# Ignore Lingua Franca generated source files +src-gen/ +bin/ +include/ From 5eca2c80224be1e63bcdc480a43c928c04ff4ee9 Mon Sep 17 00:00:00 2001 From: Pawan Kumar <57193478+saipavan4@users.noreply.github.com> Date: Tue, 14 Oct 2025 10:48:35 -0700 Subject: [PATCH 4/9] Update README.md --- drone/README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/drone/README.md b/drone/README.md index 6e94d93..a3590a1 100644 --- a/drone/README.md +++ b/drone/README.md @@ -1,12 +1,16 @@ # 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 You can test the flight of the drone using ``` lfc src/DroneBrdigeC.lf From bb6d17bac45de2d25a681b2c496bf583058696dc Mon Sep 17 00:00:00 2001 From: saipavan4 Date: Tue, 14 Oct 2025 22:52:31 +0100 Subject: [PATCH 5/9] Added other ToF sensors --- drone/src/ToFBridgeC.lf | 106 ++++++++++++++++++++++++++++++++++------ 1 file changed, 90 insertions(+), 16 deletions(-) diff --git a/drone/src/ToFBridgeC.lf b/drone/src/ToFBridgeC.lf index 37c115d..39f0597 100644 --- a/drone/src/ToFBridgeC.lf +++ b/drone/src/ToFBridgeC.lf @@ -1,4 +1,4 @@ -target C { timeout: 15 s } +target C preamble {= #include @@ -21,7 +21,7 @@ static int tof_start(tof_proc_t* tp, int bus, int addr, int rate, int timing_ms) char cmd[256]; // safer: absolute path so we never depend on cwd snprintf(cmd, sizeof(cmd), - "python3 /home/duckie/drone_msp_env/tof_reader.py --bus %d --addr 0x%02X --rate %d --timing_ms %d 2>/dev/null", + "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; @@ -55,14 +55,16 @@ static void tof_stop(tof_proc_t* tp) { } =} -reactor PyToF(bus:int=22, addr:int=41, rate:int=20, timing_ms:int=100) { +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(100 ms, 20 ms) - output height: double + timer poll(start_ms, 20 ms) + output value: double // initialize pointer container to 0 state tp: long = 0 - state last_h: double + state last_val: double reaction(startup) {= tof_proc_t* proc = (tof_proc_t*)malloc(sizeof(tof_proc_t)); @@ -71,20 +73,24 @@ reactor PyToF(bus:int=22, addr:int=41, rate:int=20, timing_ms:int=100) { fprintf(stderr, "ToF start failed\n"); free(proc); return; } self->tp = (long)(intptr_t)proc; // store pointer safely - self->last_h = 0.0; - printf("PyToF started on bus %d addr 0x%02X rate %dHz timing %dms\n", - self->bus, self->addr, self->rate, self->timing_ms); + 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) -> height {= + reaction(poll) -> value {= tof_proc_t* proc = (tof_proc_t*)(uintptr_t)self->tp; if (!proc) return; - double h = 0.0; - int r = tof_read_latest(proc, &h); + double val = 0.0; + int r = tof_read_latest(proc, &val); if (r == 0) { - self->last_h = h; - lf_set(height, h); - printf("ToF height m: %.3f\n", h); + self->last_val = val; + lf_set(value, val); + if (self->verbose) { + printf("ToF[%d:%s] value: %.3f\n", self->id, self->label, val); + } } =} @@ -94,7 +100,75 @@ reactor PyToF(bus:int=22, addr:int=41, rate:int=20, timing_ms:int=100) { =} } +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 - tof = new PyToF(bus=22, addr=41, rate=20, timing_ms=100) + 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 } From 2cb2c079aeb29e9ea7b7d9a6e94eb9a906add95b Mon Sep 17 00:00:00 2001 From: Pawan Kumar <57193478+saipavan4@users.noreply.github.com> Date: Tue, 14 Oct 2025 14:56:37 -0700 Subject: [PATCH 6/9] Update README.md --- drone/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/drone/README.md b/drone/README.md index a3590a1..3fc55e8 100644 --- a/drone/README.md +++ b/drone/README.md @@ -11,6 +11,7 @@ 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 From 35fb2a74a1254b2451df385e236f617bef9db42f Mon Sep 17 00:00:00 2001 From: Pawan Kumar <57193478+saipavan4@users.noreply.github.com> Date: Tue, 14 Oct 2025 14:56:50 -0700 Subject: [PATCH 7/9] Update README.md --- drone/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/drone/README.md b/drone/README.md index 3fc55e8..dc7eea9 100644 --- a/drone/README.md +++ b/drone/README.md @@ -12,6 +12,7 @@ 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 From 997f127dcccb8b96fc13de63b1e17adae2ea4a6d Mon Sep 17 00:00:00 2001 From: saipavan4 Date: Mon, 20 Oct 2025 23:23:56 +0100 Subject: [PATCH 8/9] Drone auto fly test code --- drone/src/avoid_planner.lf | 120 +++++++++++++++++++++++++++++++++++++ drone/src/msp_sender.lf | 61 +++++++++++++++++++ drone/src/test.lf | 32 ++++++++++ 3 files changed, 213 insertions(+) create mode 100644 drone/src/avoid_planner.lf create mode 100644 drone/src/msp_sender.lf create mode 100644 drone/src/test.lf diff --git a/drone/src/avoid_planner.lf b/drone/src/avoid_planner.lf new file mode 100644 index 0000000..a7db6e2 --- /dev/null +++ b/drone/src/avoid_planner.lf @@ -0,0 +1,120 @@ +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 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) -> 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); + lf_set(aux2,1800); // ANGLE ON + self->stage=0; self->count=0; self->i_h=0.0; self->hover_ok=0; + =} + + reaction(tick) -> roll,pitch,yaw,throttle,aux1,aux2 {= + int rc_roll=1500, rc_pitch=1500, rc_yaw=1500, rc_thr=1000; + int rc_aux1=1000, rc_aux2=1800; + + 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(uii_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); + =} +} diff --git a/drone/src/msp_sender.lf b/drone/src/msp_sender.lf new file mode 100644 index 0000000..f415eae --- /dev/null +++ b/drone/src/msp_sender.lf @@ -0,0 +1,61 @@ +target C + +preamble {= +#include +#include +#include +#include +#include +#include + +#define MSP_SET_RAW_RC 200 + +static uint8_t cs(uint8_t size, uint8_t cmd, const uint8_t* data){ + uint8_t s = size ^ cmd; for(int i=0;i2000)v=2000; + payload[2*i] = v & 0xFF; payload[2*i+1] = (v>>8)&0xFF; } + uint8_t header[3] = {'$','M','<'}, size=sizeof(payload), cmd=MSP_SET_RAW_RC, c = 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, &c, 1)!=1) return -1; + return 0; +} +=} + +reactor MSPSender(port:string="/dev/ttyACM0") { + input roll:int; input pitch:int; input yaw:int; input throttle:int; input aux1:int; input aux2:int + timer tx(0, 10 ms) + state fd:int + + reaction(startup) {= + self->fd = open_serial(self->port); + if (self->fd >= 0) printf("MSP: opened %s\n", self->port); + else printf("MSP: FAILED to open %s\n", self->port); + =} + + reaction(tx) {= + if (self->fd < 0) return; + uint16_t rc[16] = {1500,1500,1500,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000}; + if (roll->is_present) rc[0] = roll->value; + if (pitch->is_present) rc[1] = pitch->value; + if (yaw->is_present) rc[2] = yaw->value; + if (throttle->is_present) rc[3] = throttle->value; + if (aux1->is_present) rc[4] = aux1->value; + if (aux2->is_present) rc[5] = aux2->value; // ANGLE switch + send_rc(self->fd, rc); + =} +} diff --git a/drone/src/test.lf b/drone/src/test.lf new file mode 100644 index 0000000..3853542 --- /dev/null +++ b/drone/src/test.lf @@ -0,0 +1,32 @@ +target C // runs until Ctrl+C + +import PyToF from "ToFBridgeC.lf" +import AvoidPlanner from "avoid_planner.lf" +import MSPSender from "msp_sender.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") + + // sensors -> planner + front.value -> avoid.front + left.value -> avoid.left + right.value -> avoid.right + bottom.value -> avoid.bottom + top.value -> avoid.top + + // 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 From 3c319e3da0f6a3cdd95b18ce9136b4b9169b20fb Mon Sep 17 00:00:00 2001 From: Hokeun Kim Date: Mon, 20 Oct 2025 16:25:29 -0700 Subject: [PATCH 9/9] Add comments and change how inputs are fed to the avoid planner --- drone/src/avoid_planner.lf | 34 ++++++++++++++++++++-------------- 1 file changed, 20 insertions(+), 14 deletions(-) diff --git a/drone/src/avoid_planner.lf b/drone/src/avoid_planner.lf index a7db6e2..209ccab 100644 --- a/drone/src/avoid_planner.lf +++ b/drone/src/avoid_planner.lf @@ -1,7 +1,7 @@ target C reactor AvoidPlanner { - timer tick(0, 20 ms) // 50 Hz + // 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 @@ -34,24 +34,30 @@ reactor AvoidPlanner { state t_bump:int = 75 // 1.5 s state hover_ok:int = 0 - 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; =} + state initialized:bool = false - 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); - lf_set(aux2,1800); // ANGLE ON - self->stage=0; self->count=0; self->i_h=0.0; self->hover_ok=0; - =} + // 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; + } - reaction(tick) -> roll,pitch,yaw,throttle,aux1,aux2 {= 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;