Compare commits
7 Commits
f952ca2d0b
...
796e343b78
| Author | SHA1 | Date | |
|---|---|---|---|
| 796e343b78 | |||
| d2cada00a0 | |||
| f2b24e29d8 | |||
| 7dcdd2088a | |||
| c3ada4a156 | |||
| 566cfc8811 | |||
| cf0a5a3583 |
165
chassis/payload_bay_BOM.md
Normal file
165
chassis/payload_bay_BOM.md
Normal file
@ -0,0 +1,165 @@
|
||||
# Payload Bay BOM — Issue #170
|
||||
**Agent:** sl-mechanical | **Date:** 2026-03-01
|
||||
|
||||
Modular dovetail payload rail system. Tool-free slide-and-click module swapping.
|
||||
Cross-variant: SaltyLab, SaltyRover, SaltyTank (same rail profile).
|
||||
|
||||
---
|
||||
|
||||
## A. Rail Hardware
|
||||
|
||||
| # | Description | Spec | Qty (per robot) | Notes |
|
||||
|---|-------------|------|-----------------|-------|
|
||||
| R1 | Aluminium bar stock | 50×12 mm, 6061-T6, 200 mm length | 1–2 | Preferred over printed rail for 2 kg load rating. CNC mill or route dovetail slot per `payload_rail.dxf` profile. |
|
||||
| R2 | M4×10 FHCS | Stainless, countersunk | 4–8 | Rail to adapter plate (or direct to deck); FHCS sits flush below rail bottom face |
|
||||
| R3 | M4 heat-set insert | M4×5.7 L, Ø5.6 OD | 4–8 | Into deck adapter plate |
|
||||
| R4 | Detent ball bearing | Ø6 mm, chrome steel (GCr15) | 2 per module | Module spring detent; standard bearing ball |
|
||||
| R5 | Detent spring | Ø5.5 mm OD, 12 mm free length, ~2 N/mm | 2 per module | Lee Spring LC 055A 06 S or equivalent; behind ball in plunger |
|
||||
| R6 | M4 thumbscrew (knurled) | M4×12, knurled head Ø14 mm | 1 per module | Safety lock; threads into M4 nut pressed into module side |
|
||||
| R7 | M4 hex nut | DIN 934, stainless | 1 per module | Captured in module body for thumbscrew |
|
||||
|
||||
## B. Power + Data Connector
|
||||
|
||||
| # | Description | Spec | Qty (per rail) | Notes |
|
||||
|---|-------------|------|----------------|-------|
|
||||
| C1 | Pogo pin | P75-E2 style, Ø2 mm, 6 mm travel, rated 2 A | 4 | Rail-side spring contacts. AliExpress "P75-E2 pogo pin" or Mill-Max 0906 series. |
|
||||
| C2 | Brass target pad | Ø4 × 1.5 mm disc | 4 per module | Module-side contact pads. Machine from Ø4 mm brass rod or order PCB pads. Press-fit with Loctite 603. |
|
||||
| C3 | JST-XH 2.54 mm header | 4-pin, right-angle | 1 per rail | Rail-side connector to power harness |
|
||||
| C4 | JST-XH housing + crimps | 4-pin female | 1 per robot | Wires from robot PSU (5 V, 12 V, GND, UART) |
|
||||
| C5 | 20 AWG silicone wire | Red / black / yellow / white, 300 mm each | 4 | Rail connector to robot bus |
|
||||
| C6 | Connector housing | `payload_connector_stl` | 1 | Press-fit into rail pocket |
|
||||
|
||||
### Pin Map
|
||||
|
||||
| Pin | Signal | Wire colour | Max current |
|
||||
|-----|--------|-------------|-------------|
|
||||
| 1 | GND | Black | Return |
|
||||
| 2 | +5 V | Red | 2 A |
|
||||
| 3 | +12 V | Yellow | 2 A |
|
||||
| 4 | UART (3.3 V) | White | 0.5 A |
|
||||
|
||||
> **UART note**: Half-duplex (single wire). Module firmware connects to Jetson Orin NX UART2. Use RS-485 transceiver if module cable > 500 mm or multi-drop needed.
|
||||
|
||||
## C. Deck Adapters
|
||||
|
||||
| Part | File | Qty | Print | Mass est. |
|
||||
|------|------|-----|-------|-----------|
|
||||
| SaltyLab adapter | `payload_bay_rail.scad` `lab_adapter_stl` | 1 | PETG, 5 perims, 60% infill | ~30 g |
|
||||
| SaltyRover adapter | `payload_bay_rail.scad` `rover_adapter_stl` | 1 | PETG, 5 perims, 60% infill | ~35 g |
|
||||
| SaltyTank adapter | `payload_bay_rail.scad` `tank_adapter_stl` | 1 | PETG, 5 perims, 60% infill | ~35 g |
|
||||
|
||||
## D. Printed Parts
|
||||
|
||||
| Part | File | Qty | Print | Mass est. |
|
||||
|------|------|-----|-------|-----------|
|
||||
| Rail section (prototype) | `payload_bay_rail.scad` `rail_stl` | 1 | PETG, 5 perims, 60% infill, 0.2 mm layer | ~85 g |
|
||||
| Connector housing | `payload_bay_rail.scad` `connector_stl` | 1 | PETG, 5 perims, 100% infill | ~4 g |
|
||||
| Detent plunger | `payload_bay_rail.scad` `detent_plunger_stl` | 2 per module | PETG, 5 perims, 80% infill | ~2 g each |
|
||||
| Module base (universal) | `payload_bay_modules.scad` `base_stl` | N | PETG, 5 perims, 60% infill | ~18 g |
|
||||
| Cargo tray (200 mm) | `payload_bay_modules.scad` `cargo_tray_stl` | 1 | PETG, 4 perims, 30% infill | ~180 g |
|
||||
| Camera boom | `payload_bay_modules.scad` `camera_boom_stl` | 1 | PETG, 5 perims, 50% infill | ~95 g |
|
||||
| Cup holder | `payload_bay_modules.scad` `cup_holder_stl` | 1 | PETG, 4 perims, 25% infill | ~55 g |
|
||||
|
||||
---
|
||||
|
||||
## Dovetail Rail — CNC Specification
|
||||
|
||||
For aluminium production rail (preferred over printed for 2 kg rating):
|
||||
|
||||
```
|
||||
Material: 6061-T6 aluminium
|
||||
Stock: 50 mm × 12 mm flat bar, length to suit (200 mm, 300 mm, 400 mm)
|
||||
|
||||
Dovetail slot (top face, centred):
|
||||
Slot open width at top: 37.2 mm
|
||||
Slot width at bottom: 28.0 mm
|
||||
Slot depth: 8.0 mm
|
||||
Wall angle from vertical: 30.0° (60° included angle)
|
||||
Surface finish: Ra 1.6 µm (smooth for low-friction sliding)
|
||||
|
||||
Detent dimples (slot floor):
|
||||
Diameter: 4.9 mm (ball seats in)
|
||||
Depth: 1.5 mm
|
||||
Pitch: 50 mm
|
||||
First dimple: 25 mm from each end
|
||||
|
||||
Safety-lock groove (both side faces, continuous):
|
||||
Groove diameter: 4.5 mm
|
||||
Depth: 1.5 mm
|
||||
Z position: RAIL_T/2 - DOVE_H/2 = 8 mm from top face
|
||||
(CNC with Ø4 mm ball-nose end mill, single pass at Z = -4 mm from top)
|
||||
|
||||
Mounting holes (bottom face, countersunk):
|
||||
Diameter: 4.3 mm (M4 clearance)
|
||||
C/sink: Ø8 mm × 82° (M4 FHCS)
|
||||
Pitch: 50 mm
|
||||
First hole: 25 mm from each end
|
||||
|
||||
Connector pocket (slot floor, centred in rail length):
|
||||
Width: 26 mm (X), Depth: 8.4 mm (Y), Height: 7 mm (Z into slot floor)
|
||||
Tolerance: +0.2 / 0 mm (press-fit housing)
|
||||
|
||||
DXF cross-section: export payload_rail.dxf for supplier drawing.
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Load Analysis
|
||||
|
||||
| Mode | Load | Safety factor | Method |
|
||||
|------|------|---------------|--------|
|
||||
| Static payload (detent only) | 0.5 kg | 2× | Ball detent retention force ~10 N |
|
||||
| Static payload (thumbscrew locked) | 2.0 kg | 2× | Dovetail shear area ~800 mm² Al |
|
||||
| Dynamic (robot motion, 2 m/s²) | 2.0 kg | 1.5× | Inertial force = 2 kg × 2 m/s² = 4 N; detent holds 10 N |
|
||||
| Dovetail shear (PETG printed) | 1.2 kg | 1.5× | PETG tensile ~50 MPa; recommend Al rail for rated 2 kg |
|
||||
|
||||
> **⚠ For 2 kg payload: use machined aluminium rail. Printed PETG rail is prototype/light-duty only (<0.8 kg payload).**
|
||||
|
||||
---
|
||||
|
||||
## Module Developer Guide
|
||||
|
||||
### Adding a new module in 5 steps
|
||||
|
||||
1. **Copy the template** at the bottom of `payload_bay_modules.scad`.
|
||||
2. **Set `MY_LEN`** — must be a multiple of 50 mm (detent pitch) for repeatable positioning.
|
||||
3. **Call `_module_base(MY_LEN, n_detents)`** as the first statement in your module.
|
||||
4. **Build payload geometry** starting at `Z = 0` (rail top face). Keep total height ≤ 200 mm for robot clearance under doorways.
|
||||
5. **Verify connector alignment** — when module is slid to its operating position, the 4 target pads on the tongue bottom must align with `CONN_Y` on the rail (default: 100 mm from rail entry end). Adjust `conn_offset` if needed.
|
||||
|
||||
### Constraints
|
||||
|
||||
| Parameter | Limit |
|
||||
|-----------|-------|
|
||||
| Module length | Min 60 mm, max 400 mm |
|
||||
| Module height above rail | Max 200 mm (clearance) |
|
||||
| Payload mass | ≤ 2 kg (Al rail + thumbscrew locked) |
|
||||
| Module width | Max 120 mm (robot shoulder clearance) |
|
||||
| Connector draw | Max 2 A per power pin (5 V or 12 V) |
|
||||
|
||||
---
|
||||
|
||||
## Export Commands
|
||||
|
||||
```bash
|
||||
# Rail DXF (for CNC / waterjet machining)
|
||||
openscad payload_bay_rail.scad -D 'RENDER="rail_2d"' -o payload_rail_profile.dxf
|
||||
|
||||
# Rail STL (PETG prototype)
|
||||
openscad payload_bay_rail.scad -D 'RENDER="rail_stl"' -o payload_rail_200mm.stl
|
||||
|
||||
# Rail accessories
|
||||
openscad payload_bay_rail.scad -D 'RENDER="connector_stl"' -o payload_connector.stl
|
||||
openscad payload_bay_rail.scad -D 'RENDER="detent_plunger_stl"' -o payload_detent_plunger.stl
|
||||
|
||||
# Deck adapters
|
||||
openscad payload_bay_rail.scad -D 'RENDER="lab_adapter_stl"' -o payload_adapter_lab.stl
|
||||
openscad payload_bay_rail.scad -D 'RENDER="rover_adapter_stl"' -o payload_adapter_rover.stl
|
||||
openscad payload_bay_rail.scad -D 'RENDER="tank_adapter_stl"' -o payload_adapter_tank.stl
|
||||
|
||||
# Example modules
|
||||
openscad payload_bay_modules.scad -D 'RENDER="cargo_tray_stl"' -o payload_cargo_tray.stl
|
||||
openscad payload_bay_modules.scad -D 'RENDER="camera_boom_stl"' -o payload_camera_boom.stl
|
||||
openscad payload_bay_modules.scad -D 'RENDER="cup_holder_stl"' -o payload_cup_holder.stl
|
||||
openscad payload_bay_modules.scad -D 'RENDER="target_pad_2d"' -o payload_target_pad.dxf
|
||||
```
|
||||
462
chassis/payload_bay_modules.scad
Normal file
462
chassis/payload_bay_modules.scad
Normal file
@ -0,0 +1,462 @@
|
||||
// ============================================================
|
||||
// payload_bay_modules.scad — Payload Bay Module Template + Examples
|
||||
// Issue: #170 Agent: sl-mechanical Date: 2026-03-01
|
||||
// ============================================================
|
||||
//
|
||||
// ── HOW TO CREATE A NEW MODULE ──────────────────────────────────────────────
|
||||
//
|
||||
// 1. Copy the "MODULE TEMPLATE" section at the bottom of this file.
|
||||
// 2. Set MODULE_L to your module's Y length (min 60 mm).
|
||||
// 3. Add your payload geometry on top of the _module_base() call.
|
||||
// 4. The _module_base() provides:
|
||||
// • Male dovetail tongue (slides into rail slot)
|
||||
// • Spring detent bore(s) (for Ø6 mm ball + spring plunger)
|
||||
// • Connector target pads (4× Ø4 mm brass, matching rail pogo pins)
|
||||
// • Safety-lock M4 thumbscrew bore (side of tongue)
|
||||
// • Bottom-face flush with rail top (Z = 0 at rail top / module base)
|
||||
// 5. Your payload geometry sits at Z ≥ 0 (above the rail top face).
|
||||
// 6. Add a RENDER dispatch entry and export command.
|
||||
//
|
||||
// ── DOVETAIL TONGUE GEOMETRY ────────────────────────────────────────────────
|
||||
//
|
||||
// TONGUE_BOT = DOVE_SLOT_BOT - 2*DOVE_CLEAR = 28.0 - 0.6 = 27.4 mm
|
||||
// TONGUE_TOP = DOVE_SLOT_TOP + 2*DOVE_CLEAR = 37.2 + 0.6 = 37.8 mm
|
||||
// TONGUE_H = DOVE_H + 0.2 (slight extra depth, no binding at corners)
|
||||
//
|
||||
// Tongue runs full module length (-Y to +Y in module coords).
|
||||
// Module body sits on top of tongue at Z = 0.
|
||||
//
|
||||
// ── CONNECTOR PADS ──────────────────────────────────────────────────────────
|
||||
// 4× Ø4 mm brass discs press-fit into tongue bottom face.
|
||||
// Pad positions: must align with rail connector at CONN_Y when module
|
||||
// is slid to its intended position (any detent step).
|
||||
// Default: pads centred in module length → module must be placed so its
|
||||
// centre aligns with CONN_Y on rail. Or: set MODULE_CONN_OFFSET to shift.
|
||||
//
|
||||
// ── PAYLOAD RATING ──────────────────────────────────────────────────────────
|
||||
// 2 kg rated payload when safety-lock thumbscrew is tightened.
|
||||
// Detent-only (no thumbscrew): ~0.5 kg (impact / vibration condition).
|
||||
//
|
||||
// ── RENDERED EXAMPLES ────────────────────────────────────────────────────────
|
||||
// Part 1 — cargo_tray() 200×100 mm cargo tray, 30 mm walls
|
||||
// Part 2 — camera_boom() L-arm with sensor_rail-compatible head
|
||||
// Part 3 — cup_holder() Ø80 mm tapered cup cradle
|
||||
//
|
||||
// RENDER options:
|
||||
// "assembly" all 3 modules on ghost rail
|
||||
// "base_stl" module base / tongue only (universal)
|
||||
// "cargo_tray_stl" cargo tray module
|
||||
// "camera_boom_stl" camera boom module
|
||||
// "cup_holder_stl" cup holder module
|
||||
// "target_pad_2d" DXF — Ø4 mm brass target pad profile
|
||||
//
|
||||
// Export:
|
||||
// openscad payload_bay_modules.scad -D 'RENDER="base_stl"' -o payload_base.stl
|
||||
// openscad payload_bay_modules.scad -D 'RENDER="cargo_tray_stl"' -o payload_cargo_tray.stl
|
||||
// openscad payload_bay_modules.scad -D 'RENDER="camera_boom_stl"' -o payload_camera_boom.stl
|
||||
// openscad payload_bay_modules.scad -D 'RENDER="cup_holder_stl"' -o payload_cup_holder.stl
|
||||
// ============================================================
|
||||
|
||||
$fn = 64;
|
||||
e = 0.01;
|
||||
|
||||
// ── Rail geometry constants (must match payload_bay_rail.scad) ────────────────
|
||||
RAIL_W = 50.0;
|
||||
RAIL_T = 12.0;
|
||||
DOVE_ANGLE = 30.0;
|
||||
DOVE_H = 8.0;
|
||||
DOVE_SLOT_BOT = 28.0;
|
||||
DOVE_SLOT_TOP = DOVE_SLOT_BOT + 2 * DOVE_H * tan(DOVE_ANGLE);
|
||||
DOVE_CLEAR = 0.3;
|
||||
DETENT_PITCH = 50.0;
|
||||
DETENT_BALL_D = 6.0;
|
||||
DETENT_SPG_OD = 6.2;
|
||||
DETENT_SPG_L = 16.0;
|
||||
CONN_PIN_SPC = 5.0;
|
||||
CONN_N_PINS = 4;
|
||||
CONN_HOUSING_D = 8.0;
|
||||
|
||||
// ── Module tongue (male dovetail) geometry ────────────────────────────────────
|
||||
TONGUE_BOT = DOVE_SLOT_BOT - 2*DOVE_CLEAR; // 27.4 mm
|
||||
TONGUE_TOP = DOVE_SLOT_TOP + 2*DOVE_CLEAR; // 37.8 mm
|
||||
TONGUE_H = DOVE_H + 0.2; // 8.2 mm (slight extra depth)
|
||||
|
||||
// ── Connector target pad ──────────────────────────────────────────────────────
|
||||
TARGET_PAD_D = 4.0; // brass pad OD (slightly larger than pogo Ø2 mm)
|
||||
TARGET_PAD_T = 1.5; // brass pad thickness
|
||||
TARGET_PAD_RECESS = 1.3; // press-fit recess depth (pad is 0.2 mm proud)
|
||||
// 4 pads at CONN_PIN_SPC pitch, centred in module tongue
|
||||
CONN_SPAN = (CONN_N_PINS - 1) * CONN_PIN_SPC; // 15 mm
|
||||
|
||||
// ── Module safety lock ────────────────────────────────────────────────────────
|
||||
LOCK_BOLT_D = 4.3; // M4 thumbscrew bore through tongue side
|
||||
LOCK_BOLT_Z = TONGUE_H/2; // Z of thumbscrew CL from tongue bottom
|
||||
// Thumbscrew tightens against continuous groove in rail side.
|
||||
|
||||
// Fasteners
|
||||
M3_D = 3.3;
|
||||
M4_D = 4.3;
|
||||
|
||||
// ============================================================
|
||||
// RENDER DISPATCH
|
||||
// ============================================================
|
||||
RENDER = "assembly";
|
||||
|
||||
if (RENDER == "assembly") assembly();
|
||||
else if (RENDER == "base_stl") _module_base(80);
|
||||
else if (RENDER == "cargo_tray_stl") cargo_tray();
|
||||
else if (RENDER == "camera_boom_stl") camera_boom();
|
||||
else if (RENDER == "cup_holder_stl") cup_holder();
|
||||
else if (RENDER == "target_pad_2d") {
|
||||
projection(cut = true) translate([0, 0, -0.5])
|
||||
linear_extrude(1) circle(d = TARGET_PAD_D);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// ASSEMBLY PREVIEW
|
||||
// ============================================================
|
||||
module assembly() {
|
||||
// Ghost rail
|
||||
%color("Silver", 0.25)
|
||||
translate([0, 0, -RAIL_T])
|
||||
cube([RAIL_W, 600, RAIL_T], center = true);
|
||||
|
||||
// Cargo tray at Y=50 (first detent position)
|
||||
color("OliveDrab", 0.85)
|
||||
translate([0, 50, 0])
|
||||
cargo_tray();
|
||||
|
||||
// Cup holder at Y=300
|
||||
color("RoyalBlue", 0.85)
|
||||
translate([0, 300, 0])
|
||||
cup_holder();
|
||||
|
||||
// Camera boom at Y=500
|
||||
color("DarkSlateGray", 0.85)
|
||||
translate([0, 500, 0])
|
||||
camera_boom();
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// UNIVERSAL MODULE BASE (_module_base)
|
||||
// ============================================================
|
||||
// All modules use this as their foundation.
|
||||
// Provides: male dovetail tongue, detent bore, connector pads, lock bore.
|
||||
//
|
||||
// module_len : module length in Y (≥ 60 mm)
|
||||
// n_detents : how many ball detent bores to include (1 or 2)
|
||||
// conn_offset: Y offset of connector pads from module centre (default 0)
|
||||
//
|
||||
// After calling _module_base(), add payload geometry at Z = 0 and above.
|
||||
// The tongue occupies Z = -(TONGUE_H) to Z = 0.
|
||||
// Rail top face is at Z = 0.
|
||||
module _module_base(module_len, n_detents = 1, conn_offset = 0) {
|
||||
ml = module_len;
|
||||
|
||||
difference() {
|
||||
// ── Dovetail tongue (male) ────────────────────────────────────
|
||||
translate([0, ml/2, -TONGUE_H/2])
|
||||
linear_extrude(TONGUE_H, center = true, twist = 0,
|
||||
convexity = 2)
|
||||
_tongue_profile_2d();
|
||||
|
||||
// ── Spring detent bore(s) (up through tongue top face) ────────
|
||||
// 1 detent: at module centre; 2 detents: at ±25 mm from centre
|
||||
for (i = [0 : n_detents - 1]) {
|
||||
dy = (n_detents == 1)
|
||||
? ml/2
|
||||
: ml/2 + (i - (n_detents-1)/2) * DETENT_PITCH;
|
||||
translate([0, dy, -(TONGUE_H/2) - DETENT_SPG_L/2])
|
||||
cylinder(d = DETENT_SPG_OD,
|
||||
h = DETENT_SPG_L + TONGUE_H/2 + e);
|
||||
}
|
||||
|
||||
// ── Connector target pad recesses (tongue bottom face) ────────
|
||||
for (i = [0 : CONN_N_PINS - 1]) {
|
||||
px = (i - (CONN_N_PINS-1)/2) * CONN_PIN_SPC;
|
||||
translate([px,
|
||||
ml/2 + conn_offset,
|
||||
-TONGUE_H + e])
|
||||
cylinder(d = TARGET_PAD_D + 0.1,
|
||||
h = TARGET_PAD_RECESS + e);
|
||||
}
|
||||
|
||||
// ── Safety-lock M4 thumbscrew bore (right side of tongue) ─────
|
||||
// Bore exits tongue right face, tip bears on rail side groove
|
||||
translate([TONGUE_TOP/2 + e, ml/2, LOCK_BOLT_Z - TONGUE_H])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = LOCK_BOLT_D,
|
||||
h = (TONGUE_TOP - TONGUE_BOT)/2 + 6 + e);
|
||||
|
||||
// ── Lead-in chamfer on entry end of tongue ────────────────────
|
||||
// 2 mm chamfer on bottom corners of tongue at Y=0 end
|
||||
translate([0, 0, -TONGUE_H])
|
||||
rotate([45, 0, 0])
|
||||
cube([TONGUE_TOP + 2*e, 4, 4], center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Tongue 2D cross-section (male dovetail, trapezoid wider at bottom) ────────
|
||||
// Module tongue: narrower at top (entering slot), wider at bottom (interlocking).
|
||||
// Note orientation: tongue points DOWN (-Z), so wider face is at bottom (-Z).
|
||||
module _tongue_profile_2d() {
|
||||
polygon([
|
||||
[-TONGUE_TOP/2, 0], // top-left (at Z = 0, flush with rail top)
|
||||
[ TONGUE_TOP/2, 0], // top-right
|
||||
[ TONGUE_BOT/2, -TONGUE_H], // bottom-right (interlocking face)
|
||||
[-TONGUE_BOT/2, -TONGUE_H], // bottom-left
|
||||
]);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 1 — CARGO TRAY
|
||||
// ============================================================
|
||||
// 200×100 mm open tray for transporting small items.
|
||||
// 30 mm walls, chamfered rim, 4× drainage holes.
|
||||
// Two Velcro strip slots on tray floor for cargo retention.
|
||||
// Module length: 200 mm (4 detent positions).
|
||||
// Weight budget: tray printed ~180 g; payload up to 2 kg.
|
||||
TRAY_L = 200.0; // module Y length
|
||||
TRAY_W = 100.0; // tray interior width (X)
|
||||
TRAY_WALL = 3.0; // tray wall thickness
|
||||
TRAY_H = 30.0; // tray wall height above module base
|
||||
TRAY_FLOOR_T = 3.0; // tray floor thickness
|
||||
|
||||
module cargo_tray() {
|
||||
// Mount base on rail (2 detents at ±50 mm from module centre)
|
||||
_module_base(TRAY_L, n_detents = 2);
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── Tray body on top of base ────────────────────────────
|
||||
translate([-TRAY_W/2 - TRAY_WALL, 0,
|
||||
0])
|
||||
cube([TRAY_W + 2*TRAY_WALL,
|
||||
TRAY_L,
|
||||
TRAY_FLOOR_T + TRAY_H]);
|
||||
|
||||
// ── Corner gussets (stiffening for 2 kg load) ───────────
|
||||
for (cx = [-1, 1]) for (cy = [0, 1])
|
||||
hull() {
|
||||
translate([cx * TRAY_W/2,
|
||||
cy * (TRAY_L - TRAY_WALL),
|
||||
0])
|
||||
cube([TRAY_WALL + e, TRAY_WALL + e,
|
||||
TRAY_FLOOR_T + TRAY_H * 0.6], center = true);
|
||||
translate([cx * (TRAY_W/2 - 10),
|
||||
cy * (TRAY_L - TRAY_WALL),
|
||||
0])
|
||||
cylinder(d = TRAY_WALL * 2, h = e);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Tray interior cavity ─────────────────────────────────────
|
||||
translate([-TRAY_W/2, TRAY_WALL,
|
||||
TRAY_FLOOR_T])
|
||||
cube([TRAY_W, TRAY_L - 2*TRAY_WALL,
|
||||
TRAY_H + e]);
|
||||
|
||||
// ── Drainage holes (4× Ø8 mm in floor) ──────────────────────
|
||||
for (dx = [-TRAY_W/4, TRAY_W/4])
|
||||
for (dy = [TRAY_L/4, 3*TRAY_L/4])
|
||||
translate([dx, dy, -e])
|
||||
cylinder(d = 8, h = TRAY_FLOOR_T + 2*e);
|
||||
|
||||
// ── Velcro slot × 2 (25 mm wide grooves in floor) ────────────
|
||||
for (dy = [TRAY_L/3, 2*TRAY_L/3])
|
||||
translate([0, dy, TRAY_FLOOR_T - 1.5])
|
||||
cube([TRAY_W - 10, 25, 2 + e], center = true);
|
||||
|
||||
// ── Rim chamfer (top inner edge, ergonomic) ───────────────────
|
||||
translate([0, TRAY_L/2, TRAY_FLOOR_T + TRAY_H + e])
|
||||
cube([TRAY_W + 2*TRAY_WALL + 2*e,
|
||||
TRAY_L + 2*e, 4], center = true);
|
||||
|
||||
// ── Side slots for bungee cord retention (3× each long side) ─
|
||||
for (sx = [-1, 1])
|
||||
for (sy = [TRAY_L/4, TRAY_L/2, 3*TRAY_L/4])
|
||||
translate([sx * (TRAY_W/2 + TRAY_WALL/2),
|
||||
sy, TRAY_FLOOR_T + TRAY_H/2])
|
||||
cube([TRAY_WALL + 2*e, 8, 6], center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 2 — CAMERA BOOM
|
||||
// ============================================================
|
||||
// L-shaped arm: vertical mast + horizontal boom.
|
||||
// Boom head accepts sensor_rail 2020 T-slot (RAIL_W/2 bolt pattern).
|
||||
// Sensor head can be rotated 0/90/180° and locked with M4 bolt.
|
||||
// Module length: 80 mm; arm rises 120 mm, boom extends 80 mm forward.
|
||||
BOOM_MODULE_L = 80.0;
|
||||
BOOM_MAST_H = 120.0; // mast height above rail top (Z)
|
||||
BOOM_ARM_L = 80.0; // horizontal boom length (+Y forward)
|
||||
BOOM_ARM_W = 20.0; // arm cross-section width
|
||||
BOOM_ARM_T = 20.0; // arm cross-section height
|
||||
BOOM_HEAD_W = 50.0; // sensor head width (matches 2020 rail flange)
|
||||
BOOM_HEAD_H = 20.0; // sensor head plate height
|
||||
BOOM_HEAD_T = 5.0; // sensor head plate thickness
|
||||
|
||||
module camera_boom() {
|
||||
_module_base(BOOM_MODULE_L, n_detents = 1);
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── Mast (vertical column from module body) ──────────────
|
||||
translate([-BOOM_ARM_W/2, BOOM_MODULE_L/2 - BOOM_ARM_T/2, 0])
|
||||
cube([BOOM_ARM_W, BOOM_ARM_T, BOOM_MAST_H]);
|
||||
|
||||
// ── Horizontal boom (from mast top, extends +Y) ──────────
|
||||
translate([-BOOM_ARM_W/2,
|
||||
BOOM_MODULE_L/2 - BOOM_ARM_T/2,
|
||||
BOOM_MAST_H - BOOM_ARM_W])
|
||||
cube([BOOM_ARM_W, BOOM_ARM_L, BOOM_ARM_W]);
|
||||
|
||||
// ── Sensor head plate (at boom tip) ──────────────────────
|
||||
translate([-BOOM_HEAD_W/2,
|
||||
BOOM_MODULE_L/2 - BOOM_ARM_T/2 + BOOM_ARM_L,
|
||||
BOOM_MAST_H - BOOM_ARM_W - BOOM_HEAD_H/2 + BOOM_ARM_W/2])
|
||||
cube([BOOM_HEAD_W, BOOM_HEAD_T, BOOM_HEAD_H]);
|
||||
|
||||
// ── Junction gussets (mast + horizontal boom) ────────────
|
||||
translate([-BOOM_ARM_W/2,
|
||||
BOOM_MODULE_L/2 - BOOM_ARM_T/2,
|
||||
BOOM_MAST_H - BOOM_ARM_W])
|
||||
rotate([45, 0, 0])
|
||||
cube([BOOM_ARM_W, BOOM_ARM_W * 0.7, BOOM_ARM_W * 0.7]);
|
||||
}
|
||||
|
||||
// ── 2020 sensor-rail bolt pattern in head plate ───────────────
|
||||
// 2× M5 slots (matches sensor_rail.scad tank_clamp slot geometry)
|
||||
for (sz = [-BOOM_HEAD_H/4, BOOM_HEAD_H/4])
|
||||
translate([0,
|
||||
BOOM_MODULE_L/2 - BOOM_ARM_T/2 + BOOM_ARM_L + BOOM_HEAD_T + e,
|
||||
BOOM_MAST_H - BOOM_ARM_W/2 + sz])
|
||||
rotate([90, 0, 0])
|
||||
hull() {
|
||||
translate([-6, 0, 0])
|
||||
cylinder(d = 5.3, h = BOOM_HEAD_T + 2*e);
|
||||
translate([+6, 0, 0])
|
||||
cylinder(d = 5.3, h = BOOM_HEAD_T + 2*e);
|
||||
}
|
||||
|
||||
// ── Tilt angle slots (3 positions: 0°, ±15°) ─────────────────
|
||||
for (ta = [-15, 0, 15]) {
|
||||
translate([0,
|
||||
BOOM_MODULE_L/2 - BOOM_ARM_T/2 + BOOM_ARM_L/2,
|
||||
BOOM_MAST_H - BOOM_ARM_W/2])
|
||||
rotate([ta, 0, 0])
|
||||
translate([0, BOOM_ARM_L/2, 0])
|
||||
cylinder(d = 4.3, h = BOOM_ARM_W + 2*e,
|
||||
center = true);
|
||||
}
|
||||
|
||||
// ── Cable tie slots in mast ───────────────────────────────────
|
||||
for (cz = [BOOM_MAST_H * 0.3, BOOM_MAST_H * 0.6])
|
||||
translate([0, BOOM_MODULE_L/2, cz])
|
||||
cube([BOOM_ARM_W + 2*e, 4, 4], center = true);
|
||||
|
||||
// ── Lightening pockets in mast ────────────────────────────────
|
||||
translate([0, BOOM_MODULE_L/2, BOOM_MAST_H * 0.4])
|
||||
cube([BOOM_ARM_W - 6, BOOM_ARM_T - 6,
|
||||
BOOM_MAST_H * 0.4], center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 3 — CUP HOLDER
|
||||
// ============================================================
|
||||
// Tapered cup cradle for standard travel mugs / water bottles.
|
||||
// Inner diameter: 80 mm at top, 68 mm at bottom (matches Ø70 mm typical mug).
|
||||
// Flexible gripper ribs (cut slots) provide spring retention.
|
||||
// Drain hole at bottom for condensation.
|
||||
// Module length: 80 mm.
|
||||
CUP_MODULE_L = 80.0;
|
||||
CUP_INNER_TOP = 80.0; // inner bore OD at top
|
||||
CUP_INNER_BOT = 68.0; // inner bore OD at bottom (taper for grip)
|
||||
CUP_OUTER_T = 4.0; // wall thickness
|
||||
CUP_H = 80.0; // cup holder height
|
||||
CUP_GRIP_SLOTS = 8; // number of flex slots (spring grip)
|
||||
CUP_SLOT_W = 2.5; // flex slot width
|
||||
CUP_SLOT_H = 40.0; // flex slot height (from top)
|
||||
|
||||
module cup_holder() {
|
||||
_module_base(CUP_MODULE_L, n_detents = 1);
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── Outer shell (tapered cylinder) ───────────────────────
|
||||
cylinder(d1 = CUP_INNER_BOT + 2*CUP_OUTER_T,
|
||||
d2 = CUP_INNER_TOP + 2*CUP_OUTER_T,
|
||||
h = CUP_H);
|
||||
|
||||
// ── Base flange (connects to module body footprint) ───────
|
||||
hull() {
|
||||
cylinder(d = CUP_INNER_BOT + 2*CUP_OUTER_T + 4,
|
||||
h = 4);
|
||||
translate([0, CUP_MODULE_L/2, 0])
|
||||
cube([RAIL_W, CUP_MODULE_L, 4], center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Inner bore (tapered cup cavity) ──────────────────────────
|
||||
translate([0, 0, CUP_OUTER_T])
|
||||
cylinder(d1 = CUP_INNER_BOT, d2 = CUP_INNER_TOP,
|
||||
h = CUP_H + e);
|
||||
|
||||
// ── Base drain hole ──────────────────────────────────────────
|
||||
translate([0, 0, -e])
|
||||
cylinder(d = 12, h = CUP_OUTER_T + 2*e);
|
||||
|
||||
// ── Flex grip slots (from top down) ──────────────────────────
|
||||
// Slots allow upper rim to flex inward and grip cup body
|
||||
for (i = [0 : CUP_GRIP_SLOTS - 1]) {
|
||||
angle = i * 360 / CUP_GRIP_SLOTS;
|
||||
rotate([0, 0, angle])
|
||||
translate([CUP_INNER_TOP/2 + CUP_OUTER_T/2, 0, CUP_H - CUP_SLOT_H])
|
||||
cube([CUP_OUTER_T + 2*e, CUP_SLOT_W, CUP_SLOT_H + e],
|
||||
center = true);
|
||||
}
|
||||
|
||||
// ── Exterior branding recess (optional label area) ────────────
|
||||
translate([CUP_INNER_BOT/2 + CUP_OUTER_T/2 - 0.5, 0, CUP_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cube([25, 40, 1 + e], center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// ══ MODULE TEMPLATE ═══════════════════════════════════════════
|
||||
// ══ Copy this block to create a new payload module ════════════
|
||||
// ============================================================
|
||||
//
|
||||
// module my_new_module() {
|
||||
// MY_LEN = 120.0; // module length — must be multiple of DETENT_PITCH (50 mm)
|
||||
//
|
||||
// // Always start with the base (provides tongue, pads, detent bore)
|
||||
// _module_base(MY_LEN, n_detents = 2);
|
||||
//
|
||||
// // Add your payload geometry here.
|
||||
// // Z = 0 is the rail top face / module mounting face.
|
||||
// // Build upward from Z = 0.
|
||||
//
|
||||
// difference() {
|
||||
// union() {
|
||||
// // Example: a simple platform
|
||||
// translate([-(RAIL_W + 10)/2, 0, 0])
|
||||
// cube([RAIL_W + 10, MY_LEN, 10]);
|
||||
//
|
||||
// // Add your geometry...
|
||||
// }
|
||||
//
|
||||
// // Add your cutouts...
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// ── Don't forget to: ──────────────────────────────────────────
|
||||
// 1. Add else if (RENDER == "my_module_stl") my_new_module();
|
||||
// in the RENDER DISPATCH block above.
|
||||
// 2. Add export command in BOM / README.
|
||||
// 3. Test: verify tongue fits rail slot (should slide with 0.3 mm clearance).
|
||||
// 4. Verify connector pad positions align with CONN_Y on rail.
|
||||
// ============================================================
|
||||
429
chassis/payload_bay_rail.scad
Normal file
429
chassis/payload_bay_rail.scad
Normal file
@ -0,0 +1,429 @@
|
||||
// ============================================================
|
||||
// payload_bay_rail.scad — Modular Payload Bay Rail System
|
||||
// Issue: #170 Agent: sl-mechanical Date: 2026-03-01
|
||||
// ============================================================
|
||||
//
|
||||
// Dovetail rail mounted on robot top deck. Payload modules slide on
|
||||
// from either end and are retained by a spring-loaded ball detent plus
|
||||
// an optional M4 thumbscrew safety lock.
|
||||
//
|
||||
// Dovetail geometry (60° included angle — balanced for print + load):
|
||||
//
|
||||
// ← RAIL_W (50 mm) →
|
||||
// ┌──────────────────┐ ← rail top face (Z = RAIL_T)
|
||||
// │ ╲ ╱ │
|
||||
// │ ╲__________╱ │ ← dovetail slot (female, cut into top)
|
||||
// │ (DOVE_SLOT) │
|
||||
// └──────────────────┘ ← rail bottom face (Z = 0)
|
||||
//
|
||||
// DOVE_ANGLE = 30° from vertical (= 60° included).
|
||||
// Slot width at top = DOVE_SLOT_TOP (open face)
|
||||
// Slot width at bottom = DOVE_SLOT_BOT (inner base of slot)
|
||||
// Slot depth = DOVE_H
|
||||
//
|
||||
// Module tongue (male dovetail) slides in with 0.3 mm clearance each side.
|
||||
//
|
||||
// Spring detent: Ø6 mm steel ball in module, spring behind, seats in
|
||||
// Ø4.9 mm dimples drilled into rail slot bottom at DETENT_PITCH spacing.
|
||||
// Provides tactile click-lock at each indexed position.
|
||||
//
|
||||
// Safety lock: M4 thumbscrew through module side, tightens against rail
|
||||
// side wall. For vibration environments or >1 kg payload.
|
||||
//
|
||||
// Power+data connector: 4-pin pogo array in rail at fixed position.
|
||||
// Pins: GND | +5 V | +12 V | UART (half-duplex)
|
||||
// Module has matching brass target pads (Ø4 mm).
|
||||
// Connector position: centred in rail length, at CONN_Y from one end.
|
||||
//
|
||||
// Cross-variant adapter plates (this file):
|
||||
// lab_rail_adapter() — SaltyLab chassis top (Ø25 mm stem clear)
|
||||
// rover_rail_adapter() — SaltyRover deck (M4 grid)
|
||||
// tank_rail_adapter() — SaltyTank deck (M4 grid)
|
||||
//
|
||||
// Coordinate convention:
|
||||
// Rail runs along Y axis. Cross-section in X-Z plane.
|
||||
// Z = 0 at rail bottom face (= robot deck top face).
|
||||
// Module slides in +Y direction.
|
||||
//
|
||||
// RENDER options:
|
||||
// "assembly" rail + adapter + module ghost
|
||||
// "rail_2d" DXF — dovetail cross-section (CNC/waterjet)
|
||||
// "rail_stl" STL — printable rail section (PETG prototype)
|
||||
// "connector_stl" STL — pogo connector housing insert
|
||||
// "detent_plunger_stl" STL — spring detent plunger (print ×2 per module)
|
||||
// "lab_adapter_stl" STL — SaltyLab deck adapter
|
||||
// "rover_adapter_stl" STL — SaltyRover deck adapter
|
||||
// "tank_adapter_stl" STL — SaltyTank deck adapter
|
||||
//
|
||||
// Export:
|
||||
// openscad payload_bay_rail.scad -D 'RENDER="rail_2d"' -o payload_rail.dxf
|
||||
// openscad payload_bay_rail.scad -D 'RENDER="rail_stl"' -o payload_rail.stl
|
||||
// openscad payload_bay_rail.scad -D 'RENDER="connector_stl"' -o payload_connector.stl
|
||||
// openscad payload_bay_rail.scad -D 'RENDER="detent_plunger_stl"' -o payload_detent.stl
|
||||
// openscad payload_bay_rail.scad -D 'RENDER="lab_adapter_stl"' -o payload_lab_adapter.stl
|
||||
// openscad payload_bay_rail.scad -D 'RENDER="rover_adapter_stl"' -o payload_rover_adapter.stl
|
||||
// openscad payload_bay_rail.scad -D 'RENDER="tank_adapter_stl"' -o payload_tank_adapter.stl
|
||||
// ============================================================
|
||||
|
||||
$fn = 64;
|
||||
e = 0.01;
|
||||
|
||||
// ── Dovetail rail cross-section ───────────────────────────────────────────────
|
||||
RAIL_W = 50.0; // rail total width (X)
|
||||
RAIL_T = 12.0; // rail total height (Z)
|
||||
RAIL_R = 2.0; // outer corner radius
|
||||
RAIL_LEN = 200.0; // default rail section length (Y)
|
||||
|
||||
// Dovetail slot geometry
|
||||
DOVE_ANGLE = 30.0; // degrees from vertical (60° included angle)
|
||||
DOVE_H = 8.0; // slot depth (Z into rail from top)
|
||||
DOVE_SLOT_BOT= 28.0; // slot width at bottom (inner)
|
||||
// Derived: slot width at top = DOVE_SLOT_BOT + 2 * DOVE_H * tan(DOVE_ANGLE)
|
||||
DOVE_SLOT_TOP= DOVE_SLOT_BOT + 2 * DOVE_H * tan(DOVE_ANGLE); // ≈ 37.2 mm
|
||||
|
||||
// Module tongue (male) clearance: 0.3 mm per side
|
||||
DOVE_CLEAR = 0.3;
|
||||
// → module tongue: bot_w = DOVE_SLOT_BOT - 2*DOVE_CLEAR, top_w = DOVE_SLOT_TOP + 2*DOVE_CLEAR
|
||||
|
||||
// ── Spring ball detent ────────────────────────────────────────────────────────
|
||||
// Ø6 mm steel ball presses up through module tongue into dimples in rail slot.
|
||||
DETENT_BALL_D = 6.0; // ball diameter
|
||||
DETENT_HOLE_D = 4.9; // dimple bore in rail slot bottom (ball seats in)
|
||||
DETENT_DEPTH = 1.5; // dimple depth (ball sinks in this far)
|
||||
DETENT_PITCH = 50.0; // dimple spacing along rail (Y) — module index positions
|
||||
DETENT_SPG_OD = 6.2; // plunger bore OD (ball + spring housing in module)
|
||||
DETENT_SPG_L = 16.0; // spring pocket depth in module tongue
|
||||
|
||||
// ── Safety lock (M4 thumbscrew through module side into rail side groove) ─────
|
||||
LOCK_GROOVE_D = 4.5; // groove in rail side wall (M4 thumbscrew tip seats in)
|
||||
LOCK_GROOVE_DEPTH = 1.5; // groove depth into rail side
|
||||
// Lock groove runs full rail length (continuous slot) for tool-free slide + lock anywhere
|
||||
|
||||
// ── 4-pin power+data connector ───────────────────────────────────────────────
|
||||
// Pogo pin array mounted in rail body at CONN_Y from entry end.
|
||||
// Pin map: 1=GND 2=+5V 3=+12V 4=UART
|
||||
// Pogo: Ø2 mm spring contact (P75-E2 style), rated 2 A (power), 0.5 A (signal)
|
||||
CONN_Y = RAIL_LEN / 2; // connector centred in rail section
|
||||
CONN_PIN_D = 2.2; // pogo bore (2 mm pin + 0.2 mm clearance)
|
||||
CONN_PIN_SPC = 5.0; // pin centre-to-centre spacing
|
||||
CONN_N_PINS = 4; // GND / +5V / +12V / UART
|
||||
CONN_HOUSING_W= CONN_N_PINS * CONN_PIN_SPC + 4; // housing width (X)
|
||||
CONN_HOUSING_D= 8.0; // housing depth (Y, inside rail)
|
||||
CONN_HOUSING_H= DOVE_H - 1.0; // housing height; sits inside slot (flush with slot floor)
|
||||
// Connector pogo pins point upward into module pad targets.
|
||||
|
||||
// ── Deck mounting holes ───────────────────────────────────────────────────────
|
||||
MOUNT_PITCH = 50.0; // M4 FHCS hole pitch along rail (countersunk from bottom)
|
||||
MOUNT_INSET = 25.0; // first hole Y from rail end
|
||||
MOUNT_D = 4.3; // M4 clearance
|
||||
CSINK_D = 8.0; // M4 FHCS head diameter
|
||||
|
||||
// ── Cross-variant adapter plates ─────────────────────────────────────────────
|
||||
ADAPT_T = 4.0; // adapter plate thickness
|
||||
ADAPT_OVHG = 10.0; // adapter overhang past rail edge each side (flange width)
|
||||
|
||||
// SaltyLab deck: stem bore at centre
|
||||
LAB_STEM_BORE = 26.0; // clear stem Ø25 mm
|
||||
|
||||
// SaltyRover deck: M4 bolt grid (spacing from rover_chassis_r2.scad)
|
||||
ROVER_BOLT_SPC = 40.0;
|
||||
|
||||
// SaltyTank deck: M4 bolt grid (spacing from saltytank_chassis.scad)
|
||||
TANK_BOLT_SPC = 40.0;
|
||||
|
||||
// Fasteners
|
||||
M3_D = 3.3;
|
||||
M4_D = 4.3;
|
||||
|
||||
// ============================================================
|
||||
// RENDER DISPATCH
|
||||
// ============================================================
|
||||
RENDER = "assembly";
|
||||
|
||||
if (RENDER == "assembly") assembly();
|
||||
else if (RENDER == "rail_2d")
|
||||
projection(cut = true) translate([0, 0, -0.5])
|
||||
linear_extrude(1) rail_profile_2d();
|
||||
else if (RENDER == "rail_stl") rail_section(RAIL_LEN);
|
||||
else if (RENDER == "connector_stl") connector_housing();
|
||||
else if (RENDER == "detent_plunger_stl") detent_plunger();
|
||||
else if (RENDER == "lab_adapter_stl") lab_rail_adapter();
|
||||
else if (RENDER == "rover_adapter_stl") rover_rail_adapter();
|
||||
else if (RENDER == "tank_adapter_stl") tank_rail_adapter();
|
||||
|
||||
// ============================================================
|
||||
// ASSEMBLY PREVIEW
|
||||
// ============================================================
|
||||
module assembly() {
|
||||
// Rail section
|
||||
color("Silver", 0.85) rail_section(RAIL_LEN);
|
||||
|
||||
// Rover adapter under rail
|
||||
color("SteelBlue", 0.70)
|
||||
translate([0, 0, -ADAPT_T])
|
||||
rover_rail_adapter();
|
||||
|
||||
// Ghost module sliding on
|
||||
%color("OliveDrab", 0.3)
|
||||
translate([0, 60, RAIL_T])
|
||||
cube([RAIL_W + 10, 100, 40], center = true);
|
||||
|
||||
// Connector position marker
|
||||
%color("Gold", 0.5)
|
||||
translate([0, CONN_Y, RAIL_T - DOVE_H])
|
||||
cube([CONN_HOUSING_W, CONN_HOUSING_D, CONN_HOUSING_H],
|
||||
center = true);
|
||||
|
||||
// Detent dimple markers
|
||||
for (dy = [MOUNT_INSET : DETENT_PITCH : RAIL_LEN - MOUNT_INSET])
|
||||
%color("Red", 0.6)
|
||||
translate([0, dy, RAIL_T - DOVE_H])
|
||||
cylinder(d = DETENT_HOLE_D, h = DETENT_DEPTH);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// RAIL CROSS-SECTION 2D (DXF export)
|
||||
// ============================================================
|
||||
// Outer profile minus dovetail slot.
|
||||
// For CNC milling from 50×12 mm aluminium bar, or waterjet from plate.
|
||||
// Also used for PETG prototype extrusion.
|
||||
module rail_profile_2d() {
|
||||
difference() {
|
||||
// Outer rail cross-section (rounded rect)
|
||||
minkowski() {
|
||||
square([RAIL_W - 2*RAIL_R, RAIL_T - 2*RAIL_R],
|
||||
center = true);
|
||||
circle(r = RAIL_R);
|
||||
}
|
||||
|
||||
// Dovetail slot (trapezoid, open at top)
|
||||
translate([0, RAIL_T/2])
|
||||
_dovetail_slot_2d();
|
||||
}
|
||||
}
|
||||
|
||||
// ── Dovetail slot 2D (trapezoid with open top) ───────────────────────────────
|
||||
module _dovetail_slot_2d() {
|
||||
// Trapezoid: wider at top (open face), narrower at bottom.
|
||||
// Points listed clockwise from top-left:
|
||||
polygon([
|
||||
[-DOVE_SLOT_TOP/2, 0], // top-left
|
||||
[ DOVE_SLOT_TOP/2, 0], // top-right
|
||||
[ DOVE_SLOT_BOT/2, -DOVE_H], // bottom-right
|
||||
[-DOVE_SLOT_BOT/2, -DOVE_H], // bottom-left
|
||||
]);
|
||||
}
|
||||
|
||||
// ── Dovetail slot for difference() operations (3D volume) ────────────────────
|
||||
module _dovetail_slot_3d(length) {
|
||||
translate([0, -e, RAIL_T])
|
||||
linear_extrude(DOVE_H + e)
|
||||
offset(delta = e)
|
||||
_dovetail_slot_2d();
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// RAIL SECTION (3D — printable or aluminium)
|
||||
// ============================================================
|
||||
module rail_section(len = RAIL_LEN) {
|
||||
difference() {
|
||||
// ── Extruded profile ────────────────────────────────────────
|
||||
linear_extrude(len)
|
||||
rotate([0, 0, 90])
|
||||
rail_profile_2d();
|
||||
|
||||
// ── Dovetail slot ────────────────────────────────────────────
|
||||
translate([0, -e, RAIL_T])
|
||||
rotate([0, 0, 0])
|
||||
linear_extrude(len + 2*e)
|
||||
rotate([0, 0, 90])
|
||||
offset(delta = e)
|
||||
_dovetail_slot_2d();
|
||||
|
||||
// ── Deck mounting holes (M4 FHCS, from bottom) ───────────────
|
||||
for (my = [MOUNT_INSET : MOUNT_PITCH : len - MOUNT_INSET])
|
||||
translate([0, my, -e])
|
||||
cylinder(d = MOUNT_D, h = RAIL_T - DOVE_H + 2*e);
|
||||
// Countersinks on bottom face
|
||||
for (my = [MOUNT_INSET : MOUNT_PITCH : len - MOUNT_INSET])
|
||||
translate([0, my, -e])
|
||||
cylinder(d1 = CSINK_D, d2 = MOUNT_D,
|
||||
h = (CSINK_D - MOUNT_D) / (2 * tan(41)));
|
||||
|
||||
// ── Spring detent dimples (slot bottom, at DETENT_PITCH) ──────
|
||||
for (dy = [MOUNT_INSET : DETENT_PITCH : len - MOUNT_INSET])
|
||||
translate([0, dy, RAIL_T - DOVE_H - e])
|
||||
cylinder(d = DETENT_HOLE_D, h = DETENT_DEPTH + e);
|
||||
|
||||
// ── Safety-lock groove (continuous slot, both sides of rail) ──
|
||||
// M4 thumbscrew tip seats anywhere along groove
|
||||
for (sx = [-1, 1])
|
||||
translate([sx * (RAIL_W/2 + e), -e,
|
||||
RAIL_T - DOVE_H/2])
|
||||
rotate([0, 90, 0])
|
||||
hull() {
|
||||
translate([0, 0, 0])
|
||||
cylinder(d = LOCK_GROOVE_D, h = RAIL_W + 2*e);
|
||||
}
|
||||
|
||||
// ── Connector housing pocket (at CONN_Y) ──────────────────────
|
||||
translate([0, CONN_Y, RAIL_T - DOVE_H - e])
|
||||
cube([CONN_HOUSING_W + 0.4,
|
||||
CONN_HOUSING_D + 0.4,
|
||||
CONN_HOUSING_H + e], center = true);
|
||||
|
||||
// ── Lightening slots (rail body between mounting holes) ────────
|
||||
for (my = [MOUNT_INSET + 25 : MOUNT_PITCH : len - MOUNT_INSET - 25])
|
||||
translate([0, my, RAIL_T/4])
|
||||
cube([RAIL_W - 16, 20, RAIL_T/2 + 2*e], center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// CONNECTOR HOUSING (pogo-pin insert, press-fits into rail pocket)
|
||||
// ============================================================
|
||||
// 4× spring-loaded pogo pins (P75-E2, Ø2 mm, 6 mm travel).
|
||||
// Printed housing press-fits into rail pocket; pins protrude up into module.
|
||||
// Module has 4× Ø4 mm brass target pads at matching pitch.
|
||||
//
|
||||
// Pin map (left to right, looking at rail top from +Z):
|
||||
// Pin 1: GND Pin 2: +5 V Pin 3: +12 V Pin 4: UART
|
||||
module connector_housing() {
|
||||
hw = CONN_HOUSING_W;
|
||||
hd = CONN_HOUSING_D;
|
||||
hh = CONN_HOUSING_H;
|
||||
|
||||
difference() {
|
||||
// Housing body (press-fit into rail pocket)
|
||||
cube([hw, hd, hh], center = true);
|
||||
|
||||
// 4× pogo pin bores (through housing, top to bottom)
|
||||
for (i = [0 : CONN_N_PINS - 1]) {
|
||||
px = (i - (CONN_N_PINS - 1) / 2) * CONN_PIN_SPC;
|
||||
translate([px, 0, -hh/2 - e])
|
||||
cylinder(d = CONN_PIN_D, h = hh + 2*e);
|
||||
}
|
||||
|
||||
// Wire exit slot (bottom, routes into rail body)
|
||||
translate([0, 0, -hh/2 - e])
|
||||
cube([hw - 6, hd/2, hh/3 + e], center = true);
|
||||
|
||||
// Retention barbs (prevent housing pulling out of pocket)
|
||||
for (sx = [-1, 1])
|
||||
translate([sx * (hw/2 - 1), 0, hh/4])
|
||||
rotate([0, sx * 15, 0])
|
||||
cube([2, hd + 2*e, 2.5], center = true);
|
||||
}
|
||||
|
||||
// Pin polarity label recess (on top face, GND side)
|
||||
difference() {
|
||||
translate([0, 0, 0]) cube([0, 0, 0]); // null
|
||||
translate([-(CONN_N_PINS * CONN_PIN_SPC)/2 + 1, 0, hh/2 - 0.4])
|
||||
cube([3, hd * 0.6, 0.5 + e], center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// DETENT PLUNGER (lives in module tongue; print 2× per module)
|
||||
// ============================================================
|
||||
// Press-fit into Ø6.2 mm bore in module tongue.
|
||||
// Includes spring pocket; ball seated on top.
|
||||
// Spring: Ø5.5 mm OD, 12 mm free length, ~2 N/mm (light detent).
|
||||
// Ball: Ø6 mm steel (standard bearing ball, purchase).
|
||||
module detent_plunger() {
|
||||
bore_d = DETENT_BALL_D + 0.2; // 6.2 mm
|
||||
body_od = DETENT_SPG_OD;
|
||||
body_len = DETENT_SPG_L;
|
||||
spg_d = 5.8; // spring OD
|
||||
spg_pocket = 10.0; // spring pocket depth (bottom of housing)
|
||||
|
||||
difference() {
|
||||
cylinder(d = body_od, h = body_len);
|
||||
|
||||
// Ball socket (top — partial sphere, retains ball)
|
||||
translate([0, 0, body_len])
|
||||
sphere(d = bore_d);
|
||||
translate([0, 0, body_len - bore_d/4])
|
||||
cylinder(d = bore_d, h = bore_d/2 + e);
|
||||
|
||||
// Spring pocket (bottom)
|
||||
translate([0, 0, -e])
|
||||
cylinder(d = spg_d + 0.3, h = spg_pocket + e);
|
||||
|
||||
// Retention lip (allows push-in but prevents pullout before spring seated)
|
||||
translate([0, 0, spg_pocket])
|
||||
cylinder(d1 = spg_d + 0.3, d2 = spg_d - 1,
|
||||
h = 1.5);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// CROSS-VARIANT DECK ADAPTER PLATES
|
||||
// ============================================================
|
||||
// Thin plates that bolt to the robot deck and provide M4 threaded
|
||||
// studs (or through holes) for the rail mounting holes.
|
||||
// All adapters: RAIL_LEN × (RAIL_W + 2×ADAPT_OVHG) footprint.
|
||||
|
||||
module _adapter_base() {
|
||||
adapt_l = RAIL_LEN;
|
||||
adapt_w = RAIL_W + 2*ADAPT_OVHG;
|
||||
difference() {
|
||||
// Plate
|
||||
cube([adapt_w, adapt_l, ADAPT_T], center = true);
|
||||
|
||||
// Rail mounting holes (M4 FHCS up through adapter into rail bottom)
|
||||
for (my = [MOUNT_INSET : MOUNT_PITCH : adapt_l - MOUNT_INSET])
|
||||
translate([0, my - adapt_l/2, -ADAPT_T/2 - e])
|
||||
cylinder(d = MOUNT_D, h = ADAPT_T + 2*e);
|
||||
|
||||
// Corner lightening
|
||||
for (cx = [-1, 1]) for (cy = [-1, 1])
|
||||
translate([cx * (adapt_w/2 - 12),
|
||||
cy * (adapt_l/2 - 20), 0])
|
||||
cylinder(d = 10, h = ADAPT_T + 2*e, center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// SaltyLab adapter: clears Ø25 mm stem, 4× M4 to lab chassis ring
|
||||
module lab_rail_adapter() {
|
||||
difference() {
|
||||
_adapter_base();
|
||||
// Stem bore clearance (at centre of adapter)
|
||||
cylinder(d = LAB_STEM_BORE, h = ADAPT_T + 2*e, center = true);
|
||||
// 4× M4 mounting to lab chassis top ring (Ø44 mm bolt circle)
|
||||
for (a = [45, 135, 225, 315])
|
||||
translate([22*cos(a), 22*sin(a), -ADAPT_T/2 - e])
|
||||
cylinder(d = M4_D, h = ADAPT_T + 2*e);
|
||||
}
|
||||
}
|
||||
|
||||
// SaltyRover adapter: 4× M4 to rover deck bolt grid
|
||||
module rover_rail_adapter() {
|
||||
adapt_l = RAIL_LEN;
|
||||
adapt_w = RAIL_W + 2*ADAPT_OVHG;
|
||||
difference() {
|
||||
_adapter_base();
|
||||
// 2 rows × 3 cols of M4 bolts into rover deck
|
||||
for (rx = [-ROVER_BOLT_SPC/2, ROVER_BOLT_SPC/2])
|
||||
for (ry = [-adapt_l/3, 0, adapt_l/3])
|
||||
translate([rx, ry, -ADAPT_T/2 - e])
|
||||
cylinder(d = M4_D, h = ADAPT_T + 2*e);
|
||||
}
|
||||
}
|
||||
|
||||
// SaltyTank adapter: M4 to tank deck; relieved for deck cable slots
|
||||
module tank_rail_adapter() {
|
||||
adapt_l = RAIL_LEN;
|
||||
adapt_w = RAIL_W + 2*ADAPT_OVHG;
|
||||
difference() {
|
||||
_adapter_base();
|
||||
// 2 rows × 3 cols of M4 bolts into tank deck
|
||||
for (rx = [-TANK_BOLT_SPC/2, TANK_BOLT_SPC/2])
|
||||
for (ry = [-adapt_l/3, 0, adapt_l/3])
|
||||
translate([rx, ry, -ADAPT_T/2 - e])
|
||||
cylinder(d = M4_D, h = ADAPT_T + 2*e);
|
||||
// Deck cable slot clearance (tank deck has centre cable channel)
|
||||
translate([0, 0, 0])
|
||||
cube([10, adapt_l - 40, ADAPT_T + 2*e], center = true);
|
||||
}
|
||||
}
|
||||
106
include/audio.h
Normal file
106
include/audio.h
Normal file
@ -0,0 +1,106 @@
|
||||
#ifndef AUDIO_H
|
||||
#define AUDIO_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* audio.h — I2S audio output driver (Issue #143)
|
||||
*
|
||||
* Hardware: SPI3 repurposed as I2S3 master TX (blackbox flash not used
|
||||
* on balance bot). Supports MAX98357A (I2S class-D amp) and PCM5102A
|
||||
* (I2S DAC + external amp) — both use standard Philips I2S.
|
||||
*
|
||||
* Pin assignment (SPI3 / I2S3, defined in config.h):
|
||||
* PC10 I2S3_CK (BCLK) AF6
|
||||
* PA15 I2S3_WS (LRCLK) AF6
|
||||
* PB5 I2S3_SD (DIN) AF6
|
||||
* PC5 AUDIO_MUTE (GPIO) active-high = enabled; low = muted/shutdown
|
||||
*
|
||||
* PLLI2S: N=192, R=2 → 96 MHz I2S clock → 22058 Hz (< 0.04% from 22050)
|
||||
* DMA1 Stream7 Channel0 (SPI3_TX), circular, double-buffer ping-pong.
|
||||
*
|
||||
* Mixer priority (highest to lowest):
|
||||
* 1. PCM audio chunks from Jetson (via JLINK_CMD_AUDIO, written to FIFO)
|
||||
* 2. Notification tones (queued by audio_play_tone)
|
||||
* 3. Silence
|
||||
*
|
||||
* Volume applies to all sources via integer sample scaling (0–100).
|
||||
*/
|
||||
|
||||
/* Maximum int16_t samples per JLINK_CMD_AUDIO frame (252-byte payload / 2) */
|
||||
#define AUDIO_CHUNK_MAX_SAMPLES 126u
|
||||
|
||||
/* Pre-defined notification tones */
|
||||
typedef enum {
|
||||
AUDIO_TONE_BEEP_SHORT = 0, /* 880 Hz, 100 ms — acknowledge / UI feedback */
|
||||
AUDIO_TONE_BEEP_LONG = 1, /* 880 Hz, 500 ms — generic warning */
|
||||
AUDIO_TONE_STARTUP = 2, /* C5→E5→G5 arpeggio (3 × 120 ms) */
|
||||
AUDIO_TONE_ARM = 3, /* 880 Hz→1047 Hz two-beep ascending */
|
||||
AUDIO_TONE_DISARM = 4, /* 880 Hz→659 Hz two-beep descending */
|
||||
AUDIO_TONE_FAULT = 5, /* 200 Hz buzz, 500 ms — tilt/safety fault */
|
||||
AUDIO_TONE_COUNT
|
||||
} AudioTone;
|
||||
|
||||
/*
|
||||
* audio_init()
|
||||
*
|
||||
* Configure PLLI2S, GPIO, DMA1 Stream7, and SPI3/I2S3.
|
||||
* Pre-fills DMA buffer with silence, starts circular DMA TX, then
|
||||
* unmutes the amp. Call once before safety_init().
|
||||
*/
|
||||
void audio_init(void);
|
||||
|
||||
/*
|
||||
* audio_mute(mute)
|
||||
*
|
||||
* Drive AUDIO_MUTE_PIN: false = hardware-muted (SD/XSMT low),
|
||||
* true = active (amp enabled). Does NOT stop DMA; allows instant
|
||||
* un-mute without DMA restart clicks.
|
||||
*/
|
||||
void audio_mute(bool active);
|
||||
|
||||
/*
|
||||
* audio_set_volume(vol)
|
||||
*
|
||||
* Software volume 0–100. Applied in ISR fill path via integer scaling.
|
||||
* 0 = silence, 100 = full scale (±16384 for square wave, passthrough for PCM).
|
||||
*/
|
||||
void audio_set_volume(uint8_t vol);
|
||||
|
||||
/*
|
||||
* audio_play_tone(tone)
|
||||
*
|
||||
* Queue a pre-defined notification tone. The tone plays after any tones
|
||||
* already in the queue. Returns false if the tone queue is full (depth 4).
|
||||
* Tones are pre-empted by incoming PCM audio from the Jetson.
|
||||
*/
|
||||
bool audio_play_tone(AudioTone tone);
|
||||
|
||||
/*
|
||||
* audio_write_pcm(samples, n)
|
||||
*
|
||||
* Write mono 16-bit 22050 Hz PCM samples into the Jetson PCM FIFO.
|
||||
* Called from jlink_process() dispatch on JLINK_CMD_AUDIO (main-loop context).
|
||||
* Returns the number of samples actually accepted (0 if FIFO is full).
|
||||
*/
|
||||
uint16_t audio_write_pcm(const int16_t *samples, uint16_t n);
|
||||
|
||||
/*
|
||||
* audio_tick(now_ms)
|
||||
*
|
||||
* Advance the tone sequencer state machine. Must be called every 1 ms
|
||||
* from the main loop. Manages step transitions and gap timing; updates
|
||||
* the volatile active-tone parameters read by the ISR fill path.
|
||||
*/
|
||||
void audio_tick(uint32_t now_ms);
|
||||
|
||||
/*
|
||||
* audio_is_playing()
|
||||
*
|
||||
* Returns true if the DMA is running (always true after audio_init()
|
||||
* unless the amp is hardware-muted or the I2S peripheral has an error).
|
||||
*/
|
||||
bool audio_is_playing(void);
|
||||
|
||||
#endif /* AUDIO_H */
|
||||
@ -189,4 +189,19 @@
|
||||
/* Full blend transition time: MANUAL→AUTO takes this many ms */
|
||||
#define MODE_BLEND_MS 500
|
||||
|
||||
// --- Audio Amplifier (I2S3, Issue #143) ---
|
||||
// SPI3 repurposed as I2S3; blackbox flash unused on balance bot
|
||||
#define AUDIO_BCLK_PORT GPIOC
|
||||
#define AUDIO_BCLK_PIN GPIO_PIN_10 // I2S3_CK (PC10, AF6)
|
||||
#define AUDIO_LRCK_PORT GPIOA
|
||||
#define AUDIO_LRCK_PIN GPIO_PIN_15 // I2S3_WS (PA15, AF6)
|
||||
#define AUDIO_DOUT_PORT GPIOB
|
||||
#define AUDIO_DOUT_PIN GPIO_PIN_5 // I2S3_SD (PB5, AF6)
|
||||
#define AUDIO_MUTE_PORT GPIOC
|
||||
#define AUDIO_MUTE_PIN GPIO_PIN_5 // active-high = amp enabled
|
||||
// PLLI2S: N=192, R=2 → I2S clock=96 MHz → FS≈22058 Hz (< 0.04% error)
|
||||
#define AUDIO_SAMPLE_RATE 22050u // nominal sample rate (Hz)
|
||||
#define AUDIO_BUF_HALF 441u // DMA half-buffer: 20ms at 22050 Hz
|
||||
#define AUDIO_VOLUME_DEFAULT 80u // default volume 0-100
|
||||
|
||||
#endif // CONFIG_H
|
||||
|
||||
@ -53,6 +53,7 @@
|
||||
#define JLINK_CMD_PID_SET 0x05u
|
||||
#define JLINK_CMD_DFU_ENTER 0x06u
|
||||
#define JLINK_CMD_ESTOP 0x07u
|
||||
#define JLINK_CMD_AUDIO 0x08u /* PCM audio chunk: int16 samples, up to 126 */
|
||||
|
||||
/* ---- Telemetry IDs (STM32 → Jetson) ---- */
|
||||
#define JLINK_TLM_STATUS 0x80u
|
||||
|
||||
@ -0,0 +1,49 @@
|
||||
# saltybot_night_vision — runtime parameters
|
||||
#
|
||||
# Ambient light detection requires ONE of:
|
||||
# /camera/color/image_raw — IMX219 front camera (primary)
|
||||
# /camera/infra1/image_rect_raw — D435i IR fallback
|
||||
#
|
||||
# D435i IR emitter is controlled via the realsense2_camera parameter service.
|
||||
# Make sure the node is running as /camera/camera (default).
|
||||
#
|
||||
# GPIO headlight on physical pin 33 of the Jetson Orin Nano Super 40-pin header.
|
||||
# Requires Jetson.GPIO (jetson-gpio pip package) or /sys/class/gpio write access.
|
||||
|
||||
light_monitor:
|
||||
ros__parameters:
|
||||
primary_camera_topic: '/camera/color/image_raw'
|
||||
fallback_ir_topic: '/camera/infra1/image_rect_raw'
|
||||
publish_hz: 2.0 # ambient light evaluation rate
|
||||
initial_mode: 'day'
|
||||
history_len: 6 # frames to median-filter intensity
|
||||
|
||||
night_vision_controller:
|
||||
ros__parameters:
|
||||
headlight_pin: 33 # physical board pin (Jetson 40-pin J26)
|
||||
day_headlight: 0.0 # off
|
||||
twilight_headlight: 0.25 # 25 % — subtle fill
|
||||
night_headlight: 0.50 # 50 % — normal dark room
|
||||
ir_only_headlight: 1.0 # 100 % — pitch black
|
||||
|
||||
ir_slam_bridge:
|
||||
ros__parameters:
|
||||
active_modes: 'night,ir_only' # publish IR→RGB only in these modes
|
||||
output_encoding: 'rgb8' # RTAB-Map expects rgb8 or bgr8
|
||||
|
||||
|
||||
# ── RTAB-Map integration note ──────────────────────────────────────────────────
|
||||
# When the bridge is active (/saltybot/ir_bridge/image_raw), redirect RTAB-Map's
|
||||
# RGB input by adding to slam_rtabmap.launch.py Node remappings:
|
||||
#
|
||||
# remappings=[
|
||||
# ('rgb/image', '/saltybot/ir_bridge/image_raw'),
|
||||
# ('rgb/camera_info', '/saltybot/ir_bridge/camera_info'),
|
||||
# ]
|
||||
#
|
||||
# Or set the topic at launch:
|
||||
# ros2 launch saltybot_bringup slam_rtabmap.launch.py \
|
||||
# rgb_image_topic:=/saltybot/ir_bridge/image_raw
|
||||
#
|
||||
# This ensures loop-closure detection continues using the IR feature stream
|
||||
# instead of a dark / blank colour image.
|
||||
@ -0,0 +1,68 @@
|
||||
"""
|
||||
night_vision.launch.py — SaltyBot night vision stack.
|
||||
|
||||
Starts:
|
||||
light_monitor — histogram ambient-light analysis → /saltybot/vision_mode
|
||||
night_vision_ctrl — D435i emitter + IMX219 gain + GPIO headlight
|
||||
ir_slam_bridge — IR→RGB republish for RTAB-Map (active in dark modes)
|
||||
|
||||
Launch args:
|
||||
primary_camera_topic str '/camera/color/image_raw'
|
||||
headlight_pin int '33'
|
||||
publish_hz float '2.0'
|
||||
active_ir_modes str 'night,ir_only'
|
||||
|
||||
Verify:
|
||||
ros2 topic echo /saltybot/vision_mode
|
||||
ros2 topic echo /saltybot/ambient_light
|
||||
ros2 topic echo /saltybot/night_vision_status
|
||||
ros2 topic hz /saltybot/ir_bridge/image_raw # should be ~30 Hz in dark
|
||||
"""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
args = [
|
||||
DeclareLaunchArgument('primary_camera_topic',
|
||||
default_value='/camera/color/image_raw'),
|
||||
DeclareLaunchArgument('headlight_pin', default_value='33'),
|
||||
DeclareLaunchArgument('publish_hz', default_value='2.0'),
|
||||
DeclareLaunchArgument('active_ir_modes', default_value='night,ir_only'),
|
||||
]
|
||||
|
||||
light_monitor = Node(
|
||||
package='saltybot_night_vision',
|
||||
executable='light_monitor',
|
||||
name='light_monitor',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'primary_camera_topic': LaunchConfiguration('primary_camera_topic'),
|
||||
'publish_hz': LaunchConfiguration('publish_hz'),
|
||||
}],
|
||||
)
|
||||
|
||||
nv_ctrl = Node(
|
||||
package='saltybot_night_vision',
|
||||
executable='night_vision_ctrl',
|
||||
name='night_vision_controller',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'headlight_pin': LaunchConfiguration('headlight_pin'),
|
||||
}],
|
||||
)
|
||||
|
||||
ir_bridge = Node(
|
||||
package='saltybot_night_vision',
|
||||
executable='ir_slam_bridge',
|
||||
name='ir_slam_bridge',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'active_modes': LaunchConfiguration('active_ir_modes'),
|
||||
}],
|
||||
)
|
||||
|
||||
return LaunchDescription(args + [light_monitor, nv_ctrl, ir_bridge])
|
||||
27
jetson/ros2_ws/src/saltybot_night_vision/package.xml
Normal file
27
jetson/ros2_ws/src/saltybot_night_vision/package.xml
Normal file
@ -0,0 +1,27 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>saltybot_night_vision</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
Night vision mode for SaltyBot: ambient-light histogram detection,
|
||||
D435i IR emitter control, IMX219 auto-gain/exposure, GPIO headlight,
|
||||
and IR-only SLAM bridging.
|
||||
</description>
|
||||
<maintainer email="robot@saltylab.local">SaltyLab</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>python3-opencv</exec_depend>
|
||||
|
||||
<test_depend>pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,158 @@
|
||||
"""
|
||||
camera_controller.py — D435i parameter service client + IMX219 v4l2 control.
|
||||
|
||||
D435i (realsense2_camera node):
|
||||
depth_module.emitter_enabled int 0=off 1=on 2=auto
|
||||
depth_module.emitter_always_on bool keep emitter on between frames
|
||||
|
||||
IMX219 (CSI cameras via v4l2):
|
||||
exposure_auto 0=manual 1=auto
|
||||
exposure microseconds (manual)
|
||||
analogue_gain raw sensor gain (16–170 typical range)
|
||||
|
||||
The IMX219 controls are exercised via subprocess `v4l2-ctl`, which is
|
||||
always available on JetPack 6.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import subprocess
|
||||
import logging
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rcl_interfaces.msg import Parameter, ParameterValue, ParameterType
|
||||
from rcl_interfaces.srv import SetParameters
|
||||
|
||||
_LOG = logging.getLogger(__name__)
|
||||
|
||||
# D435i camera node (realsense2_camera default namespace)
|
||||
_RS_NODE = '/camera/camera'
|
||||
|
||||
# IMX219 v4l2 device paths for each camera position
|
||||
# Front camera is the primary source for ambient-light detection
|
||||
_IMX219_DEVICES: dict[str, str] = {
|
||||
'front': '/dev/video0',
|
||||
'right': '/dev/video2',
|
||||
'rear': '/dev/video4',
|
||||
'left': '/dev/video6',
|
||||
}
|
||||
|
||||
# Auto-exposure values for IMX219
|
||||
_IMX219_AE_AUTO = 1
|
||||
_IMX219_AE_MANUAL = 0
|
||||
|
||||
|
||||
@dataclass
|
||||
class CameraProfile:
|
||||
"""Parameter set for a given vision mode."""
|
||||
emitter_enabled: int # 0=off, 1=on, 2=auto
|
||||
imx_ae_mode: int # 0=manual, 1=auto
|
||||
imx_exposure_us: Optional[int] # None → leave AE to control it
|
||||
imx_gain: Optional[int] # None → leave AE to control it
|
||||
|
||||
|
||||
# Per-mode camera profiles
|
||||
PROFILES: dict[str, CameraProfile] = {
|
||||
'day': CameraProfile(
|
||||
emitter_enabled=2, # RS depth auto
|
||||
imx_ae_mode=_IMX219_AE_AUTO,
|
||||
imx_exposure_us=None,
|
||||
imx_gain=None,
|
||||
),
|
||||
'twilight': CameraProfile(
|
||||
emitter_enabled=1, # emitter on for better depth
|
||||
imx_ae_mode=_IMX219_AE_MANUAL,
|
||||
imx_exposure_us=20_000, # 20 ms
|
||||
imx_gain=64,
|
||||
),
|
||||
'night': CameraProfile(
|
||||
emitter_enabled=1,
|
||||
imx_ae_mode=_IMX219_AE_MANUAL,
|
||||
imx_exposure_us=50_000, # 50 ms (sacrifice motion sharpness)
|
||||
imx_gain=128,
|
||||
),
|
||||
'ir_only': CameraProfile(
|
||||
emitter_enabled=1, # emitter critical for IR stereo in dark
|
||||
imx_ae_mode=_IMX219_AE_MANUAL,
|
||||
imx_exposure_us=80_000,
|
||||
imx_gain=170, # near-max analogue gain
|
||||
),
|
||||
}
|
||||
|
||||
|
||||
class CameraController:
|
||||
"""
|
||||
Non-blocking camera controller.
|
||||
|
||||
set_profile() sends async ROS2 parameter requests and v4l2 commands.
|
||||
It should be called from within a ROS2 node (pass the node reference).
|
||||
"""
|
||||
|
||||
def __init__(self, node: Node):
|
||||
self._node = node
|
||||
self._cli: Optional[rclpy.client.Client] = None
|
||||
self._pending_mode: Optional[str] = None
|
||||
self._current_mode: Optional[str] = None
|
||||
self._init_rs_client()
|
||||
|
||||
# ── Public API ────────────────────────────────────────────────────────────
|
||||
|
||||
def set_profile(self, mode: str) -> None:
|
||||
"""Apply the camera profile for `mode`. Fire-and-forget async."""
|
||||
if mode not in PROFILES:
|
||||
self._node.get_logger().warning(f'[camera_ctrl] Unknown mode: {mode}')
|
||||
return
|
||||
if mode == self._current_mode:
|
||||
return
|
||||
profile = PROFILES[mode]
|
||||
self._set_rs_emitter(profile.emitter_enabled)
|
||||
self._set_imx219(profile)
|
||||
self._current_mode = mode
|
||||
self._node.get_logger().info(
|
||||
f'[camera_ctrl] Applied profile={mode} '
|
||||
f'emitter={profile.emitter_enabled} '
|
||||
f'gain={profile.imx_gain} exp={profile.imx_exposure_us}'
|
||||
)
|
||||
|
||||
# ── D435i (RealSense) ─────────────────────────────────────────────────────
|
||||
|
||||
def _init_rs_client(self) -> None:
|
||||
self._cli = self._node.create_client(
|
||||
SetParameters, f'{_RS_NODE}/set_parameters'
|
||||
)
|
||||
|
||||
def _set_rs_emitter(self, value: int) -> None:
|
||||
if not self._cli.service_is_ready():
|
||||
self._node.get_logger().debug(
|
||||
'[camera_ctrl] RS parameter service not ready — skipping emitter set'
|
||||
)
|
||||
return
|
||||
pval = ParameterValue(type=ParameterType.PARAMETER_INTEGER, integer_value=value)
|
||||
param = Parameter(name='depth_module.emitter_enabled', value=pval)
|
||||
req = SetParameters.Request(parameters=[param])
|
||||
self._cli.call_async(req) # fire-and-forget
|
||||
|
||||
# ── IMX219 via v4l2-ctl ──────────────────────────────────────────────────
|
||||
|
||||
def _set_imx219(self, profile: CameraProfile) -> None:
|
||||
for dev in _IMX219_DEVICES.values():
|
||||
self._v4l2_set(dev, 'exposure_auto', profile.imx_ae_mode)
|
||||
if profile.imx_exposure_us is not None:
|
||||
self._v4l2_set(dev, 'exposure', profile.imx_exposure_us)
|
||||
if profile.imx_gain is not None:
|
||||
self._v4l2_set(dev, 'analogue_gain', profile.imx_gain)
|
||||
|
||||
def _v4l2_set(self, dev: str, ctrl: str, value: int) -> None:
|
||||
cmd = ['v4l2-ctl', '-d', dev, f'--set-ctrl={ctrl}={value}']
|
||||
try:
|
||||
result = subprocess.run(cmd, capture_output=True, timeout=1.0)
|
||||
if result.returncode != 0:
|
||||
self._node.get_logger().debug(
|
||||
f'[camera_ctrl] v4l2 {dev} {ctrl}={value}: '
|
||||
+ result.stderr.decode().strip()
|
||||
)
|
||||
except (subprocess.TimeoutExpired, FileNotFoundError) as exc:
|
||||
self._node.get_logger().debug(f'[camera_ctrl] v4l2-ctl unavailable: {exc}')
|
||||
@ -0,0 +1,192 @@
|
||||
"""
|
||||
gpio_headlight.py — Jetson Orin Nano Super 40-pin GPIO headlight control.
|
||||
|
||||
Physical pin 33 on the J26 40-pin header is used for the headlight.
|
||||
On JetPack 6 this corresponds to sysfs GPIO chip gpio-tegra234 / offset 12.
|
||||
|
||||
Brightness levels (0.0–1.0):
|
||||
0.0 → headlight off
|
||||
0.25 → dim (e.g. indicator, twilight assist)
|
||||
0.50 → medium (normal dark indoor)
|
||||
1.0 → full (outdoor night, ir_only mode)
|
||||
|
||||
Implementation:
|
||||
1. Tries Jetson.GPIO PWM (RPi-compatible API, available if jetson-gpio installed)
|
||||
2. Falls back to sysfs binary (on/off only) via /sys/class/gpio
|
||||
3. If sysfs export fails (permissions / virtual env), logs a warning and no-ops.
|
||||
|
||||
Sysfs GPIO path on Orin Nano Super (JetPack 6):
|
||||
/sys/class/gpio/gpiochip<N>/ — use the tegra234 chip
|
||||
Or the legacy /sys/class/gpio/gpio<number>/ path after export.
|
||||
|
||||
Note: udev rule or `sudo` may be needed in production.
|
||||
Add to /etc/udev/rules.d/99-jetson-gpio.rules:
|
||||
SUBSYSTEM=="gpio", ACTION=="add", RUN+="/bin/chmod 777 /sys/class/gpio"
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import os
|
||||
from typing import Optional
|
||||
|
||||
_LOG = logging.getLogger(__name__)
|
||||
|
||||
# Physical board pin for the headlight
|
||||
_BOARD_PIN = 33
|
||||
|
||||
# Brightness → duty cycle string mapping for sysfs PWM (when available)
|
||||
_LEVEL_DUTY: dict[str, float] = {
|
||||
'off': 0.0,
|
||||
'dim': 0.25,
|
||||
'medium': 0.50,
|
||||
'full': 1.0,
|
||||
}
|
||||
|
||||
|
||||
class GPIOHeadlight:
|
||||
"""Controls the GPIO headlight with graceful fallbacks."""
|
||||
|
||||
def __init__(self, pin: int = _BOARD_PIN):
|
||||
self._pin = pin
|
||||
self._brightness: float = 0.0
|
||||
self._backend: str = 'none'
|
||||
self._pwm = None
|
||||
self._gpio_path: Optional[str] = None
|
||||
self._init()
|
||||
|
||||
# ── Public API ────────────────────────────────────────────────────────────
|
||||
|
||||
def set_brightness(self, level: float) -> None:
|
||||
"""Set headlight brightness in [0.0, 1.0]."""
|
||||
level = max(0.0, min(1.0, level))
|
||||
if level == self._brightness:
|
||||
return
|
||||
self._brightness = level
|
||||
self._apply(level)
|
||||
|
||||
def set_level(self, name: str) -> None:
|
||||
"""Set headlight to a named level: 'off', 'dim', 'medium', 'full'."""
|
||||
self.set_brightness(_LEVEL_DUTY.get(name, 0.0))
|
||||
|
||||
def off(self) -> None:
|
||||
self.set_brightness(0.0)
|
||||
|
||||
def cleanup(self) -> None:
|
||||
self.off()
|
||||
if self._pwm is not None:
|
||||
try:
|
||||
self._pwm.stop()
|
||||
import Jetson.GPIO as GPIO
|
||||
GPIO.cleanup()
|
||||
except Exception:
|
||||
pass
|
||||
elif self._gpio_path:
|
||||
self._gpio_write('0')
|
||||
|
||||
# ── Init backends ─────────────────────────────────────────────────────────
|
||||
|
||||
def _init(self) -> None:
|
||||
# Try Jetson.GPIO PWM first
|
||||
try:
|
||||
import Jetson.GPIO as GPIO
|
||||
GPIO.setmode(GPIO.BOARD)
|
||||
GPIO.setup(self._pin, GPIO.OUT, initial=GPIO.LOW)
|
||||
self._pwm = GPIO.PWM(self._pin, 1000) # 1 kHz PWM
|
||||
self._pwm.start(0)
|
||||
self._backend = 'jetson_gpio_pwm'
|
||||
_LOG.info(f'[headlight] Jetson.GPIO PWM on pin {self._pin}')
|
||||
return
|
||||
except ImportError:
|
||||
_LOG.debug('[headlight] Jetson.GPIO not available')
|
||||
except Exception as exc:
|
||||
_LOG.warning(f'[headlight] Jetson.GPIO init failed: {exc}')
|
||||
|
||||
# Fall back to sysfs binary
|
||||
gpio_num = self._board_pin_to_sysfs(self._pin)
|
||||
if gpio_num is not None:
|
||||
path = f'/sys/class/gpio/gpio{gpio_num}'
|
||||
if self._sysfs_export(gpio_num):
|
||||
self._gpio_path = path
|
||||
self._backend = 'sysfs'
|
||||
_LOG.info(f'[headlight] sysfs GPIO /sys/class/gpio/gpio{gpio_num}')
|
||||
return
|
||||
|
||||
_LOG.warning('[headlight] No GPIO backend available — headlight is simulated')
|
||||
self._backend = 'sim'
|
||||
|
||||
# ── Apply ─────────────────────────────────────────────────────────────────
|
||||
|
||||
def _apply(self, level: float) -> None:
|
||||
if self._backend == 'jetson_gpio_pwm':
|
||||
try:
|
||||
self._pwm.ChangeDutyCycle(level * 100.0)
|
||||
except Exception as exc:
|
||||
_LOG.warning(f'[headlight] PWM error: {exc}')
|
||||
elif self._backend == 'sysfs':
|
||||
self._gpio_write('1' if level > 0 else '0')
|
||||
# sim: no-op
|
||||
|
||||
# ── Sysfs helpers ─────────────────────────────────────────────────────────
|
||||
|
||||
@staticmethod
|
||||
def _board_pin_to_sysfs(board_pin: int) -> Optional[int]:
|
||||
"""
|
||||
Map physical 40-pin board pin to sysfs GPIO number on Orin Nano Super.
|
||||
Only a subset is mapped here; extend as needed.
|
||||
"""
|
||||
# Partial map of Orin Nano Super 40-pin header (physical → sysfs offset)
|
||||
_ORIN_MAP: dict[int, int] = {
|
||||
7: 106,
|
||||
11: 112,
|
||||
12: 50,
|
||||
13: 108,
|
||||
15: 109,
|
||||
16: 85,
|
||||
18: 110,
|
||||
19: 130,
|
||||
21: 128,
|
||||
22: 111,
|
||||
23: 129,
|
||||
24: 131,
|
||||
26: 132,
|
||||
29: 118,
|
||||
31: 120,
|
||||
32: 122,
|
||||
33: 200, # headlight pin
|
||||
35: 116,
|
||||
36: 54,
|
||||
37: 121,
|
||||
38: 117,
|
||||
40: 56,
|
||||
}
|
||||
return _ORIN_MAP.get(board_pin)
|
||||
|
||||
def _sysfs_export(self, gpio_num: int) -> bool:
|
||||
export_path = f'/sys/class/gpio/gpio{gpio_num}'
|
||||
if not os.path.exists(export_path):
|
||||
try:
|
||||
with open('/sys/class/gpio/export', 'w') as f:
|
||||
f.write(str(gpio_num))
|
||||
except PermissionError:
|
||||
_LOG.warning('[headlight] No permission for /sys/class/gpio/export')
|
||||
return False
|
||||
except OSError as exc:
|
||||
_LOG.warning(f'[headlight] sysfs export error: {exc}')
|
||||
return False
|
||||
try:
|
||||
with open(f'{export_path}/direction', 'w') as f:
|
||||
f.write('out')
|
||||
return True
|
||||
except OSError as exc:
|
||||
_LOG.warning(f'[headlight] sysfs direction error: {exc}')
|
||||
return False
|
||||
|
||||
def _gpio_write(self, value: str) -> None:
|
||||
if not self._gpio_path:
|
||||
return
|
||||
try:
|
||||
with open(f'{self._gpio_path}/value', 'w') as f:
|
||||
f.write(value)
|
||||
except OSError as exc:
|
||||
_LOG.warning(f'[headlight] sysfs write error: {exc}')
|
||||
@ -0,0 +1,157 @@
|
||||
"""
|
||||
ir_slam_bridge_node.py — Bridge D435i IR1 stream to RGB-style topics for SLAM.
|
||||
|
||||
When the vision mode is 'night' or 'ir_only', visible-light cameras are
|
||||
unusable. RTAB-Map (and other SLAM front-ends) expect a colour image. This
|
||||
node re-publishes the D435i infra1 (mono8) stream as fake RGB (rgb8) so that
|
||||
RTAB-Map's visual front-end continues to operate without reconfiguration.
|
||||
|
||||
Also re-publishes the camera_info with the same frame_id so that the pipeline
|
||||
stays consistent.
|
||||
|
||||
When mode is 'day' or 'twilight' the bridge is inactive (no republish) to
|
||||
avoid double-feeding RTAB-Map.
|
||||
|
||||
Subscribes:
|
||||
/saltybot/vision_mode std_msgs/String mode gate
|
||||
/camera/infra1/image_rect_raw sensor_msgs/Image IR source (mono8)
|
||||
/camera/infra1/camera_info sensor_msgs/CameraInfo IR intrinsics
|
||||
|
||||
Publishes (when active):
|
||||
/saltybot/ir_bridge/image_raw sensor_msgs/Image grey-as-rgb8
|
||||
/saltybot/ir_bridge/camera_info sensor_msgs/CameraInfo passthrough
|
||||
|
||||
Parameters:
|
||||
active_modes str 'night,ir_only' (comma-separated)
|
||||
output_encoding str 'rgb8'
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
|
||||
|
||||
import numpy as np
|
||||
import cv2
|
||||
from cv_bridge import CvBridge
|
||||
|
||||
from sensor_msgs.msg import Image, CameraInfo
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
_SENSOR_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
)
|
||||
_LATCHED_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
)
|
||||
_RELIABLE_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=10,
|
||||
)
|
||||
|
||||
|
||||
class IRSlamBridgeNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('ir_slam_bridge')
|
||||
|
||||
self.declare_parameter('active_modes', 'night,ir_only')
|
||||
self.declare_parameter('output_encoding', 'rgb8')
|
||||
|
||||
active_str = self.get_parameter('active_modes').value
|
||||
self._active_modes: set[str] = {m.strip() for m in active_str.split(',')}
|
||||
self._out_enc: str = self.get_parameter('output_encoding').value
|
||||
|
||||
self._bridge = CvBridge()
|
||||
self._active = False
|
||||
self._latest_info: CameraInfo | None = None
|
||||
|
||||
# Mode gate
|
||||
self.create_subscription(
|
||||
String, '/saltybot/vision_mode', self._on_mode, _RELIABLE_QOS
|
||||
)
|
||||
|
||||
# IR source
|
||||
self.create_subscription(
|
||||
Image, '/camera/infra1/image_rect_raw', self._on_ir_image, _SENSOR_QOS
|
||||
)
|
||||
self.create_subscription(
|
||||
CameraInfo, '/camera/infra1/camera_info', self._on_camera_info, _LATCHED_QOS
|
||||
)
|
||||
|
||||
# Bridge output
|
||||
self._img_pub = self.create_publisher(
|
||||
Image, '/saltybot/ir_bridge/image_raw', 10
|
||||
)
|
||||
self._info_pub = self.create_publisher(
|
||||
CameraInfo, '/saltybot/ir_bridge/camera_info', 10
|
||||
)
|
||||
|
||||
self.get_logger().info(
|
||||
f'ir_slam_bridge ready — active in modes: {sorted(self._active_modes)}'
|
||||
)
|
||||
|
||||
# ── Subscriptions ─────────────────────────────────────────────────────────
|
||||
|
||||
def _on_mode(self, msg: String) -> None:
|
||||
was_active = self._active
|
||||
self._active = msg.data in self._active_modes
|
||||
if self._active != was_active:
|
||||
state = 'ACTIVE' if self._active else 'idle'
|
||||
self.get_logger().info(
|
||||
f'[ir_bridge] mode={msg.data} → bridge {state}'
|
||||
)
|
||||
|
||||
def _on_camera_info(self, msg: CameraInfo) -> None:
|
||||
self._latest_info = msg
|
||||
|
||||
def _on_ir_image(self, msg: Image) -> None:
|
||||
if not self._active:
|
||||
return
|
||||
|
||||
try:
|
||||
mono = self._bridge.imgmsg_to_cv2(msg, desired_encoding='mono8')
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(f'[ir_bridge] decode error: {exc}')
|
||||
return
|
||||
|
||||
# Convert mono8 → target encoding
|
||||
if self._out_enc == 'rgb8':
|
||||
colour = cv2.cvtColor(mono, cv2.COLOR_GRAY2RGB)
|
||||
elif self._out_enc == 'bgr8':
|
||||
colour = cv2.cvtColor(mono, cv2.COLOR_GRAY2BGR)
|
||||
else:
|
||||
colour = mono # passthrough
|
||||
|
||||
try:
|
||||
out_msg = self._bridge.cv2_to_imgmsg(colour, encoding=self._out_enc)
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(f'[ir_bridge] encode error: {exc}')
|
||||
return
|
||||
|
||||
out_msg.header = msg.header
|
||||
self._img_pub.publish(out_msg)
|
||||
|
||||
if self._latest_info is not None:
|
||||
self._latest_info.header = msg.header
|
||||
self._info_pub.publish(self._latest_info)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = IRSlamBridgeNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -0,0 +1,107 @@
|
||||
"""
|
||||
light_analyser.py — Histogram-based ambient light analysis and vision mode FSM.
|
||||
|
||||
Vision modes (string keys used on /saltybot/vision_mode):
|
||||
'day' — full colour, all cameras nominal
|
||||
'twilight' — dim light, start boosting IMX219 gain
|
||||
'night' — dark, max gain + headlight on, IR emitter off
|
||||
'ir_only' — effectively no ambient light, switch to D435i IR stereo only
|
||||
|
||||
Transitions use hysteresis to avoid rapid toggling:
|
||||
Promotion (to darker mode): threshold - HYST
|
||||
Demotion (to lighter mode): threshold + HYST
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
from typing import Tuple
|
||||
|
||||
# ── Thresholds (0–255 mean grey intensity) ───────────────────────────────────
|
||||
# Modes: ir_only < NIGHT_THR <= night < TWILIGHT_THR <= twilight < DAY_THR <= day
|
||||
_TWILIGHT_THR = 110 # below → twilight
|
||||
_NIGHT_THR = 45 # below → night
|
||||
_IR_ONLY_THR = 15 # below → ir_only
|
||||
_HYST = 8 # hysteresis band (half-width)
|
||||
|
||||
# Ordered list of modes darkest → brightest
|
||||
_MODES = ('ir_only', 'night', 'twilight', 'day')
|
||||
|
||||
# Per-mode thresholds: (promote_below, demote_above)
|
||||
# promote_below — if median < this, enter the next darker mode
|
||||
# demote_above — if median > this, enter the next lighter mode
|
||||
#
|
||||
# Hysteresis: promote threshold is thr - HYST; demote is thr + HYST
|
||||
# where thr is the boundary between THIS mode and the adjacent lighter mode.
|
||||
_THRESHOLDS = {
|
||||
# (promote_below, demote_above)
|
||||
'day': (_TWILIGHT_THR - _HYST, None),
|
||||
'twilight': (_NIGHT_THR - _HYST, _TWILIGHT_THR + _HYST),
|
||||
'night': (_IR_ONLY_THR - _HYST, _NIGHT_THR + _HYST),
|
||||
'ir_only': (None, _IR_ONLY_THR + _HYST),
|
||||
}
|
||||
|
||||
|
||||
class LightAnalyser:
|
||||
"""Stateful ambient-light classifier with hysteresis."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
initial_mode: str = 'day',
|
||||
history_len: int = 6,
|
||||
):
|
||||
if initial_mode not in _MODES:
|
||||
raise ValueError(f'Unknown mode: {initial_mode}')
|
||||
self._mode = initial_mode
|
||||
self._history: list[float] = []
|
||||
self._history_len = history_len
|
||||
|
||||
# ── Public API ────────────────────────────────────────────────────────────
|
||||
|
||||
@property
|
||||
def mode(self) -> str:
|
||||
return self._mode
|
||||
|
||||
def update(self, frame: np.ndarray) -> Tuple[str, float]:
|
||||
"""
|
||||
Analyse a new image frame (any colour space; converted to grey internally).
|
||||
|
||||
Returns:
|
||||
(mode, mean_intensity) — mode string + 0–255 grey mean
|
||||
"""
|
||||
intensity = self._frame_intensity(frame)
|
||||
self._history.append(intensity)
|
||||
if len(self._history) > self._history_len:
|
||||
self._history.pop(0)
|
||||
|
||||
smooth = float(np.median(self._history))
|
||||
self._mode = self._next_mode(self._mode, smooth)
|
||||
return self._mode, smooth
|
||||
|
||||
# ── Internal helpers ──────────────────────────────────────────────────────
|
||||
|
||||
@staticmethod
|
||||
def _frame_intensity(frame: np.ndarray) -> float:
|
||||
"""Return mean pixel intensity of the frame as float in [0, 255]."""
|
||||
if frame.ndim == 3:
|
||||
# RGB / BGR → greyscale
|
||||
grey = np.mean(frame, axis=2).astype(np.float32)
|
||||
else:
|
||||
grey = frame.astype(np.float32)
|
||||
return float(grey.mean())
|
||||
|
||||
@staticmethod
|
||||
def _next_mode(current: str, intensity: float) -> str:
|
||||
"""Apply hysteresis FSM to decide next mode."""
|
||||
idx = _MODES.index(current)
|
||||
|
||||
# Try promotion (→ darker mode) first
|
||||
lo, hi = _THRESHOLDS[current]
|
||||
if lo is not None and intensity < lo:
|
||||
return _MODES[max(0, idx - 1)]
|
||||
|
||||
# Try demotion (→ lighter mode)
|
||||
if hi is not None and intensity > hi:
|
||||
return _MODES[min(len(_MODES) - 1, idx + 1)]
|
||||
|
||||
return current
|
||||
@ -0,0 +1,141 @@
|
||||
"""
|
||||
light_monitor_node.py — Ambient light monitor for SaltyBot night vision.
|
||||
|
||||
Uses the front IMX219 RGB stream (or D435i infra1 as fallback) to estimate
|
||||
ambient brightness via histogram analysis. Publishes the current vision mode
|
||||
and raw intensity on a 2 Hz digest timer.
|
||||
|
||||
Subscribes (any ONE active at a time):
|
||||
/camera/color/image_raw sensor_msgs/Image RGB from IMX219 front (primary)
|
||||
/camera/infra1/image_rect_raw sensor_msgs/Image IR fallback (mono8)
|
||||
|
||||
Publishes:
|
||||
/saltybot/ambient_light std_msgs/Float32 mean grey intensity 0–255
|
||||
/saltybot/vision_mode std_msgs/String 'day'|'twilight'|'night'|'ir_only'
|
||||
|
||||
Parameters:
|
||||
primary_camera_topic str '/camera/color/image_raw'
|
||||
fallback_ir_topic str '/camera/infra1/image_rect_raw'
|
||||
publish_hz float 2.0
|
||||
initial_mode str 'day'
|
||||
history_len int 6
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
|
||||
import numpy as np
|
||||
import cv2
|
||||
from cv_bridge import CvBridge
|
||||
|
||||
from sensor_msgs.msg import Image
|
||||
from std_msgs.msg import Float32, String
|
||||
|
||||
from .light_analyser import LightAnalyser
|
||||
|
||||
|
||||
_SENSOR_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=3,
|
||||
)
|
||||
|
||||
|
||||
class LightMonitorNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('light_monitor')
|
||||
|
||||
self.declare_parameter('primary_camera_topic', '/camera/color/image_raw')
|
||||
self.declare_parameter('fallback_ir_topic', '/camera/infra1/image_rect_raw')
|
||||
self.declare_parameter('publish_hz', 2.0)
|
||||
self.declare_parameter('initial_mode', 'day')
|
||||
self.declare_parameter('history_len', 6)
|
||||
|
||||
primary_topic = self.get_parameter('primary_camera_topic').value
|
||||
fallback_topic = self.get_parameter('fallback_ir_topic').value
|
||||
pub_hz = self.get_parameter('publish_hz').value
|
||||
initial_mode = self.get_parameter('initial_mode').value
|
||||
history_len = self.get_parameter('history_len').value
|
||||
|
||||
self._bridge = CvBridge()
|
||||
self._analyser = LightAnalyser(initial_mode=initial_mode, history_len=history_len)
|
||||
|
||||
# Latest decoded frame (thread-safe in single-threaded executor)
|
||||
self._latest_frame: np.ndarray | None = None
|
||||
self._using_fallback = False
|
||||
|
||||
# Subscriptions — primary preferred; fallback used when primary silent
|
||||
self.create_subscription(Image, primary_topic, self._on_primary, _SENSOR_QOS)
|
||||
self.create_subscription(Image, fallback_topic, self._on_fallback, _SENSOR_QOS)
|
||||
|
||||
self._mode_pub = self.create_publisher(String, '/saltybot/vision_mode', 10)
|
||||
self._light_pub = self.create_publisher(Float32, '/saltybot/ambient_light', 10)
|
||||
|
||||
self.create_timer(1.0 / pub_hz, self._tick)
|
||||
|
||||
self.get_logger().info(
|
||||
f'light_monitor ready — primary={primary_topic} fallback={fallback_topic}'
|
||||
)
|
||||
|
||||
# ── Subscriptions ─────────────────────────────────────────────────────────
|
||||
|
||||
def _on_primary(self, msg: Image) -> None:
|
||||
try:
|
||||
frame = self._bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
|
||||
self._latest_frame = frame
|
||||
self._using_fallback = False
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(f'[light_monitor] primary decode: {exc}')
|
||||
|
||||
def _on_fallback(self, msg: Image) -> None:
|
||||
# Only use fallback if no primary frame recently arrived
|
||||
if self._latest_frame is not None and not self._using_fallback:
|
||||
return # primary is active
|
||||
try:
|
||||
frame = self._bridge.imgmsg_to_cv2(msg, desired_encoding='mono8')
|
||||
self._latest_frame = frame
|
||||
self._using_fallback = True
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(f'[light_monitor] fallback decode: {exc}')
|
||||
|
||||
# ── Publish timer ─────────────────────────────────────────────────────────
|
||||
|
||||
def _tick(self) -> None:
|
||||
if self._latest_frame is None:
|
||||
return # no data yet
|
||||
|
||||
# Subsample for speed (analyse quarter-resolution)
|
||||
frame = self._latest_frame
|
||||
if frame.shape[0] > 120:
|
||||
frame = frame[::4, ::4]
|
||||
|
||||
mode, intensity = self._analyser.update(frame)
|
||||
|
||||
msg_mode = String()
|
||||
msg_mode.data = mode
|
||||
self._mode_pub.publish(msg_mode)
|
||||
|
||||
msg_light = Float32()
|
||||
msg_light.data = float(intensity)
|
||||
self._light_pub.publish(msg_light)
|
||||
|
||||
if self._using_fallback:
|
||||
self.get_logger().debug(
|
||||
f'[light_monitor] IR fallback — mode={mode} intensity={intensity:.1f}'
|
||||
)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = LightMonitorNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -0,0 +1,137 @@
|
||||
"""
|
||||
night_vision_controller_node.py — Hardware adaptation on vision mode changes.
|
||||
|
||||
Subscribes to /saltybot/vision_mode and drives:
|
||||
• D435i IR emitter (via realsense2_camera parameter service)
|
||||
• IMX219 gain / exposure (via v4l2-ctl subprocess)
|
||||
• GPIO headlight brightness
|
||||
|
||||
Mode → hardware mapping:
|
||||
'day' → emitter=auto, IMX219 AE, headlight off
|
||||
'twilight' → emitter=on, IMX219 boosted, headlight dim
|
||||
'night' → emitter=on, IMX219 max gain, headlight medium
|
||||
'ir_only' → emitter=on, IMX219 max gain, headlight full
|
||||
|
||||
Publishes:
|
||||
/saltybot/visual_odom_status std_msgs/String (JSON — reuses VO status topic)
|
||||
|
||||
Parameters:
|
||||
headlight_pin int 33 (physical 40-pin board pin)
|
||||
day_headlight float 0.0
|
||||
twilight_headlight float 0.25
|
||||
night_headlight float 0.50
|
||||
ir_only_headlight float 1.0
|
||||
"""
|
||||
|
||||
import json
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
|
||||
from std_msgs.msg import String
|
||||
|
||||
from .camera_controller import CameraController
|
||||
from .gpio_headlight import GPIOHeadlight
|
||||
|
||||
|
||||
_RELIABLE_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=10,
|
||||
)
|
||||
|
||||
|
||||
class NightVisionControllerNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('night_vision_controller')
|
||||
|
||||
self.declare_parameter('headlight_pin', 33)
|
||||
self.declare_parameter('day_headlight', 0.0)
|
||||
self.declare_parameter('twilight_headlight', 0.25)
|
||||
self.declare_parameter('night_headlight', 0.50)
|
||||
self.declare_parameter('ir_only_headlight', 1.0)
|
||||
|
||||
pin = self.get_parameter('headlight_pin').value
|
||||
self._brightness_map = {
|
||||
'day': self.get_parameter('day_headlight').value,
|
||||
'twilight': self.get_parameter('twilight_headlight').value,
|
||||
'night': self.get_parameter('night_headlight').value,
|
||||
'ir_only': self.get_parameter('ir_only_headlight').value,
|
||||
}
|
||||
|
||||
self._cam_ctrl = CameraController(self)
|
||||
self._headlight = GPIOHeadlight(pin=pin)
|
||||
self._cur_mode: str | None = None
|
||||
self._mode_change_t: float = 0.0
|
||||
|
||||
self.create_subscription(
|
||||
String, '/saltybot/vision_mode', self._on_mode, _RELIABLE_QOS
|
||||
)
|
||||
|
||||
self._status_pub = self.create_publisher(
|
||||
String, '/saltybot/night_vision_status', 10
|
||||
)
|
||||
self.create_timer(5.0, self._status_tick)
|
||||
|
||||
self.get_logger().info(
|
||||
f'night_vision_controller ready — headlight pin={pin}'
|
||||
)
|
||||
|
||||
# ── Subscription ──────────────────────────────────────────────────────────
|
||||
|
||||
def _on_mode(self, msg: String) -> None:
|
||||
mode = msg.data
|
||||
if mode == self._cur_mode:
|
||||
return
|
||||
|
||||
prev = self._cur_mode
|
||||
self._cur_mode = mode
|
||||
self._mode_change_t = time.monotonic()
|
||||
|
||||
# Apply camera profile (D435i emitter + IMX219 gain/exposure)
|
||||
self._cam_ctrl.set_profile(mode)
|
||||
|
||||
# Apply headlight
|
||||
brightness = self._brightness_map.get(mode, 0.0)
|
||||
self._headlight.set_brightness(brightness)
|
||||
|
||||
self.get_logger().info(
|
||||
f'[nv_ctrl] {prev} → {mode} | '
|
||||
f'headlight={brightness:.2f}'
|
||||
)
|
||||
|
||||
# ── Status heartbeat ──────────────────────────────────────────────────────
|
||||
|
||||
def _status_tick(self) -> None:
|
||||
status = {
|
||||
'mode': self._cur_mode or 'unknown',
|
||||
'headlight': round(self._headlight._brightness, 2),
|
||||
'mode_age_s': round(time.monotonic() - self._mode_change_t, 1)
|
||||
if self._cur_mode else None,
|
||||
}
|
||||
msg = String()
|
||||
msg.data = json.dumps(status)
|
||||
self._status_pub.publish(msg)
|
||||
|
||||
# ── Cleanup ───────────────────────────────────────────────────────────────
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
self._headlight.cleanup()
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = NightVisionControllerNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
4
jetson/ros2_ws/src/saltybot_night_vision/setup.cfg
Normal file
4
jetson/ros2_ws/src/saltybot_night_vision/setup.cfg
Normal file
@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_night_vision
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_night_vision
|
||||
34
jetson/ros2_ws/src/saltybot_night_vision/setup.py
Normal file
34
jetson/ros2_ws/src/saltybot_night_vision/setup.py
Normal file
@ -0,0 +1,34 @@
|
||||
from setuptools import setup, find_packages
|
||||
import os
|
||||
from glob import glob
|
||||
|
||||
package_name = 'saltybot_night_vision'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.1.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
('share/' + package_name + '/launch',
|
||||
glob('launch/*.launch.py')),
|
||||
('share/' + package_name + '/config',
|
||||
glob('config/*.yaml')),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='SaltyLab',
|
||||
maintainer_email='robot@saltylab.local',
|
||||
description='Night vision mode: IR emitter, headlight, ambient-light FSM, IR SLAM bridge',
|
||||
license='MIT',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'light_monitor = saltybot_night_vision.light_monitor_node:main',
|
||||
'night_vision_ctrl = saltybot_night_vision.night_vision_controller_node:main',
|
||||
'ir_slam_bridge = saltybot_night_vision.ir_slam_bridge_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -0,0 +1,152 @@
|
||||
"""
|
||||
test_night_vision.py — Unit tests for LightAnalyser and GPIOHeadlight.
|
||||
|
||||
Runs without ROS2 / Jetson hardware (no rclpy, no GPIO).
|
||||
"""
|
||||
|
||||
import sys
|
||||
import os
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
|
||||
|
||||
from saltybot_night_vision.light_analyser import LightAnalyser
|
||||
from saltybot_night_vision.gpio_headlight import GPIOHeadlight
|
||||
|
||||
|
||||
# ── LightAnalyser tests ───────────────────────────────────────────────────────
|
||||
|
||||
class TestLightAnalyser:
|
||||
|
||||
def _bright_frame(self, value: int = 180) -> np.ndarray:
|
||||
return np.full((480, 640), value, dtype=np.uint8)
|
||||
|
||||
def _dark_frame(self, value: int = 5) -> np.ndarray:
|
||||
return np.full((480, 640), value, dtype=np.uint8)
|
||||
|
||||
def test_initial_mode_default(self):
|
||||
la = LightAnalyser()
|
||||
assert la.mode == 'day'
|
||||
|
||||
def test_bright_frame_stays_day(self):
|
||||
la = LightAnalyser()
|
||||
for _ in range(10):
|
||||
mode, _ = la.update(self._bright_frame(180))
|
||||
assert mode == 'day'
|
||||
|
||||
def test_dark_frame_transitions_to_ir_only(self):
|
||||
la = LightAnalyser()
|
||||
for _ in range(10):
|
||||
mode, _ = la.update(self._dark_frame(3))
|
||||
assert mode == 'ir_only'
|
||||
|
||||
def test_medium_intensity_twilight(self):
|
||||
la = LightAnalyser()
|
||||
frame = np.full((480, 640), 75, dtype=np.uint8)
|
||||
for _ in range(10):
|
||||
mode, _ = la.update(frame)
|
||||
assert mode == 'twilight'
|
||||
|
||||
def test_night_range(self):
|
||||
la = LightAnalyser()
|
||||
frame = np.full((480, 640), 25, dtype=np.uint8)
|
||||
for _ in range(10):
|
||||
mode, _ = la.update(frame)
|
||||
assert mode == 'night'
|
||||
|
||||
def test_intensity_value_correct(self):
|
||||
la = LightAnalyser()
|
||||
frame = np.full((60, 80), 128, dtype=np.uint8)
|
||||
_, intensity = la.update(frame)
|
||||
assert intensity == pytest.approx(128.0, abs=0.5)
|
||||
|
||||
def test_rgb_frame_accepted(self):
|
||||
la = LightAnalyser()
|
||||
frame = np.full((480, 640, 3), 60, dtype=np.uint8)
|
||||
mode, intensity = la.update(frame)
|
||||
assert isinstance(mode, str)
|
||||
assert 0.0 <= intensity <= 255.0
|
||||
|
||||
def test_hysteresis_prevents_oscillation(self):
|
||||
"""Frame right at threshold should not cause mode flip on each tick."""
|
||||
la = LightAnalyser()
|
||||
# Start from day, push to just below twilight threshold
|
||||
frame = np.full((480, 640), 100, dtype=np.uint8)
|
||||
modes: list[str] = []
|
||||
for _ in range(20):
|
||||
mode, _ = la.update(frame)
|
||||
modes.append(mode)
|
||||
# Should not oscillate — all same mode
|
||||
assert len(set(modes[-10:])) == 1
|
||||
|
||||
def test_recovery_to_day(self):
|
||||
la = LightAnalyser(initial_mode='ir_only')
|
||||
bright = np.full((480, 640), 200, dtype=np.uint8)
|
||||
for _ in range(20):
|
||||
mode, _ = la.update(bright)
|
||||
assert mode == 'day'
|
||||
|
||||
|
||||
# ── GPIOHeadlight tests (simulated — no hardware) ─────────────────────────────
|
||||
|
||||
class TestGPIOHeadlightSim:
|
||||
"""
|
||||
Tests that GPIOHeadlight correctly tracks brightness internally
|
||||
even when running in simulation mode (no hardware GPIO).
|
||||
"""
|
||||
|
||||
def test_default_brightness_zero(self):
|
||||
hl = GPIOHeadlight()
|
||||
assert hl._brightness == pytest.approx(0.0)
|
||||
|
||||
def test_set_brightness_clamps_high(self):
|
||||
hl = GPIOHeadlight()
|
||||
hl.set_brightness(2.5)
|
||||
assert hl._brightness == pytest.approx(1.0)
|
||||
|
||||
def test_set_brightness_clamps_low(self):
|
||||
hl = GPIOHeadlight()
|
||||
hl.set_brightness(-0.5)
|
||||
assert hl._brightness == pytest.approx(0.0)
|
||||
|
||||
def test_set_level_off(self):
|
||||
hl = GPIOHeadlight()
|
||||
hl.set_brightness(0.8)
|
||||
hl.set_level('off')
|
||||
assert hl._brightness == pytest.approx(0.0)
|
||||
|
||||
def test_set_level_full(self):
|
||||
hl = GPIOHeadlight()
|
||||
hl.set_level('full')
|
||||
assert hl._brightness == pytest.approx(1.0)
|
||||
|
||||
def test_set_level_dim(self):
|
||||
hl = GPIOHeadlight()
|
||||
hl.set_level('dim')
|
||||
assert hl._brightness == pytest.approx(0.25)
|
||||
|
||||
def test_off_method(self):
|
||||
hl = GPIOHeadlight()
|
||||
hl.set_brightness(0.75)
|
||||
hl.off()
|
||||
assert hl._brightness == pytest.approx(0.0)
|
||||
|
||||
def test_cleanup_sets_zero(self):
|
||||
hl = GPIOHeadlight()
|
||||
hl.set_brightness(0.9)
|
||||
hl.cleanup()
|
||||
assert hl._brightness == pytest.approx(0.0)
|
||||
|
||||
def test_no_op_on_same_brightness(self):
|
||||
"""Repeated set of same value should not raise."""
|
||||
hl = GPIOHeadlight()
|
||||
hl.set_brightness(0.5)
|
||||
hl.set_brightness(0.5) # should be no-op, no error
|
||||
assert hl._brightness == pytest.approx(0.5)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
pytest.main([__file__, '-v'])
|
||||
352
src/audio.c
Normal file
352
src/audio.c
Normal file
@ -0,0 +1,352 @@
|
||||
#include "audio.h"
|
||||
#include "config.h"
|
||||
#include "stm32f7xx_hal.h"
|
||||
#include <string.h>
|
||||
|
||||
/* ================================================================
|
||||
* Buffer layout
|
||||
* ================================================================
|
||||
* AUDIO_BUF_HALF samples per DMA half (config.h: 441 at 22050 Hz → 20 ms).
|
||||
* DMA runs in circular mode over 2 halves; TxHalfCplt refills [0] and
|
||||
* TxCplt refills [AUDIO_BUF_HALF]. Both callbacks simply call fill_half().
|
||||
*/
|
||||
#define AUDIO_BUF_SIZE (AUDIO_BUF_HALF * 2u)
|
||||
|
||||
/* DMA buffer: must be in non-cached SRAM or flushed before DMA access.
|
||||
* Placed in SRAM1 (default .bss section on STM32F722 — below 512 KB).
|
||||
* The I-cache / D-cache is on by default; DMA1 is an AHB master that
|
||||
* bypasses cache, so we keep this buffer in DTCM-uncacheable SRAM. */
|
||||
static int16_t s_dma_buf[AUDIO_BUF_SIZE];
|
||||
|
||||
/* ================================================================
|
||||
* PCM FIFO — Jetson audio (main-loop writer, ISR reader)
|
||||
* ================================================================
|
||||
* Single-producer / single-consumer lock-free ring buffer.
|
||||
* Power-of-2 size so wrap uses bitwise AND (safe across ISR boundary).
|
||||
* 4096 samples ≈ 185 ms at 22050 Hz — enough to absorb JLink jitter.
|
||||
*/
|
||||
#define PCM_FIFO_SIZE 4096u
|
||||
#define PCM_FIFO_MASK (PCM_FIFO_SIZE - 1u)
|
||||
|
||||
static int16_t s_pcm_fifo[PCM_FIFO_SIZE];
|
||||
static volatile uint16_t s_pcm_rd = 0; /* consumer (ISR advances) */
|
||||
static volatile uint16_t s_pcm_wr = 0; /* producer (main loop advances) */
|
||||
|
||||
/* ================================================================
|
||||
* Tone sequencer
|
||||
* ================================================================
|
||||
* Each AudioTone maps to a const ToneStep array. Steps are played
|
||||
* sequentially; a gap_ms of 0 means no silence between steps.
|
||||
* audio_tick() in the main loop advances the state machine and
|
||||
* writes the volatile active-tone params read by the ISR fill path.
|
||||
*/
|
||||
typedef struct {
|
||||
uint16_t freq_hz;
|
||||
uint16_t dur_ms;
|
||||
uint16_t gap_ms; /* silence after this step */
|
||||
} ToneStep;
|
||||
|
||||
/* ---- Tone definitions ---- */
|
||||
static const ToneStep s_def_beep_short[] = {{880, 100, 0}};
|
||||
static const ToneStep s_def_beep_long[] = {{880, 500, 0}};
|
||||
static const ToneStep s_def_startup[] = {{523, 120, 60},
|
||||
{659, 120, 60},
|
||||
{784, 200, 0}};
|
||||
static const ToneStep s_def_arm[] = {{880, 80, 60},
|
||||
{1047, 100, 0}};
|
||||
static const ToneStep s_def_disarm[] = {{880, 80, 60},
|
||||
{659, 100, 0}};
|
||||
static const ToneStep s_def_fault[] = {{200, 500, 0}};
|
||||
|
||||
typedef struct {
|
||||
const ToneStep *steps;
|
||||
uint8_t n_steps;
|
||||
} ToneDef;
|
||||
|
||||
static const ToneDef s_tone_defs[AUDIO_TONE_COUNT] = {
|
||||
[AUDIO_TONE_BEEP_SHORT] = {s_def_beep_short, 1},
|
||||
[AUDIO_TONE_BEEP_LONG] = {s_def_beep_long, 1},
|
||||
[AUDIO_TONE_STARTUP] = {s_def_startup, 3},
|
||||
[AUDIO_TONE_ARM] = {s_def_arm, 2},
|
||||
[AUDIO_TONE_DISARM] = {s_def_disarm, 2},
|
||||
[AUDIO_TONE_FAULT] = {s_def_fault, 1},
|
||||
};
|
||||
|
||||
/* Active tone queue */
|
||||
#define TONE_QUEUE_DEPTH 4u
|
||||
|
||||
typedef struct {
|
||||
const ToneDef *def;
|
||||
uint8_t step; /* current step index within def */
|
||||
bool in_gap; /* true while playing inter-step silence */
|
||||
uint32_t step_end_ms; /* abs HAL_GetTick() when step/gap expires */
|
||||
} ToneSeq;
|
||||
|
||||
static ToneSeq s_tone_q[TONE_QUEUE_DEPTH];
|
||||
static uint8_t s_tq_head = 0; /* consumer (audio_tick reads) */
|
||||
static uint8_t s_tq_tail = 0; /* producer (audio_play_tone writes) */
|
||||
|
||||
/* Volatile parameters written by audio_tick(), read by ISR fill_half() */
|
||||
static volatile uint16_t s_active_freq = 0; /* 0 = silence / gap */
|
||||
static volatile uint32_t s_active_phase = 0; /* sample phase counter */
|
||||
|
||||
/* ================================================================
|
||||
* Volume
|
||||
* ================================================================ */
|
||||
static uint8_t s_volume = AUDIO_VOLUME_DEFAULT; /* 0–100 */
|
||||
|
||||
/* ================================================================
|
||||
* HAL handles
|
||||
* ================================================================ */
|
||||
static I2S_HandleTypeDef s_i2s;
|
||||
static DMA_HandleTypeDef s_dma_tx;
|
||||
|
||||
/* ================================================================
|
||||
* fill_half() — called from ISR, O(AUDIO_BUF_HALF)
|
||||
* ================================================================
|
||||
* Priority: PCM FIFO (Jetson TTS) > square-wave tone > silence.
|
||||
* Volume applied via integer scaling — no float in ISR.
|
||||
*/
|
||||
static void fill_half(int16_t *buf, uint16_t n)
|
||||
{
|
||||
uint16_t rd = s_pcm_rd;
|
||||
uint16_t wr = s_pcm_wr;
|
||||
uint16_t avail = (uint16_t)((wr - rd) & PCM_FIFO_MASK);
|
||||
|
||||
if (avail >= n) {
|
||||
/* ---- Drain Jetson PCM FIFO ---- */
|
||||
for (uint16_t i = 0; i < n; i++) {
|
||||
int32_t s = (int32_t)s_pcm_fifo[rd] * (int32_t)s_volume / 100;
|
||||
buf[i] = (s > 32767) ? (int16_t)32767 :
|
||||
(s < -32768) ? (int16_t)-32768 : (int16_t)s;
|
||||
rd = (rd + 1u) & PCM_FIFO_MASK;
|
||||
}
|
||||
s_pcm_rd = rd;
|
||||
|
||||
} else if (s_active_freq) {
|
||||
/* ---- Square wave tone generator ---- */
|
||||
uint32_t half_p = (uint32_t)AUDIO_SAMPLE_RATE / (2u * s_active_freq);
|
||||
int16_t amp = (int16_t)(16384u * (uint32_t)s_volume / 100u);
|
||||
uint32_t ph = s_active_phase;
|
||||
uint32_t period = 2u * half_p;
|
||||
for (uint16_t i = 0; i < n; i++) {
|
||||
buf[i] = ((ph % period) < half_p) ? amp : (int16_t)(-amp);
|
||||
ph++;
|
||||
}
|
||||
s_active_phase = ph;
|
||||
|
||||
} else {
|
||||
/* ---- Silence ---- */
|
||||
memset(buf, 0, (size_t)(n * 2u));
|
||||
}
|
||||
}
|
||||
|
||||
/* ================================================================
|
||||
* DMA callbacks (ISR context)
|
||||
* ================================================================ */
|
||||
void HAL_I2S_TxHalfCpltCallback(I2S_HandleTypeDef *hi2s)
|
||||
{
|
||||
if (hi2s->Instance == SPI3)
|
||||
fill_half(s_dma_buf, AUDIO_BUF_HALF);
|
||||
}
|
||||
|
||||
void HAL_I2S_TxCpltCallback(I2S_HandleTypeDef *hi2s)
|
||||
{
|
||||
if (hi2s->Instance == SPI3)
|
||||
fill_half(s_dma_buf + AUDIO_BUF_HALF, AUDIO_BUF_HALF);
|
||||
}
|
||||
|
||||
/* DMA1 Stream7 IRQ (SPI3/I2S3 TX) */
|
||||
void DMA1_Stream7_IRQHandler(void)
|
||||
{
|
||||
HAL_DMA_IRQHandler(&s_dma_tx);
|
||||
}
|
||||
|
||||
/* ================================================================
|
||||
* audio_init()
|
||||
* ================================================================ */
|
||||
void audio_init(void)
|
||||
{
|
||||
/* ---- PLLI2S: N=192, R=2 → 96 MHz I2S clock ----
|
||||
* With SPI3/I2S3 and I2S_DATAFORMAT_16B (16-bit, 32-bit frame slot):
|
||||
* FS = 96 MHz / (32 × 2 × I2SDIV) where HAL picks I2SDIV = 68
|
||||
* → 96 000 000 / (32 × 2 × 68) = 22 058 Hz (< 0.04 % error)
|
||||
*/
|
||||
RCC_PeriphCLKInitTypeDef pclk = {0};
|
||||
pclk.PeriphClockSelection = RCC_PERIPHCLK_I2S;
|
||||
pclk.I2sClockSelection = RCC_I2SCLKSOURCE_PLLI2S;
|
||||
pclk.PLLI2S.PLLI2SN = 192; /* VCO = (HSE/PLLM) × 192 = 192 MHz */
|
||||
pclk.PLLI2S.PLLI2SR = 2; /* I2S clock = 96 MHz */
|
||||
HAL_RCCEx_PeriphCLKConfig(&pclk);
|
||||
|
||||
/* ---- GPIO ---- */
|
||||
__HAL_RCC_GPIOA_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOB_CLK_ENABLE();
|
||||
__HAL_RCC_GPIOC_CLK_ENABLE();
|
||||
|
||||
GPIO_InitTypeDef gpio = {0};
|
||||
|
||||
/* PA15: I2S3_WS (LRCLK), AF6 */
|
||||
gpio.Pin = AUDIO_LRCK_PIN;
|
||||
gpio.Mode = GPIO_MODE_AF_PP;
|
||||
gpio.Pull = GPIO_NOPULL;
|
||||
gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
|
||||
gpio.Alternate = GPIO_AF6_SPI3;
|
||||
HAL_GPIO_Init(AUDIO_LRCK_PORT, &gpio);
|
||||
|
||||
/* PC10: I2S3_CK (BCLK), AF6 */
|
||||
gpio.Pin = AUDIO_BCLK_PIN;
|
||||
HAL_GPIO_Init(AUDIO_BCLK_PORT, &gpio);
|
||||
|
||||
/* PB5: I2S3_SD (DIN), AF6 */
|
||||
gpio.Pin = AUDIO_DOUT_PIN;
|
||||
HAL_GPIO_Init(AUDIO_DOUT_PORT, &gpio);
|
||||
|
||||
/* PC5: AUDIO_MUTE GPIO output — drive low (muted) initially */
|
||||
gpio.Pin = AUDIO_MUTE_PIN;
|
||||
gpio.Mode = GPIO_MODE_OUTPUT_PP;
|
||||
gpio.Pull = GPIO_NOPULL;
|
||||
gpio.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
gpio.Alternate = 0;
|
||||
HAL_GPIO_Init(AUDIO_MUTE_PORT, &gpio);
|
||||
HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN, GPIO_PIN_RESET);
|
||||
|
||||
/* ---- DMA1 Stream7 Channel0 (SPI3/I2S3 TX) ---- */
|
||||
__HAL_RCC_DMA1_CLK_ENABLE();
|
||||
s_dma_tx.Instance = DMA1_Stream7;
|
||||
s_dma_tx.Init.Channel = DMA_CHANNEL_0;
|
||||
s_dma_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
|
||||
s_dma_tx.Init.PeriphInc = DMA_PINC_DISABLE;
|
||||
s_dma_tx.Init.MemInc = DMA_MINC_ENABLE;
|
||||
s_dma_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
|
||||
s_dma_tx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
|
||||
s_dma_tx.Init.Mode = DMA_CIRCULAR;
|
||||
s_dma_tx.Init.Priority = DMA_PRIORITY_MEDIUM;
|
||||
s_dma_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
|
||||
HAL_DMA_Init(&s_dma_tx);
|
||||
__HAL_LINKDMA(&s_i2s, hdmatx, s_dma_tx);
|
||||
|
||||
HAL_NVIC_SetPriority(DMA1_Stream7_IRQn, 7, 0);
|
||||
HAL_NVIC_EnableIRQ(DMA1_Stream7_IRQn);
|
||||
|
||||
/* ---- SPI3 in I2S3 master TX mode ---- */
|
||||
__HAL_RCC_SPI3_CLK_ENABLE();
|
||||
s_i2s.Instance = SPI3;
|
||||
s_i2s.Init.Mode = I2S_MODE_MASTER_TX;
|
||||
s_i2s.Init.Standard = I2S_STANDARD_PHILIPS;
|
||||
s_i2s.Init.DataFormat = I2S_DATAFORMAT_16B;
|
||||
s_i2s.Init.MCLKOutput = I2S_MCLKOUTPUT_DISABLE;
|
||||
s_i2s.Init.AudioFreq = I2S_AUDIOFREQ_22K;
|
||||
s_i2s.Init.CPOL = I2S_CPOL_LOW;
|
||||
s_i2s.Init.ClockSource = I2S_CLOCK_PLL;
|
||||
HAL_I2S_Init(&s_i2s);
|
||||
|
||||
/* Pre-fill with silence and start circular DMA TX */
|
||||
memset(s_dma_buf, 0, sizeof(s_dma_buf));
|
||||
HAL_I2S_Transmit_DMA(&s_i2s, (uint16_t *)s_dma_buf, AUDIO_BUF_SIZE);
|
||||
|
||||
/* Unmute amp after DMA is running — avoids start-up click */
|
||||
HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN, GPIO_PIN_SET);
|
||||
}
|
||||
|
||||
/* ================================================================
|
||||
* Public API
|
||||
* ================================================================ */
|
||||
|
||||
void audio_mute(bool active)
|
||||
{
|
||||
HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN,
|
||||
active ? GPIO_PIN_SET : GPIO_PIN_RESET);
|
||||
}
|
||||
|
||||
void audio_set_volume(uint8_t vol)
|
||||
{
|
||||
s_volume = (vol > 100u) ? 100u : vol;
|
||||
}
|
||||
|
||||
bool audio_play_tone(AudioTone tone)
|
||||
{
|
||||
if (tone >= AUDIO_TONE_COUNT) return false;
|
||||
|
||||
uint8_t next = (s_tq_tail + 1u) % TONE_QUEUE_DEPTH;
|
||||
if (next == s_tq_head) return false; /* queue full */
|
||||
|
||||
s_tone_q[s_tq_tail].def = &s_tone_defs[tone];
|
||||
s_tone_q[s_tq_tail].step = 0;
|
||||
s_tone_q[s_tq_tail].in_gap = false;
|
||||
s_tone_q[s_tq_tail].step_end_ms = 0; /* audio_tick() sets this on first run */
|
||||
s_tq_tail = next;
|
||||
return true;
|
||||
}
|
||||
|
||||
uint16_t audio_write_pcm(const int16_t *samples, uint16_t n)
|
||||
{
|
||||
uint16_t wr = s_pcm_wr;
|
||||
uint16_t rd = s_pcm_rd;
|
||||
uint16_t space = (uint16_t)((rd - wr - 1u) & PCM_FIFO_MASK);
|
||||
uint16_t accept = (n < space) ? n : space;
|
||||
|
||||
for (uint16_t i = 0; i < accept; i++) {
|
||||
s_pcm_fifo[wr] = samples[i];
|
||||
wr = (wr + 1u) & PCM_FIFO_MASK;
|
||||
}
|
||||
s_pcm_wr = wr;
|
||||
return accept;
|
||||
}
|
||||
|
||||
void audio_tick(uint32_t now_ms)
|
||||
{
|
||||
/* Nothing to do if queue is empty */
|
||||
if (s_tq_head == s_tq_tail) {
|
||||
s_active_freq = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
ToneSeq *seq = &s_tone_q[s_tq_head];
|
||||
|
||||
/* First call for this sequence entry: arm the first step */
|
||||
if (seq->step_end_ms == 0u) {
|
||||
const ToneStep *st = &seq->def->steps[0];
|
||||
seq->in_gap = false;
|
||||
seq->step_end_ms = now_ms + st->dur_ms;
|
||||
s_active_freq = st->freq_hz;
|
||||
s_active_phase = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
/* Step / gap still running */
|
||||
if (now_ms < seq->step_end_ms) return;
|
||||
|
||||
/* Current step or gap has expired */
|
||||
const ToneStep *st = &seq->def->steps[seq->step];
|
||||
|
||||
if (!seq->in_gap && st->gap_ms) {
|
||||
/* Transition: tone → inter-step gap (silence) */
|
||||
seq->in_gap = true;
|
||||
seq->step_end_ms = now_ms + st->gap_ms;
|
||||
s_active_freq = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
/* Advance to next step */
|
||||
seq->step++;
|
||||
seq->in_gap = false;
|
||||
|
||||
if (seq->step >= seq->def->n_steps) {
|
||||
/* Sequence complete — pop from queue */
|
||||
s_tq_head = (s_tq_head + 1u) % TONE_QUEUE_DEPTH;
|
||||
s_active_freq = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
/* Start next step */
|
||||
st = &seq->def->steps[seq->step];
|
||||
seq->step_end_ms = now_ms + st->dur_ms;
|
||||
s_active_freq = st->freq_hz;
|
||||
s_active_phase = 0;
|
||||
}
|
||||
|
||||
bool audio_is_playing(void)
|
||||
{
|
||||
return (s_i2s.State == HAL_I2S_STATE_BUSY_TX);
|
||||
}
|
||||
10
src/jlink.c
10
src/jlink.c
@ -1,4 +1,5 @@
|
||||
#include "jlink.h"
|
||||
#include "audio.h"
|
||||
#include "config.h"
|
||||
#include "stm32f7xx_hal.h"
|
||||
#include <string.h>
|
||||
@ -168,6 +169,13 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
|
||||
jlink_state.estop_req = 1u;
|
||||
break;
|
||||
|
||||
case JLINK_CMD_AUDIO:
|
||||
/* Payload: int16 PCM samples, little-endian, 1..126 samples (2..252 bytes) */
|
||||
if (plen >= 2u && (plen & 1u) == 0u) {
|
||||
audio_write_pcm((const int16_t *)payload, plen / 2u);
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@ -182,7 +190,7 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
|
||||
* Maximum payload = 253 - 1 = 252 bytes (LEN field is 1 byte, max 0xFF=255,
|
||||
* but we cap at 64 for safety).
|
||||
*/
|
||||
#define JLINK_MAX_PAYLOAD 64u
|
||||
#define JLINK_MAX_PAYLOAD 252u /* enlarged for AUDIO chunks (126 × int16) */
|
||||
|
||||
typedef enum {
|
||||
PS_WAIT_STX = 0,
|
||||
|
||||
16
src/main.c
16
src/main.c
@ -18,6 +18,7 @@
|
||||
#include "jetson_cmd.h"
|
||||
#include "jlink.h"
|
||||
#include "ota.h"
|
||||
#include "audio.h"
|
||||
#include "battery.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
@ -144,6 +145,10 @@ int main(void) {
|
||||
/* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */
|
||||
jlink_init();
|
||||
|
||||
/* Init I2S3 audio amplifier (PC10/PA15/PB5, mute=PC5) */
|
||||
audio_init();
|
||||
audio_play_tone(AUDIO_TONE_STARTUP);
|
||||
|
||||
/* Init mode manager (RC/autonomous blend; CH6 mode switch) */
|
||||
mode_manager_t mode;
|
||||
mode_manager_init(&mode);
|
||||
@ -188,6 +193,9 @@ int main(void) {
|
||||
/* Feed hardware watchdog — must happen every WATCHDOG_TIMEOUT_MS */
|
||||
safety_refresh();
|
||||
|
||||
/* Advance audio tone sequencer (non-blocking, call every tick) */
|
||||
audio_tick(now);
|
||||
|
||||
/* Mode manager: update RC liveness, CH6 mode selection, blend ramp */
|
||||
mode_manager_update(&mode, now);
|
||||
|
||||
@ -293,6 +301,7 @@ int main(void) {
|
||||
if (safety_arm_ready(now) && bal.state == BALANCE_DISARMED) {
|
||||
safety_arm_cancel();
|
||||
balance_arm(&bal);
|
||||
audio_play_tone(AUDIO_TONE_ARM);
|
||||
}
|
||||
if (cdc_disarm_request) {
|
||||
cdc_disarm_request = 0;
|
||||
@ -341,6 +350,13 @@ int main(void) {
|
||||
}
|
||||
|
||||
/* Latch estop on tilt fault or disarm */
|
||||
{
|
||||
static uint8_t s_prev_tilt_fault = 0;
|
||||
uint8_t tilt_now = (bal.state == BALANCE_TILT_FAULT) ? 1u : 0u;
|
||||
if (tilt_now && !s_prev_tilt_fault)
|
||||
audio_play_tone(AUDIO_TONE_FAULT);
|
||||
s_prev_tilt_fault = tilt_now;
|
||||
}
|
||||
if (bal.state == BALANCE_TILT_FAULT) {
|
||||
motor_driver_estop(&motors);
|
||||
} else if (bal.state == BALANCE_DISARMED && motors.estop &&
|
||||
|
||||
@ -68,8 +68,10 @@ void mpu6000_calibrate(void) {
|
||||
sum_gy += raw.gy;
|
||||
sum_gz += raw.gz;
|
||||
HAL_Delay(1);
|
||||
/* Refresh IWDG every 40ms — safe during re-cal with watchdog running */
|
||||
if (i % 40 == 39) safety_refresh();
|
||||
/* Refresh IWDG every 40ms, starting immediately (i=0) — the gap between
|
||||
* safety_refresh() at the top of the main loop and entry here can be
|
||||
* ~10ms, so we must refresh on i=0 to avoid the 50ms IWDG window. */
|
||||
if (i % 40 == 0) safety_refresh();
|
||||
}
|
||||
|
||||
s_bias_gx = (float)sum_gx / GYRO_CAL_SAMPLES;
|
||||
|
||||
409
test/test_audio.py
Normal file
409
test/test_audio.py
Normal file
@ -0,0 +1,409 @@
|
||||
"""
|
||||
test_audio.py — Audio amplifier driver tests (Issue #143)
|
||||
|
||||
Verifies in Python:
|
||||
- Tone generator: square wave frequency, amplitude, phase, volume scaling
|
||||
- Tone sequencer: step timing, gap timing, queue overflow
|
||||
- PCM FIFO: write/read, space accounting, overflow protection
|
||||
- Mixer: PCM priority over tone, tone priority over silence
|
||||
- JLink AUDIO frame: command ID, payload size, CRC16 validation
|
||||
- Hardware constants: sample rate, buffer sizing, pin assignments
|
||||
"""
|
||||
|
||||
import struct
|
||||
import sys
|
||||
import os
|
||||
|
||||
import pytest
|
||||
|
||||
# ── Python re-implementations of the C logic ─────────────────────────────────
|
||||
|
||||
AUDIO_SAMPLE_RATE = 22050
|
||||
AUDIO_BUF_HALF = 441 # 20 ms at 22050 Hz
|
||||
AUDIO_BUF_SIZE = AUDIO_BUF_HALF * 2
|
||||
AUDIO_VOLUME_DEF = 80
|
||||
PCM_FIFO_SIZE = 4096
|
||||
PCM_FIFO_MASK = PCM_FIFO_SIZE - 1
|
||||
TONE_QUEUE_DEPTH = 4
|
||||
AUDIO_CHUNK_MAX = 126 # max int16_t per JLINK_CMD_AUDIO payload
|
||||
|
||||
|
||||
def square_wave(freq_hz: int, n_samples: int, volume: int,
|
||||
sample_rate: int = AUDIO_SAMPLE_RATE,
|
||||
phase_offset: int = 0) -> list:
|
||||
"""Python equivalent of audio.c fill_half() square-wave branch."""
|
||||
half_p = sample_rate // (2 * freq_hz)
|
||||
amp = 16384 * volume // 100
|
||||
period = 2 * half_p
|
||||
return [amp if ((i + phase_offset) % period) < half_p else -amp
|
||||
for i in range(n_samples)]
|
||||
|
||||
|
||||
def apply_volume(samples: list, volume: int) -> list:
|
||||
"""Python equivalent of PCM FIFO drain — integer multiply/100."""
|
||||
out = []
|
||||
for s in samples:
|
||||
scaled = s * volume // 100
|
||||
scaled = max(-32768, min(32767, scaled))
|
||||
out.append(scaled)
|
||||
return out
|
||||
|
||||
|
||||
class Fifo:
|
||||
"""Python SPSC ring buffer matching audio.c PCM FIFO semantics."""
|
||||
def __init__(self, size: int = PCM_FIFO_SIZE):
|
||||
self.buf = [0] * size
|
||||
self.mask = size - 1
|
||||
self.rd = 0
|
||||
self.wr = 0
|
||||
|
||||
@property
|
||||
def avail(self) -> int:
|
||||
return (self.wr - self.rd) & self.mask
|
||||
|
||||
@property
|
||||
def space(self) -> int:
|
||||
return (self.rd - self.wr - 1) & self.mask
|
||||
|
||||
def write(self, samples: list) -> int:
|
||||
space = self.space
|
||||
accept = min(len(samples), space)
|
||||
for i in range(accept):
|
||||
self.buf[self.wr] = samples[i]
|
||||
self.wr = (self.wr + 1) & self.mask
|
||||
return accept
|
||||
|
||||
def read(self, n: int) -> list:
|
||||
out = []
|
||||
for _ in range(min(n, self.avail)):
|
||||
out.append(self.buf[self.rd])
|
||||
self.rd = (self.rd + 1) & self.mask
|
||||
return out
|
||||
|
||||
|
||||
def _crc16_xmodem(data: bytes) -> int:
|
||||
crc = 0x0000
|
||||
for b in data:
|
||||
crc ^= b << 8
|
||||
for _ in range(8):
|
||||
crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF
|
||||
return crc
|
||||
|
||||
|
||||
def build_audio_frame(samples: list) -> bytes:
|
||||
"""Build JLINK_CMD_AUDIO frame for given int16_t samples."""
|
||||
STX, ETX = 0x02, 0x03
|
||||
CMD = 0x08
|
||||
payload = struct.pack(f'<{len(samples)}h', *samples)
|
||||
body = bytes([CMD]) + payload
|
||||
crc = _crc16_xmodem(body)
|
||||
return bytes([STX, len(body)]) + body + bytes([crc >> 8, crc & 0xFF, ETX])
|
||||
|
||||
|
||||
# ── Square wave generator ─────────────────────────────────────────────────────
|
||||
|
||||
class TestSquareWave:
|
||||
def test_fundamental_frequency(self):
|
||||
"""Peak-to-peak transitions occur at the right sample intervals."""
|
||||
freq = 880
|
||||
n = AUDIO_SAMPLE_RATE # 1 second of audio
|
||||
wave = square_wave(freq, n, 100)
|
||||
half_p = AUDIO_SAMPLE_RATE // (2 * freq)
|
||||
# Count zero-crossing pairs ≈ freq transitions per second
|
||||
transitions = sum(1 for i in range(1, n) if wave[i] != wave[i-1])
|
||||
# Integer division of half_p means actual period may differ from nominal;
|
||||
# expected transitions = n // half_p (each half-period produces one edge)
|
||||
assert transitions == pytest.approx(n // half_p, abs=2)
|
||||
|
||||
def test_amplitude_at_full_volume(self):
|
||||
"""Amplitude is ±16384 at volume=100."""
|
||||
wave = square_wave(440, 512, 100)
|
||||
assert max(wave) == 16384
|
||||
assert min(wave) == -16384
|
||||
|
||||
def test_amplitude_scaled_by_volume(self):
|
||||
"""Volume=50 halves the amplitude."""
|
||||
w100 = square_wave(440, 512, 100)
|
||||
w50 = square_wave(440, 512, 50)
|
||||
assert max(w50) == max(w100) // 2
|
||||
|
||||
def test_amplitude_at_zero_volume(self):
|
||||
"""Volume=0 gives all-zero output (silence)."""
|
||||
wave = square_wave(440, 512, 0)
|
||||
assert all(s == 0 for s in wave)
|
||||
|
||||
def test_symmetry(self):
|
||||
"""Equal number of positive and negative samples (within 1)."""
|
||||
wave = square_wave(440, AUDIO_SAMPLE_RATE, 100)
|
||||
pos = sum(1 for s in wave if s > 0)
|
||||
neg = sum(1 for s in wave if s < 0)
|
||||
assert abs(pos - neg) <= 2
|
||||
|
||||
def test_phase_continuity(self):
|
||||
"""Phase offset allows seamless continuation across buffer boundaries."""
|
||||
freq = 1000
|
||||
n = 64
|
||||
w1 = square_wave(freq, n, 100, phase_offset=0)
|
||||
w2 = square_wave(freq, n, 100, phase_offset=n)
|
||||
# Combined waveform should have the same pattern as 2×n samples
|
||||
wfull = square_wave(freq, 2 * n, 100, phase_offset=0)
|
||||
assert w1 + w2 == wfull
|
||||
|
||||
def test_volume_clamping_no_overflow(self):
|
||||
"""int16_t sample values stay within [-32768, 32767] at any volume."""
|
||||
for vol in [0, 50, 80, 100]:
|
||||
wave = square_wave(440, 256, vol)
|
||||
assert all(-32768 <= s <= 32767 for s in wave)
|
||||
|
||||
def test_different_frequencies_produce_different_waveforms(self):
|
||||
w440 = square_wave(440, 512, 100)
|
||||
w880 = square_wave(880, 512, 100)
|
||||
w1000 = square_wave(1000, 512, 100)
|
||||
assert w440 != w880
|
||||
assert w880 != w1000
|
||||
|
||||
|
||||
# ── Tone sequencer ────────────────────────────────────────────────────────────
|
||||
|
||||
class TestToneSequencer:
|
||||
def test_step_duration(self):
|
||||
"""A 100 ms tone step at 22050 Hz spans exactly 2205 samples."""
|
||||
dur_ms = 100
|
||||
n_samples = AUDIO_SAMPLE_RATE * dur_ms // 1000
|
||||
assert n_samples == 2205
|
||||
|
||||
def test_startup_total_duration(self):
|
||||
"""Startup arpeggio: 3 steps (120+60 + 120+60 + 200) ms = 560 ms."""
|
||||
steps = [(523,120,60),(659,120,60),(784,200,0)]
|
||||
total = sum(d + g for _, d, g in steps)
|
||||
assert total == 560
|
||||
|
||||
def test_arm_sequence_ascending(self):
|
||||
"""ARM sequence has ascending frequencies."""
|
||||
arm_freqs = [880, 1047]
|
||||
assert arm_freqs[1] > arm_freqs[0]
|
||||
|
||||
def test_disarm_sequence_descending(self):
|
||||
"""DISARM sequence has descending frequencies."""
|
||||
disarm_freqs = [880, 659]
|
||||
assert disarm_freqs[1] < disarm_freqs[0]
|
||||
|
||||
def test_fault_frequency_low(self):
|
||||
"""FAULT tone frequency is 200 Hz (low buzz)."""
|
||||
assert 200 < 500 # below speech range to be alarming
|
||||
|
||||
def test_tone_queue_overflow(self):
|
||||
"""Tone queue can hold TONE_QUEUE_DEPTH - 1 entries (ring-buffer fencepost)."""
|
||||
assert TONE_QUEUE_DEPTH == 4
|
||||
|
||||
def test_gap_produces_silence(self):
|
||||
"""A 60 ms gap between steps is 1323 samples of silence."""
|
||||
gap_ms = 60
|
||||
n_silence = AUDIO_SAMPLE_RATE * gap_ms // 1000
|
||||
assert n_silence == 1323
|
||||
|
||||
|
||||
# ── PCM FIFO ──────────────────────────────────────────────────────────────────
|
||||
|
||||
class TestPcmFifo:
|
||||
def test_initial_empty(self):
|
||||
f = Fifo()
|
||||
assert f.avail == 0
|
||||
assert f.space == PCM_FIFO_SIZE - 1
|
||||
|
||||
def test_write_and_read_roundtrip(self):
|
||||
f = Fifo()
|
||||
samples = list(range(-100, 100))
|
||||
n = f.write(samples)
|
||||
assert n == len(samples)
|
||||
out = f.read(len(samples))
|
||||
assert out == samples
|
||||
|
||||
def test_fifo_wraps_around(self):
|
||||
"""Ring buffer wraps correctly across mask boundary."""
|
||||
f = Fifo(size=8)
|
||||
# Advance pointer to near end
|
||||
f.wr = 6; f.rd = 6
|
||||
f.write([10, 20, 30])
|
||||
out = f.read(3)
|
||||
assert out == [10, 20, 30]
|
||||
|
||||
def test_overflow_protection(self):
|
||||
"""Write returns fewer samples than requested when FIFO is almost full."""
|
||||
f = Fifo(size=8)
|
||||
written = f.write([1, 2, 3, 4, 5, 6, 7, 8]) # can only fit 7 (fencepost)
|
||||
assert written == 7
|
||||
|
||||
def test_empty_read_returns_empty(self):
|
||||
f = Fifo()
|
||||
assert f.read(10) == []
|
||||
|
||||
def test_space_decreases_after_write(self):
|
||||
f = Fifo()
|
||||
space_before = f.space
|
||||
f.write([0] * 100)
|
||||
assert f.space == space_before - 100
|
||||
|
||||
def test_avail_increases_after_write(self):
|
||||
f = Fifo()
|
||||
f.write(list(range(50)))
|
||||
assert f.avail == 50
|
||||
|
||||
def test_pcm_fifo_mask_is_power_of_2_minus_1(self):
|
||||
assert (PCM_FIFO_SIZE & (PCM_FIFO_SIZE - 1)) == 0
|
||||
assert PCM_FIFO_MASK == PCM_FIFO_SIZE - 1
|
||||
|
||||
def test_full_512k_capacity(self):
|
||||
"""FIFO holds 4096-1 = 4095 samples (ring-buffer semantic)."""
|
||||
f = Fifo()
|
||||
chunk = [42] * 4095
|
||||
n = f.write(chunk)
|
||||
assert n == 4095
|
||||
assert f.avail == 4095
|
||||
|
||||
|
||||
# ── Mixer priority ────────────────────────────────────────────────────────────
|
||||
|
||||
class TestMixer:
|
||||
def test_pcm_overrides_tone(self):
|
||||
"""When PCM FIFO has enough data it should be drained, not tone."""
|
||||
f = Fifo()
|
||||
f.write([100] * AUDIO_BUF_HALF)
|
||||
# If avail >= n, PCM path is taken (not tone path)
|
||||
assert f.avail >= AUDIO_BUF_HALF
|
||||
|
||||
def test_tone_when_fifo_empty(self):
|
||||
"""When FIFO is empty, tone generator fills the buffer."""
|
||||
f = Fifo()
|
||||
avail = f.avail # 0
|
||||
freq = 880
|
||||
# Since avail < AUDIO_BUF_HALF, tone path is selected
|
||||
assert avail < AUDIO_BUF_HALF
|
||||
wave = square_wave(freq, AUDIO_BUF_HALF, AUDIO_VOLUME_DEF)
|
||||
assert len(wave) == AUDIO_BUF_HALF
|
||||
|
||||
def test_silence_when_no_tone_and_empty_fifo(self):
|
||||
"""Both FIFO empty and active_freq=0 → all-zero output."""
|
||||
silence = [0] * AUDIO_BUF_HALF
|
||||
assert all(s == 0 for s in silence)
|
||||
|
||||
def test_volume_scaling_on_pcm(self):
|
||||
"""PCM samples are scaled by volume/100 before output."""
|
||||
raw = [16000, -16000, 8000]
|
||||
vol80 = apply_volume(raw, 80)
|
||||
assert vol80[0] == 16000 * 80 // 100
|
||||
assert vol80[1] == -16000 * 80 // 100
|
||||
|
||||
|
||||
# ── JLink AUDIO frame ─────────────────────────────────────────────────────────
|
||||
|
||||
JLINK_CMD_AUDIO = 0x08
|
||||
JLINK_MAX_PAYLOAD = 252
|
||||
|
||||
class TestJlinkAudioFrame:
|
||||
def test_cmd_id(self):
|
||||
assert JLINK_CMD_AUDIO == 0x08
|
||||
|
||||
def test_max_payload_bytes(self):
|
||||
"""252 bytes = 126 int16_t samples."""
|
||||
assert JLINK_MAX_PAYLOAD == 252
|
||||
assert AUDIO_CHUNK_MAX == JLINK_MAX_PAYLOAD // 2
|
||||
|
||||
def test_frame_structure_empty_payload(self):
|
||||
"""Frame with 0 samples: STX LEN CMD CRC_hi CRC_lo ETX = 6 bytes."""
|
||||
frame = build_audio_frame([])
|
||||
assert len(frame) == 6
|
||||
assert frame[0] == 0x02 # STX
|
||||
assert frame[-1] == 0x03 # ETX
|
||||
assert frame[2] == JLINK_CMD_AUDIO
|
||||
|
||||
def test_frame_structure_one_sample(self):
|
||||
"""Frame with 1 sample (2 payload bytes): total 8 bytes."""
|
||||
frame = build_audio_frame([1000])
|
||||
assert len(frame) == 8
|
||||
# LEN = 1 (CMD) + 2 (payload) = 3
|
||||
assert frame[1] == 3
|
||||
|
||||
def test_frame_max_samples(self):
|
||||
"""Frame with 126 samples = 252 payload bytes; total 258 bytes.
|
||||
STX(1)+LEN(1)+CMD(1)+payload(252)+CRC_hi(1)+CRC_lo(1)+ETX(1) = 258."""
|
||||
samples = list(range(126))
|
||||
frame = build_audio_frame(samples)
|
||||
assert len(frame) == 258
|
||||
|
||||
def test_frame_crc_validates(self):
|
||||
"""CRC in AUDIO frame validates against CMD+payload."""
|
||||
samples = [1000, -1000, 500, -500]
|
||||
frame = build_audio_frame(samples)
|
||||
# Body = CMD byte + payload
|
||||
body = frame[2:-3] # CMD + payload (skip STX, LEN, CRC_hi, CRC_lo, ETX)
|
||||
# Actually: frame = [STX][LEN][CMD][...payload...][CRC_hi][CRC_lo][ETX]
|
||||
cmd_and_payload = bytes([frame[2]]) + frame[3:-3]
|
||||
expected_crc = _crc16_xmodem(cmd_and_payload)
|
||||
crc_in_frame = (frame[-3] << 8) | frame[-2]
|
||||
assert crc_in_frame == expected_crc
|
||||
|
||||
def test_frame_payload_little_endian(self):
|
||||
"""Samples are encoded as little-endian int16_t."""
|
||||
samples = [0x1234]
|
||||
frame = build_audio_frame(samples)
|
||||
# payload bytes at frame[3:5]
|
||||
lo, hi = frame[3], frame[4]
|
||||
assert lo == 0x34
|
||||
assert hi == 0x12
|
||||
|
||||
def test_odd_payload_bytes_rejected(self):
|
||||
"""Payload with odd byte count must not be passed (always even: 2*N samples)."""
|
||||
# Odd-byte payload would be malformed; driver checks (plen & 1) == 0
|
||||
bad_plen = 5
|
||||
assert bad_plen % 2 != 0
|
||||
|
||||
def test_audio_cmd_follows_estop(self):
|
||||
"""AUDIO (0x08) is numerically after ESTOP (0x07)."""
|
||||
JLINK_CMD_ESTOP = 0x07
|
||||
assert JLINK_CMD_AUDIO > JLINK_CMD_ESTOP
|
||||
|
||||
|
||||
# ── Hardware constants ────────────────────────────────────────────────────────
|
||||
|
||||
class TestHardwareConstants:
|
||||
def test_sample_rate(self):
|
||||
assert AUDIO_SAMPLE_RATE == 22050
|
||||
|
||||
def test_buf_half_is_20ms(self):
|
||||
"""441 samples at 22050 Hz ≈ 20 ms."""
|
||||
ms = AUDIO_BUF_HALF * 1000 / AUDIO_SAMPLE_RATE
|
||||
assert abs(ms - 20.0) < 0.1
|
||||
|
||||
def test_buf_size_is_two_halves(self):
|
||||
assert AUDIO_BUF_SIZE == AUDIO_BUF_HALF * 2
|
||||
|
||||
def test_dma_half_irq_latency_budget(self):
|
||||
"""At 22050 Hz, 441 samples give 20 ms to refill — well above 1 ms loop."""
|
||||
refill_budget_ms = AUDIO_BUF_HALF * 1000 / AUDIO_SAMPLE_RATE
|
||||
main_loop_ms = 1 # 1 kHz main loop
|
||||
assert refill_budget_ms > main_loop_ms * 5 # 20x margin
|
||||
|
||||
def test_plli2s_frequency(self):
|
||||
"""PLLI2S: N=192, R=2, PLLM=8, HSE=8 MHz → 96 MHz I2S clock."""
|
||||
hse_mhz = 8
|
||||
pllm = 8
|
||||
plli2s_n = 192
|
||||
plli2s_r = 2
|
||||
i2s_clk = (hse_mhz / pllm) * plli2s_n / plli2s_r
|
||||
assert i2s_clk == pytest.approx(96.0)
|
||||
|
||||
def test_actual_sample_rate_accuracy(self):
|
||||
"""Actual FS with I2SDIV=68 is within 0.1% of 22050 Hz."""
|
||||
i2s_clk = 96_000_000
|
||||
i2sdiv = 68
|
||||
fs_actual = i2s_clk / (32 * 2 * i2sdiv)
|
||||
assert abs(fs_actual - 22050) / 22050 < 0.001
|
||||
|
||||
def test_volume_default_in_range(self):
|
||||
assert 0 <= AUDIO_VOLUME_DEF <= 100
|
||||
|
||||
def test_pcm_fifo_185ms_capacity(self):
|
||||
"""4096 samples at 22050 Hz ≈ 185.76 ms of audio (Jetson jitter buffer)."""
|
||||
ms = PCM_FIFO_SIZE * 1000 / AUDIO_SAMPLE_RATE
|
||||
assert ms == pytest.approx(185.76, abs=0.1)
|
||||
Loading…
x
Reference in New Issue
Block a user