Compare commits
1 Commits
8592361095
...
64de6667b6
| Author | SHA1 | Date | |
|---|---|---|---|
| 64de6667b6 |
@ -1,599 +0,0 @@
|
||||
// ============================================================
|
||||
// gimbal_camera_mount.scad — Pan/Tilt Gimbal Mount for RealSense D435i
|
||||
// Issue: #552 Agent: sl-mechanical Date: 2026-03-14
|
||||
// ============================================================
|
||||
//
|
||||
// Parametric gimbal bracket system mounting an Intel RealSense D435i
|
||||
// (or similar box camera) on a 2-axis pan/tilt gimbal driven by
|
||||
// ST3215 serial bus servos (25T spline, Feetech/Waveshare).
|
||||
//
|
||||
// Architecture:
|
||||
// Pan axis — base T-nut clamps to 2020 rail; pan servo rotates yoke
|
||||
// Tilt axis — tilt servo horn plate bolts to ST3215 horn; camera cradle
|
||||
// rocks on tilt axis
|
||||
// Camera — D435i captured via 1/4-20 UNC hex nut in cradle floor
|
||||
// Damping — PETG flexure ribs on camera contact faces (or TPU pads)
|
||||
// Wiring — USB-C cable routed through channel in cradle arm
|
||||
//
|
||||
// Part catalogue:
|
||||
// Part 1 — tnut_rail_base() 2020 rail T-nut base + pan servo seat
|
||||
// Part 2 — pan_yoke() U-yoke connecting pan servo to tilt axis
|
||||
// Part 3 — tilt_horn_plate() Plate bolting to ST3215 tilt servo horn
|
||||
// Part 4 — camera_cradle() D435i cradle with 1/4-20 captured nut
|
||||
// Part 5 — vibe_pad() PETG flexure vibration-damping pad (×2)
|
||||
// Part 6 — assembly_preview() Full assembly preview
|
||||
//
|
||||
// Hardware BOM (per gimbal):
|
||||
// 2× ST3215 serial bus servo (pan + tilt)
|
||||
// 2× servo horn (25T spline, ≥Ø36 mm, 4× M3 bolt holes on Ø24 mm BC)
|
||||
// 2× M3 × 8 mm SHCS horn-to-plate bolts (×4 each horn = 8 total)
|
||||
// 1× M3 × 16 mm SHCS + nut T-nut rail clamp thumbscrew
|
||||
// 1× 1/4-20 UNC × 8 mm SHCS camera retention bolt (or existing tripod screw)
|
||||
// 1× 1/4-20 UNC hex nut captured in cradle floor
|
||||
// 4× M3 × 12 mm SHCS yoke-to-tilt-plate pivot axle bolts
|
||||
// 2× M3 × 25 mm SHCS pan yoke attachment to servo body
|
||||
// (optional) 2× vibe_pad printed in TPU 95A
|
||||
//
|
||||
// ST3215 servo interface (caliper-verified Feetech ST3215):
|
||||
// Body footprint : 40.0 × 20.0 mm (W × D), 36.5 mm tall
|
||||
// Shaft centre H : 28.5 mm from mounting face
|
||||
// Shaft spline : 25T, centre Ø5.8 mm, D-cut
|
||||
// Mount holes : 4× M3 on 32 × 10 mm rectangular pattern (18 mm offset)
|
||||
// Horn bolt circle: Ø24 mm, 4× M3
|
||||
// Horn OD : ~36 mm
|
||||
//
|
||||
// D435i camera interface (caliper-verified):
|
||||
// Body : 90 × 25 × 25 mm (W × D × H)
|
||||
// Tripod thread : 1/4-20 UNC, centred bottom face, 9 mm from front
|
||||
// USB-C connector: right rear, 8 × 5 mm opening, 4 mm from edge
|
||||
//
|
||||
// Parametric camera size (override to adapt to other cameras):
|
||||
// CAM_W, CAM_D, CAM_H — body envelope
|
||||
// CAM_MOUNT_X — tripod hole X offset from camera centre
|
||||
// CAM_MOUNT_Y — tripod hole Y offset from front face
|
||||
//
|
||||
// Coordinate convention:
|
||||
// Camera looks in +Y direction (forward)
|
||||
// Pan axis is Z (vertical); tilt axis is X (lateral)
|
||||
// Rail runs along Z; T-nut base at Z=0
|
||||
// All parts at assembly origin; translate for assembly_preview
|
||||
//
|
||||
// RENDER options:
|
||||
// "assembly" full assembly preview (default)
|
||||
// "tnut_rail_base_stl" Part 1
|
||||
// "pan_yoke_stl" Part 2
|
||||
// "tilt_horn_plate_stl" Part 3
|
||||
// "camera_cradle_stl" Part 4
|
||||
// "vibe_pad_stl" Part 5
|
||||
//
|
||||
// Export commands:
|
||||
// openscad gimbal_camera_mount.scad -D 'RENDER="tnut_rail_base_stl"' -o gcm_tnut_base.stl
|
||||
// openscad gimbal_camera_mount.scad -D 'RENDER="pan_yoke_stl"' -o gcm_pan_yoke.stl
|
||||
// openscad gimbal_camera_mount.scad -D 'RENDER="tilt_horn_plate_stl"' -o gcm_tilt_horn_plate.stl
|
||||
// openscad gimbal_camera_mount.scad -D 'RENDER="camera_cradle_stl"' -o gcm_camera_cradle.stl
|
||||
// openscad gimbal_camera_mount.scad -D 'RENDER="vibe_pad_stl"' -o gcm_vibe_pad.stl
|
||||
// ============================================================
|
||||
|
||||
$fn = 64;
|
||||
e = 0.01; // epsilon for boolean clearance
|
||||
|
||||
// ── Parametric camera envelope ────────────────────────────────────────────────
|
||||
// Override these for cameras other than D435i
|
||||
CAM_W = 90.0; // camera body width (X)
|
||||
CAM_D = 25.0; // camera body depth (Y)
|
||||
CAM_H = 25.0; // camera body height (Z)
|
||||
CAM_MOUNT_X = 0.0; // tripod hole X offset from camera body centre
|
||||
CAM_MOUNT_Y = 9.0; // tripod hole from front face (Y) [D435i: 9 mm]
|
||||
CAM_USBC_X = CAM_W/2 - 4; // USB-C connector X (right side)
|
||||
CAM_USBC_Z = CAM_H/2; // USB-C connector Z (mid-height rear)
|
||||
CAM_USBC_W = 9.0; // USB-C opening width (X)
|
||||
CAM_USBC_H = 5.0; // USB-C opening height (Z)
|
||||
|
||||
// ── Rail geometry (matches sensor_rail.scad / sensor_rail_brackets.scad) ─────
|
||||
RAIL_W = 20.0;
|
||||
SLOT_OPEN = 6.0;
|
||||
SLOT_INNER_W = 10.2;
|
||||
SLOT_INNER_H = 5.8;
|
||||
SLOT_NECK_H = 3.2;
|
||||
|
||||
// ── T-nut geometry (matches sensor_rail_brackets.scad) ───────────────────────
|
||||
TNUT_W = 9.8;
|
||||
TNUT_H = 5.5;
|
||||
TNUT_L = 12.0;
|
||||
TNUT_M3_NUT_AF = 5.5;
|
||||
TNUT_M3_NUT_H = 2.5;
|
||||
TNUT_BOLT_D = 3.3; // M3 clearance
|
||||
|
||||
// ── T-nut base plate geometry ─────────────────────────────────────────────────
|
||||
BASE_W = 44.0; // wide enough for pan servo body (40 mm)
|
||||
BASE_H = 40.0; // height along rail (Z)
|
||||
BASE_T = SLOT_NECK_H + 2.0; // plate depth (Y), rail-face side
|
||||
|
||||
// ── ST3215 servo geometry ─────────────────────────────────────────────────────
|
||||
SERVO_W = 40.0; // servo body width (X)
|
||||
SERVO_D = 20.0; // servo body depth (Y)
|
||||
SERVO_H = 36.5; // servo body height (Z)
|
||||
SERVO_SHAFT_Z = 28.5; // shaft centre height from mounting face
|
||||
SERVO_HOLE_X = 16.0; // mount hole half-span X (32 mm span)
|
||||
SERVO_HOLE_Y = 5.0; // mount hole half-span Y (10 mm span)
|
||||
SERVO_M3_D = 3.3; // M3 clearance
|
||||
|
||||
// ── Servo horn geometry ───────────────────────────────────────────────────────
|
||||
HORN_OD = 36.0; // horn outer diameter
|
||||
HORN_SPLINE_D = 5.9; // 25T spline bore clearance (5.8 + 0.1)
|
||||
HORN_BC_D = 24.0; // bolt circle diameter (4× M3)
|
||||
HORN_BOLT_D = 3.3; // M3 clearance through horn plate
|
||||
HORN_PLATE_T = 5.0; // tilt horn plate thickness
|
||||
|
||||
// ── Yoke geometry ─────────────────────────────────────────────────────────────
|
||||
YOKE_WALL_T = 5.0; // yoke arm wall thickness
|
||||
YOKE_ARM_H = 50.0; // yoke arm height (Z) — clears servo body + camera
|
||||
YOKE_INNER_W = CAM_W + 8.0; // yoke inner span (camera + pad clearance)
|
||||
YOKE_BASE_T = 8.0; // yoke base plate thickness
|
||||
|
||||
// ── Tilt pivot ────────────────────────────────────────────────────────────────
|
||||
PIVOT_D = 4.3; // M4 pivot axle bore
|
||||
PIVOT_BOSS_D = 10.0; // boss OD around pivot bore
|
||||
PIVOT_BOSS_L = 6.0; // boss protrusion from yoke wall
|
||||
|
||||
// ── Camera cradle geometry ────────────────────────────────────────────────────
|
||||
CRADLE_WALL_T = 4.0; // cradle side wall thickness
|
||||
CRADLE_FLOOR_T = 5.0; // cradle floor thickness (holds 1/4-20 nut)
|
||||
CRADLE_LIP_T = 3.0; // front retaining lip thickness
|
||||
CRADLE_LIP_H = 8.0; // front lip height
|
||||
CABLE_CH_W = 12.0; // USB-C cable channel width
|
||||
CABLE_CH_H = 8.0; // USB-C cable channel height
|
||||
|
||||
// ── 1/4-20 UNC tripod thread ──────────────────────────────────────────────────
|
||||
QTR20_D = 6.6; // 1/4-20 clearance bore
|
||||
QTR20_NUT_AF = 11.1; // 1/4-20 hex nut across-flats (standard)
|
||||
QTR20_NUT_H = 5.5; // 1/4-20 hex nut height
|
||||
|
||||
// ── Vibration-damping pad geometry ────────────────────────────────────────────
|
||||
PAD_W = CAM_W - 2*CRADLE_WALL_T - 2;
|
||||
PAD_H = CAM_H + 4;
|
||||
PAD_T = 2.5; // pad body thickness
|
||||
RIB_H = 1.5; // flexure rib height
|
||||
RIB_W = 1.2; // rib width
|
||||
RIB_PITCH = 5.0; // rib pitch
|
||||
|
||||
// ── Fastener sizes ────────────────────────────────────────────────────────────
|
||||
M3_D = 3.3;
|
||||
M4_D = 4.3;
|
||||
M3_NUT_AF = 5.5;
|
||||
M3_NUT_H = 2.4;
|
||||
|
||||
// ============================================================
|
||||
// RENDER DISPATCH
|
||||
// ============================================================
|
||||
RENDER = "assembly";
|
||||
|
||||
if (RENDER == "assembly") assembly_preview();
|
||||
else if (RENDER == "tnut_rail_base_stl") tnut_rail_base();
|
||||
else if (RENDER == "pan_yoke_stl") pan_yoke();
|
||||
else if (RENDER == "tilt_horn_plate_stl") tilt_horn_plate();
|
||||
else if (RENDER == "camera_cradle_stl") camera_cradle();
|
||||
else if (RENDER == "vibe_pad_stl") vibe_pad();
|
||||
|
||||
// ============================================================
|
||||
// ASSEMBLY PREVIEW
|
||||
// ============================================================
|
||||
module assembly_preview() {
|
||||
asm_rail_z = 0;
|
||||
// Rail section ghost (200 mm)
|
||||
%color("Silver", 0.25)
|
||||
translate([-RAIL_W/2, -RAIL_W/2, asm_rail_z])
|
||||
cube([RAIL_W, RAIL_W, 200]);
|
||||
|
||||
// T-nut rail base
|
||||
color("OliveDrab", 0.85)
|
||||
translate([0, 0, asm_rail_z + 80])
|
||||
tnut_rail_base();
|
||||
|
||||
// Pan servo ghost (sitting in base seat)
|
||||
%color("DimGray", 0.4)
|
||||
translate([-SERVO_W/2, BASE_T, asm_rail_z + 80 + (BASE_H - SERVO_H)/2])
|
||||
cube([SERVO_W, SERVO_D, SERVO_H]);
|
||||
|
||||
// Pan yoke rising from servo shaft
|
||||
color("SteelBlue", 0.85)
|
||||
translate([0, BASE_T + SERVO_D, asm_rail_z + 80 + BASE_H/2])
|
||||
pan_yoke();
|
||||
|
||||
// Tilt horn plate (tilt axis — left yoke wall)
|
||||
color("DarkOrange", 0.85)
|
||||
translate([-YOKE_INNER_W/2 - YOKE_WALL_T - HORN_PLATE_T,
|
||||
BASE_T + SERVO_D + YOKE_BASE_T,
|
||||
asm_rail_z + 80 + BASE_H/2 + YOKE_ARM_H/2])
|
||||
rotate([0, 90, 0])
|
||||
tilt_horn_plate();
|
||||
|
||||
// Camera cradle (centred in yoke)
|
||||
color("DarkSlateGray", 0.85)
|
||||
translate([0, BASE_T + SERVO_D + YOKE_BASE_T + CRADLE_FLOOR_T,
|
||||
asm_rail_z + 80 + BASE_H/2 + YOKE_ARM_H/2 - CAM_H/2])
|
||||
camera_cradle();
|
||||
|
||||
// D435i ghost
|
||||
%color("Black", 0.4)
|
||||
translate([-CAM_W/2,
|
||||
BASE_T + SERVO_D + YOKE_BASE_T + CRADLE_FLOOR_T + PAD_T,
|
||||
asm_rail_z + 80 + Base_H_mid() - CAM_H/2])
|
||||
cube([CAM_W, CAM_D, CAM_H]);
|
||||
|
||||
// Vibe pads (front + rear camera face)
|
||||
color("DimGray", 0.80) {
|
||||
translate([-CAM_W/2 + CRADLE_WALL_T + 1,
|
||||
Base_T + SERVO_D + YOKE_BASE_T + CRADLE_FLOOR_T,
|
||||
asm_rail_z + 80 + Base_H_mid() - PAD_H/2])
|
||||
rotate([90, 0, 0])
|
||||
vibe_pad();
|
||||
}
|
||||
}
|
||||
|
||||
// helper (avoids recomputing same expression)
|
||||
function Base_T() = BASE_T;
|
||||
function Base_H_mid() = BASE_H/2 + YOKE_ARM_H/2;
|
||||
|
||||
// ============================================================
|
||||
// PART 1 — T-NUT RAIL BASE (pan servo seat + rail clamp)
|
||||
// ============================================================
|
||||
// Mounts to 2020 rail via standard T-nut tongue.
|
||||
// Front face (+Y side) provides flat seat for pan ST3215 servo body.
|
||||
// Servo body recessed 1 mm into seat for positive lateral registration.
|
||||
// Pan servo shaft axis = Z (vertical) → pan rotation about Z.
|
||||
//
|
||||
// Print: PETG, 5 perims, 50 % gyroid. Orient face-plate down (flat).
|
||||
module tnut_rail_base() {
|
||||
difference() {
|
||||
union() {
|
||||
// ── Face plate (against rail outer face, -Y side) ────────────
|
||||
translate([-BASE_W/2, -BASE_T, 0])
|
||||
cube([BASE_W, BASE_T, BASE_H]);
|
||||
|
||||
// ── T-nut neck (enters rail slot, +Y side of face plate) ─────
|
||||
translate([-TNUT_W/2, 0, (BASE_H - TNUT_L)/2])
|
||||
cube([TNUT_W, SLOT_NECK_H + e, TNUT_L]);
|
||||
|
||||
// ── T-nut inner body (wider, locks inside T-groove) ──────────
|
||||
translate([-TNUT_W/2, SLOT_NECK_H - e, (BASE_H - TNUT_L)/2])
|
||||
cube([TNUT_W, TNUT_H - SLOT_NECK_H + e, TNUT_L]);
|
||||
|
||||
// ── Pan servo seat boss (front face, +Y side) ────────────────
|
||||
// Proud pad that servo body sits on; 1 mm registration recess
|
||||
translate([-BASE_W/2, -BASE_T, 0])
|
||||
cube([BASE_W, BASE_T + 6, BASE_H]);
|
||||
}
|
||||
|
||||
// ── Rail clamp bolt bore (M3 through face plate) ─────────────────
|
||||
translate([0, -BASE_T - e, BASE_H/2])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = TNUT_BOLT_D, h = BASE_T + TNUT_H + 2*e);
|
||||
|
||||
// ── M3 hex nut pocket (inside T-nut body) ────────────────────────
|
||||
translate([0, SLOT_NECK_H + 0.3, BASE_H/2])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = TNUT_M3_NUT_AF / cos(30),
|
||||
h = TNUT_M3_NUT_H + 0.3, $fn = 6);
|
||||
|
||||
// ── Servo body recess (1 mm registration pocket in seat face) ────
|
||||
translate([-SERVO_W/2 - 0.3, -BASE_T + 6 - 1.0,
|
||||
(BASE_H - SERVO_H)/2 - 0.3])
|
||||
cube([SERVO_W + 0.6, 1.2, SERVO_H + 0.6]);
|
||||
|
||||
// ── Pan servo mount holes (4× M3 in rectangular pattern) ─────────
|
||||
for (sx = [-SERVO_HOLE_X, SERVO_HOLE_X])
|
||||
for (sy = [-SERVO_HOLE_Y, SERVO_HOLE_Y])
|
||||
translate([sx, -BASE_T + 6 + e, BASE_H/2 + sy])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = SERVO_M3_D, h = BASE_T + 2*e);
|
||||
|
||||
// ── Pan servo shaft bore (passes shaft through base if needed) ────
|
||||
// Centre of shaft at Z = BASE_H/2, no bore needed (shaft exits top)
|
||||
|
||||
// ── Lightening pockets ────────────────────────────────────────────
|
||||
translate([0, -BASE_T/2 + 3, BASE_H/2])
|
||||
cube([BASE_W - 14, BASE_T - 4, BASE_H - 14], center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 2 — PAN YOKE
|
||||
// ============================================================
|
||||
// U-shaped yoke that attaches to pan servo horn (below) and carries
|
||||
// the tilt axis (above). Two vertical arms straddle the camera cradle.
|
||||
// Tilt servo sits on top of one arm; tilt pivot boss on the other.
|
||||
//
|
||||
// Yoke base bolts to pan servo horn (4× M3 on HORN_BC_D bolt circle).
|
||||
// Pan servo horn spline bore passes through yoke base centre.
|
||||
// Tilt axis: M4 pivot axle through boss on each arm (X-axis rotation).
|
||||
//
|
||||
// Print: upright (yoke in final orientation), PETG, 5 perims, 40% gyroid.
|
||||
module pan_yoke() {
|
||||
arm_z_total = YOKE_ARM_H + YOKE_BASE_T;
|
||||
inner_w = YOKE_INNER_W;
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── Yoke base plate (bolts to pan servo horn) ─────────────────
|
||||
translate([-inner_w/2 - YOKE_WALL_T, 0, 0])
|
||||
cube([inner_w + 2*YOKE_WALL_T, YOKE_BASE_T, YOKE_BASE_T]);
|
||||
|
||||
// ── Left arm ──────────────────────────────────────────────────
|
||||
translate([-inner_w/2 - YOKE_WALL_T, 0, 0])
|
||||
cube([YOKE_WALL_T, YOKE_BASE_T, arm_z_total]);
|
||||
|
||||
// ── Right arm (tilt servo side) ───────────────────────────────
|
||||
translate([inner_w/2, 0, 0])
|
||||
cube([YOKE_WALL_T, YOKE_BASE_T, arm_z_total]);
|
||||
|
||||
// ── Tilt pivot bosses (both arms, X-axis) ─────────────────────
|
||||
// Left pivot boss (plain pivot — M4 bolt)
|
||||
translate([-inner_w/2 - YOKE_WALL_T - PIVOT_BOSS_L,
|
||||
YOKE_BASE_T/2,
|
||||
YOKE_BASE_T + YOKE_ARM_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = PIVOT_BOSS_D, h = PIVOT_BOSS_L + YOKE_WALL_T);
|
||||
|
||||
// Right pivot boss (tilt servo horn seat)
|
||||
translate([inner_w/2,
|
||||
YOKE_BASE_T/2,
|
||||
YOKE_BASE_T + YOKE_ARM_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = PIVOT_BOSS_D + 4, h = PIVOT_BOSS_L + YOKE_WALL_T);
|
||||
|
||||
// ── Tilt servo body seat on right arm top ─────────────────────
|
||||
translate([inner_w/2, 0, arm_z_total - SERVO_H - 4])
|
||||
cube([YOKE_WALL_T + SERVO_D + 2, YOKE_BASE_T, SERVO_H + 4]);
|
||||
}
|
||||
|
||||
// ── Pan horn spline bore (centre of yoke base) ────────────────────
|
||||
translate([0, YOKE_BASE_T/2, YOKE_BASE_T/2])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = HORN_SPLINE_D, h = YOKE_BASE_T + 2*e,
|
||||
center = true);
|
||||
|
||||
// ── Pan horn bolt holes (4× M3 on HORN_BC_D) ─────────────────────
|
||||
for (a = [45, 135, 225, 315])
|
||||
translate([HORN_BC_D/2 * cos(a),
|
||||
YOKE_BASE_T/2,
|
||||
HORN_BC_D/2 * sin(a) + YOKE_BASE_T/2])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = HORN_BOLT_D, h = YOKE_BASE_T + 2*e,
|
||||
center = true);
|
||||
|
||||
// ── Left tilt pivot bore (M4 clearance) ───────────────────────────
|
||||
translate([-inner_w/2 - YOKE_WALL_T - PIVOT_BOSS_L - e,
|
||||
YOKE_BASE_T/2,
|
||||
YOKE_BASE_T + YOKE_ARM_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = PIVOT_D, h = PIVOT_BOSS_L + YOKE_WALL_T + 2*e);
|
||||
|
||||
// ── Right tilt pivot bore (larger — tilt horn plate seats here) ───
|
||||
translate([inner_w/2 - e,
|
||||
YOKE_BASE_T/2,
|
||||
YOKE_BASE_T + YOKE_ARM_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = HORN_SPLINE_D,
|
||||
h = PIVOT_BOSS_L + YOKE_WALL_T + 2*e);
|
||||
|
||||
// ── Tilt servo mount holes in right arm seat ──────────────────────
|
||||
for (sz = [-SERVO_HOLE_Y, SERVO_HOLE_Y])
|
||||
translate([inner_w/2 + YOKE_WALL_T + SERVO_D/2,
|
||||
YOKE_BASE_T/2,
|
||||
arm_z_total - SERVO_H/2 + sz])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = SERVO_M3_D, h = YOKE_BASE_T + 2*e,
|
||||
center = true);
|
||||
|
||||
// ── M3 nut pockets (tilt servo mount, rear of arm seat) ──────────
|
||||
for (sz = [-SERVO_HOLE_Y, SERVO_HOLE_Y])
|
||||
translate([inner_w/2 + YOKE_WALL_T + SERVO_D/2,
|
||||
YOKE_BASE_T - M3_NUT_H - 0.5,
|
||||
arm_z_total - SERVO_H/2 + sz])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = M3_NUT_AF / cos(30), h = M3_NUT_H + 0.5,
|
||||
$fn = 6);
|
||||
|
||||
// ── Lightening slots in yoke arms ─────────────────────────────────
|
||||
translate([-inner_w/2 - YOKE_WALL_T/2,
|
||||
YOKE_BASE_T/2,
|
||||
YOKE_BASE_T + YOKE_ARM_H/2 - 10])
|
||||
cube([YOKE_WALL_T - 2, YOKE_BASE_T - 2, YOKE_ARM_H - 24],
|
||||
center = true);
|
||||
translate([inner_w/2 + YOKE_WALL_T/2,
|
||||
YOKE_BASE_T/2,
|
||||
YOKE_BASE_T + YOKE_ARM_H/2 - 10])
|
||||
cube([YOKE_WALL_T - 2, YOKE_BASE_T - 2, YOKE_ARM_H - 30],
|
||||
center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 3 — TILT HORN PLATE
|
||||
// ============================================================
|
||||
// Disc plate bolting to tilt ST3215 servo horn on the right yoke arm.
|
||||
// Servo horn spline centres into disc bore (captured, no free rotation).
|
||||
// Camera cradle attaches to opposite face via 2× M3 bolts.
|
||||
//
|
||||
// Tilt range: ±45° limited by yoke arm geometry.
|
||||
// Plate thickness HORN_PLATE_T provides stiffness for cantilevered cradle.
|
||||
//
|
||||
// Print: flat (disc face down), PETG, 5 perims, 50 % infill.
|
||||
module tilt_horn_plate() {
|
||||
plate_od = HORN_OD + 8; // plate OD (4 mm rim outside horn BC)
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── Main disc ─────────────────────────────────────────────────
|
||||
cylinder(d = plate_od, h = HORN_PLATE_T);
|
||||
|
||||
// ── Cradle attachment arm (extends to camera cradle) ──────────
|
||||
// Rectangular boss on top of disc toward camera
|
||||
translate([-CAM_W/2, HORN_PLATE_T - e, -CAM_H/2])
|
||||
cube([CAM_W, HORN_PLATE_T + 4, CAM_H]);
|
||||
}
|
||||
|
||||
// ── Servo horn spline bore (centre) ───────────────────────────────
|
||||
translate([0, 0, -e])
|
||||
cylinder(d = HORN_SPLINE_D, h = HORN_PLATE_T + 2*e);
|
||||
|
||||
// ── Horn bolt holes (4× M3 on HORN_BC_D) ─────────────────────────
|
||||
for (a = [45, 135, 225, 315])
|
||||
translate([HORN_BC_D/2 * cos(a),
|
||||
HORN_BC_D/2 * sin(a), -e])
|
||||
cylinder(d = HORN_BOLT_D, h = HORN_PLATE_T + 2*e);
|
||||
|
||||
// ── Pivot axle bore (M4, coaxial with horn centre) ────────────────
|
||||
translate([0, 0, -e])
|
||||
cylinder(d = PIVOT_D, h = HORN_PLATE_T + 2*e);
|
||||
|
||||
// ── Cradle attachment bolts (2× M3 in arm boss) ──────────────────
|
||||
for (cz = [-CAM_H/2 + 6, CAM_H/2 - 6])
|
||||
translate([0, HORN_PLATE_T + 2, cz])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = M3_D, h = HORN_PLATE_T + 6 + 2*e);
|
||||
|
||||
// ── M3 hex nut pockets (rear of disc face) ────────────────────────
|
||||
for (cz = [-CAM_H/2 + 6, CAM_H/2 - 6])
|
||||
translate([0, M3_NUT_H + 0.5, cz])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = M3_NUT_AF / cos(30),
|
||||
h = M3_NUT_H + 0.5, $fn = 6);
|
||||
|
||||
// ── Weight-relief arcs (between horn bolt holes) ──────────────────
|
||||
for (a = [0, 90, 180, 270])
|
||||
translate([(plate_od/2 - 5) * cos(a),
|
||||
(plate_od/2 - 5) * sin(a), -e])
|
||||
cylinder(d = 6, h = HORN_PLATE_T + 2*e);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 4 — CAMERA CRADLE
|
||||
// ============================================================
|
||||
// Open-front U-cradle holding D435i via captured 1/4-20 hex nut.
|
||||
// Front lip retains camera from sliding forward (+Y).
|
||||
// Vibration-damping pads seat in recessed pockets on inner faces.
|
||||
// USB-C cable routing channel exits cradle right rear wall.
|
||||
//
|
||||
// 1/4-20 captured nut in cradle floor — tighten with standard
|
||||
// tripod screw or M6→1/4-20 adapter from camera bottom.
|
||||
//
|
||||
// Print: cradle-floor-down (flat), PETG, 5 perims, 40 % gyroid.
|
||||
// No supports needed (overhangs < 45°).
|
||||
module camera_cradle() {
|
||||
outer_w = CAM_W + 2*CRADLE_WALL_T;
|
||||
outer_h = CAM_H + CRADLE_FLOOR_T;
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── Cradle body ───────────────────────────────────────────────
|
||||
translate([-outer_w/2, 0, 0])
|
||||
cube([outer_w, CAM_D + CRADLE_WALL_T, outer_h]);
|
||||
|
||||
// ── Front retaining lip ───────────────────────────────────────
|
||||
translate([-outer_w/2, CAM_D + CRADLE_WALL_T - CRADLE_LIP_T, 0])
|
||||
cube([outer_w, CRADLE_LIP_T, CRADLE_LIP_H]);
|
||||
|
||||
// ── Cable channel boss (right rear, exits +X side) ────────────
|
||||
translate([CAM_W/2 + CRADLE_WALL_T - e,
|
||||
0,
|
||||
CRADLE_FLOOR_T + CAM_H/2 - CABLE_CH_H/2])
|
||||
cube([CABLE_CH_W + CRADLE_WALL_T, CAM_D * 0.6, CABLE_CH_H]);
|
||||
|
||||
// ── Tilt horn attachment tabs (left + right, bolt to horn plate)─
|
||||
for (sx = [-outer_w/2 - 4, outer_w/2])
|
||||
translate([sx, CAM_D/2, CRADLE_FLOOR_T + CAM_H/2 - 6])
|
||||
cube([4, 12, 12]);
|
||||
}
|
||||
|
||||
// ── Camera pocket (hollow interior) ──────────────────────────────
|
||||
translate([-CAM_W/2, 0, CRADLE_FLOOR_T])
|
||||
cube([CAM_W, CAM_D + CRADLE_WALL_T + e, CAM_H + e]);
|
||||
|
||||
// ── 1/4-20 UNC clearance bore (camera tripod thread, bottom) ─────
|
||||
translate([CAM_MOUNT_X, CAM_MOUNT_Y, -e])
|
||||
cylinder(d = QTR20_D, h = CRADLE_FLOOR_T + 2*e);
|
||||
|
||||
// ── 1/4-20 hex nut pocket (captured in cradle floor) ─────────────
|
||||
translate([CAM_MOUNT_X, CAM_MOUNT_Y, CRADLE_FLOOR_T - QTR20_NUT_H - 0.5])
|
||||
cylinder(d = QTR20_NUT_AF / cos(30),
|
||||
h = QTR20_NUT_H + 0.6, $fn = 6);
|
||||
|
||||
// ── USB-C cable channel (exit through right rear wall) ────────────
|
||||
translate([CAM_W/2 - e,
|
||||
0,
|
||||
CRADLE_FLOOR_T + CAM_H/2 - CABLE_CH_H/2])
|
||||
cube([CABLE_CH_W + CRADLE_WALL_T + 2*e,
|
||||
CAM_D * 0.6 + e, CABLE_CH_H]);
|
||||
|
||||
// ── Vibe pad recesses on inner camera-contact faces ───────────────
|
||||
// Rear wall recess (camera front face → +Y side of rear wall)
|
||||
translate([-CAM_W/2 + CRADLE_WALL_T, CRADLE_WALL_T, CRADLE_FLOOR_T])
|
||||
cube([CAM_W, PAD_T, CAM_H]);
|
||||
|
||||
// ── Tilt horn bolt holes in attachment tabs ───────────────────────
|
||||
for (sx = [-outer_w/2 - 4 - e, outer_w/2 - e])
|
||||
translate([sx, CAM_D/2 + 6, CRADLE_FLOOR_T + CAM_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = M3_D, h = 6 + 2*e);
|
||||
|
||||
// ── M3 nut pockets in attachment tabs ─────────────────────────────
|
||||
translate([outer_w/2 + 4 - M3_NUT_H - 0.4,
|
||||
CAM_D/2 + 6,
|
||||
CRADLE_FLOOR_T + CAM_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = M3_NUT_AF / cos(30),
|
||||
h = M3_NUT_H + 0.4, $fn = 6);
|
||||
translate([-outer_w/2 - 4 - e,
|
||||
CAM_D/2 + 6,
|
||||
CRADLE_FLOOR_T + CAM_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = M3_NUT_AF / cos(30),
|
||||
h = M3_NUT_H + 0.4, $fn = 6);
|
||||
|
||||
// ── Lightening pockets in cradle walls ────────────────────────────
|
||||
for (face_x = [-CAM_W/2 - CRADLE_WALL_T - e, CAM_W/2 - e])
|
||||
translate([face_x, CAM_D * 0.2, CRADLE_FLOOR_T + 3])
|
||||
cube([CRADLE_WALL_T + 2*e, CAM_D * 0.55, CAM_H - 6]);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 5 — VIBRATION-DAMPING PAD
|
||||
// ============================================================
|
||||
// Flat pad with transverse PETG flexure ribs pressing against camera body.
|
||||
// Rib geometry (thin fins ~1.5 mm tall) deflects under camera vibration,
|
||||
// attenuating high-frequency input from motor/drive-train.
|
||||
// For superior damping: print in TPU 95A (no infill changes needed).
|
||||
// Pads seat in recessed pockets in camera cradle inner wall.
|
||||
// Optional M2 bolt-through at corners or adhesive-back foam tape.
|
||||
//
|
||||
// Print: pad-back-face-down, PETG or TPU 95A, 3 perims, 20 % infill.
|
||||
module vibe_pad() {
|
||||
rib_count = floor((PAD_W - RIB_W) / RIB_PITCH);
|
||||
|
||||
union() {
|
||||
// ── Base plate ────────────────────────────────────────────────────
|
||||
translate([-PAD_W/2, -PAD_T, -PAD_H/2])
|
||||
cube([PAD_W, PAD_T, PAD_H]);
|
||||
|
||||
// ── Flexure ribs (parallel to Z, spaced RIB_PITCH apart) ─────────
|
||||
for (i = [0 : rib_count - 1]) {
|
||||
rx = -PAD_W/2 + RIB_PITCH/2 + i * RIB_PITCH + RIB_W/2;
|
||||
if (rx <= PAD_W/2 - RIB_W/2)
|
||||
translate([rx, 0, 0])
|
||||
cube([RIB_W, RIB_H, PAD_H - 6], center = true);
|
||||
}
|
||||
|
||||
// ── Corner nubs (M2 bolt-through retention, optional) ─────────────
|
||||
for (px = [-PAD_W/2 + 5, PAD_W/2 - 5])
|
||||
for (pz = [-PAD_H/2 + 5, PAD_H/2 - 5])
|
||||
translate([px, -PAD_T/2, pz])
|
||||
difference() {
|
||||
cylinder(d = 5, h = PAD_T, center = true);
|
||||
cylinder(d = 2.4, h = PAD_T + 2*e, center = true);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,502 +1,76 @@
|
||||
// ============================================================
|
||||
// rplidar_mount.scad — RPLIDAR A1 Elevated Bracket for 2020 T-Slot Rail
|
||||
// Issue: #561 Agent: sl-mechanical Date: 2026-03-14
|
||||
// (supersedes Rev A anti-vibration ring — ring integrated as Part 4)
|
||||
// rplidar_mount.scad — RPLIDAR A1M8 Anti-Vibration Ring Rev A
|
||||
// Agent: sl-mechanical 2026-02-28
|
||||
// ============================================================
|
||||
// Flat ring sits between platform and RPLIDAR A1M8.
|
||||
// Anti-vibration isolation via 4× M3 silicone grommets
|
||||
// (same type as FC vibration mounts — Ø6 mm silicone, M3).
|
||||
//
|
||||
// Complete elevated mount system for RPLIDAR A1 on 2020 aluminium T-slot
|
||||
// rail. Scanner raised ELEV_H mm above rail attachment point so the
|
||||
// 360° laser scan plane clears the rover/tank chassis body.
|
||||
// Bolt stack (bottom → top):
|
||||
// M3×30 SHCS → platform (8 mm) → grommet (8 mm) →
|
||||
// ring (4 mm) → RPLIDAR bottom (threaded M3, ~6 mm engagement)
|
||||
//
|
||||
// Architecture:
|
||||
// T-nut base → clamps to 2020 rail (standard thumbscrew interface)
|
||||
// Column → parametric-height hollow mast; USB cable routed inside
|
||||
// Platform → disc receives RPLIDAR via 4× M3 on Ø40 mm bolt circle
|
||||
// Vibe ring → anti-vibration isolation ring with silicone grommet seats
|
||||
// Cable guide → snap-on clips along column for USB cable management
|
||||
//
|
||||
// Part catalogue:
|
||||
// Part 1 — tnut_base() 2020 T-nut rail base + column stub socket
|
||||
// Part 2 — column() Hollow elevation mast (parametric ELEV_H)
|
||||
// Part 3 — scan_platform() RPLIDAR mounting disc + motor connector slot
|
||||
// Part 4 — vibe_ring() Anti-vibration isolation ring (grommet seats)
|
||||
// Part 5 — cable_guide() Snap-on cable management clip for column
|
||||
// Part 6 — assembly_preview()
|
||||
//
|
||||
// Hardware BOM (per mount):
|
||||
// 1× M3 × 16 mm SHCS + M3 hex nut rail clamp thumbscrew
|
||||
// 4× M3 × 30 mm SHCS RPLIDAR → vibe_ring → platform
|
||||
// 4× M3 silicone grommets (Ø6 mm) anti-vibration isolators
|
||||
// 4× M3 hex nuts captured in platform underside
|
||||
// 2× M4 × 12 mm SHCS column → base socket bolts
|
||||
// 2× M4 hex nuts captured in base socket
|
||||
// 1× USB-A cable (RPLIDAR → Jetson) routed through column bore
|
||||
//
|
||||
// RPLIDAR A1 interface (caliper-verified Slamtec RPLIDAR A1):
|
||||
// Body diameter : Ø70 mm
|
||||
// Bolt circle : Ø40 mm, 4× M3, at 45°/135°/225°/315°
|
||||
// USB connector : micro-USB, right-rear quadrant, exits at 0° (front)
|
||||
// Motor connector : JST 2-pin, rear centreline
|
||||
// Scan plane height : 19 mm above bolt mounting face
|
||||
// Min clearance : Ø80 mm cylinder around body for 360° scan
|
||||
//
|
||||
// Parametric constants (override for variants):
|
||||
// ELEV_H — scan elevation above rail face (default 120 mm)
|
||||
// COL_OD — column outer diameter (default 25 mm)
|
||||
// RAIL choice — RAIL_W = 20 for 2020, = 40 for 4040 extrusion
|
||||
//
|
||||
// Print settings:
|
||||
// Material : PETG (all parts); vibe_ring optionally in TPU 95A
|
||||
// Perimeters : 5 (tnut_base, column, platform), 3 (vibe_ring, cable_guide)
|
||||
// Infill : 40 % gyroid (structural), 20 % (vibe_ring, guide)
|
||||
// Orientation:
|
||||
// tnut_base — face-plate flat on bed (no supports)
|
||||
// column — standing upright (no supports; hollow bore bridgeable)
|
||||
// scan_platform — disc face down (no supports)
|
||||
// vibe_ring — flat on bed (no supports)
|
||||
// cable_guide — clip-open face down (no supports)
|
||||
//
|
||||
// Export commands:
|
||||
// openscad rplidar_mount.scad -D 'RENDER="tnut_base_stl"' -o rpm_tnut_base.stl
|
||||
// openscad rplidar_mount.scad -D 'RENDER="column_stl"' -o rpm_column.stl
|
||||
// openscad rplidar_mount.scad -D 'RENDER="platform_stl"' -o rpm_platform.stl
|
||||
// openscad rplidar_mount.scad -D 'RENDER="vibe_ring_stl"' -o rpm_vibe_ring.stl
|
||||
// openscad rplidar_mount.scad -D 'RENDER="cable_guide_stl"' -o rpm_cable_guide.stl
|
||||
// RENDER options:
|
||||
// "ring" print-ready flat ring (default)
|
||||
// "assembly" ring in position on platform stub
|
||||
// ============================================================
|
||||
|
||||
RENDER = "ring";
|
||||
|
||||
// ── RPLIDAR A1M8 ─────────────────────────────────────────────
|
||||
RPL_BODY_D = 70.0; // body diameter
|
||||
RPL_BC = 58.0; // M3 mounting bolt circle
|
||||
|
||||
// ── Mount ring ───────────────────────────────────────────────
|
||||
RING_OD = 82.0; // outer diameter (RPL_BODY_D + 12 mm)
|
||||
RING_ID = 30.0; // inner cutout (cable / airflow)
|
||||
RING_H = 4.0; // ring thickness
|
||||
|
||||
BOLT_D = 3.3; // M3 clearance through-hole
|
||||
GROMMET_D = 7.0; // silicone grommet OD (seat recess on bottom)
|
||||
GROMMET_H = 1.0; // seating recess depth
|
||||
|
||||
$fn = 64;
|
||||
e = 0.01;
|
||||
|
||||
// ── Parametric elevation ──────────────────────────────────────────────────────
|
||||
ELEV_H = 120.0; // scan plane elevation above rail face (mm)
|
||||
// increase for taller chassis; min ~60 mm recommended
|
||||
|
||||
// ── RPLIDAR A1 interface constants ───────────────────────────────────────────
|
||||
RPL_BODY_D = 70.0; // scanner body outer diameter
|
||||
RPL_BC_D = 40.0; // mounting bolt circle diameter (4× M3 at 45° offsets)
|
||||
RPL_BOLT_D = 3.3; // M3 clearance bore
|
||||
RPL_SCAN_Z = 19.0; // scan plane height above mount face
|
||||
RPL_CLEAR_D = 82.0; // minimum radial clearance diameter for 360° scan
|
||||
|
||||
// ── Rail geometry (matches sensor_rail.scad) ─────────────────────────────────
|
||||
RAIL_W = 20.0;
|
||||
SLOT_OPEN = 6.0;
|
||||
SLOT_INNER_W = 10.2;
|
||||
SLOT_INNER_H = 5.8;
|
||||
SLOT_NECK_H = 3.2;
|
||||
|
||||
// ── T-nut geometry (matches sensor_rail_brackets.scad) ───────────────────────
|
||||
TNUT_W = 9.8;
|
||||
TNUT_H = 5.5;
|
||||
TNUT_L = 12.0;
|
||||
TNUT_M3_NUT_AF = 5.5;
|
||||
TNUT_M3_NUT_H = 2.5;
|
||||
TNUT_BOLT_D = 3.3;
|
||||
|
||||
// ── Base plate geometry ───────────────────────────────────────────────────────
|
||||
BASE_FACE_W = 38.0; // wider than rail, provides column socket footprint
|
||||
BASE_FACE_H = 38.0; // height along rail Z
|
||||
BASE_FACE_T = SLOT_NECK_H + 2.0; // plate depth (Y)
|
||||
|
||||
// ── Column geometry ───────────────────────────────────────────────────────────
|
||||
COL_OD = 25.0; // column outer diameter
|
||||
COL_ID = 17.0; // column inner bore (cable routing + weight saving)
|
||||
COL_SOCKET_D = COL_OD + 6.0; // socket boss OD (column inserts into base)
|
||||
COL_SOCKET_L = 14.0; // socket depth in base (14 mm engagement)
|
||||
COL_BOLT_BC = COL_OD + 4.0; // M4 column-lock bolt span (centre-to-centre)
|
||||
COL_SLOT_W = 5.0; // cable exit slot width in column base
|
||||
COL_SLOT_H = 8.0; // cable exit slot height
|
||||
|
||||
// ── Platform geometry ─────────────────────────────────────────────────────────
|
||||
PLAT_OD = RPL_CLEAR_D + 4.0; // platform disc OD (covers scan clear zone)
|
||||
PLAT_T = 5.0; // platform disc thickness
|
||||
PLAT_SOCKET_D = COL_OD + 0.3; // column-top socket ID (slip fit)
|
||||
PLAT_SOCKET_L = 12.0; // socket depth on platform underside
|
||||
PLAT_RIM_T = 3.5; // rim wall thickness around RPLIDAR body
|
||||
|
||||
// ── Anti-vibration ring geometry ─────────────────────────────────────────────
|
||||
RING_OD = RPL_BODY_D + 12.0; // 82 mm (body + 6 mm rim)
|
||||
RING_ID = 28.0; // central bore (connector/cable access)
|
||||
RING_H = 4.0; // ring thickness
|
||||
GROMMET_D = 7.0; // silicone grommet OD pocket
|
||||
GROMMET_RECESS = 1.5; // grommet seating recess depth (bottom face)
|
||||
|
||||
// ── Cable guide clip geometry ─────────────────────────────────────────────────
|
||||
GUIDE_CABLE_D = 6.0; // max cable OD (USB-A cable)
|
||||
GUIDE_T = 2.0; // clip wall thickness
|
||||
GUIDE_BODY_W = 20.0; // clip body width
|
||||
GUIDE_BODY_H = 12.0; // clip body height
|
||||
|
||||
// ── Fastener sizes ────────────────────────────────────────────────────────────
|
||||
M3_D = 3.3;
|
||||
M4_D = 4.3;
|
||||
M3_NUT_AF = 5.5;
|
||||
M3_NUT_H = 2.4;
|
||||
M4_NUT_AF = 7.0;
|
||||
M4_NUT_H = 3.2;
|
||||
|
||||
// ============================================================
|
||||
// RENDER DISPATCH
|
||||
// ============================================================
|
||||
RENDER = "assembly";
|
||||
|
||||
if (RENDER == "assembly") assembly_preview();
|
||||
else if (RENDER == "tnut_base_stl") tnut_base();
|
||||
else if (RENDER == "column_stl") column();
|
||||
else if (RENDER == "platform_stl") scan_platform();
|
||||
else if (RENDER == "vibe_ring_stl") vibe_ring();
|
||||
else if (RENDER == "cable_guide_stl") cable_guide();
|
||||
|
||||
// ============================================================
|
||||
// ASSEMBLY PREVIEW
|
||||
// ============================================================
|
||||
module assembly_preview() {
|
||||
// Ghost 2020 rail section (250 mm)
|
||||
%color("Silver", 0.28)
|
||||
translate([-RAIL_W/2, -RAIL_W/2, 0])
|
||||
cube([RAIL_W, RAIL_W, 250]);
|
||||
|
||||
// T-nut base at Z=60 on rail
|
||||
color("OliveDrab", 0.85)
|
||||
translate([0, 0, 60])
|
||||
tnut_base();
|
||||
|
||||
// Column rising from base
|
||||
color("SteelBlue", 0.85)
|
||||
translate([0, BASE_FACE_T + COL_OD/2, 60 + BASE_FACE_H/2])
|
||||
column();
|
||||
|
||||
// Vibe ring on top of platform
|
||||
color("Teal", 0.85)
|
||||
translate([0, BASE_FACE_T + COL_OD/2,
|
||||
60 + BASE_FACE_H/2 + ELEV_H + PLAT_T])
|
||||
vibe_ring();
|
||||
|
||||
// Scan platform at column top
|
||||
color("DarkSlateGray", 0.85)
|
||||
translate([0, BASE_FACE_T + COL_OD/2,
|
||||
60 + BASE_FACE_H/2 + ELEV_H])
|
||||
scan_platform();
|
||||
|
||||
// RPLIDAR body ghost
|
||||
%color("Black", 0.35)
|
||||
translate([0, BASE_FACE_T + COL_OD/2,
|
||||
60 + BASE_FACE_H/2 + ELEV_H + PLAT_T + RING_H + 1])
|
||||
cylinder(d = RPL_BODY_D, h = 30);
|
||||
|
||||
// Cable guides at 30 mm intervals along column
|
||||
for (gz = [20, 50, 80])
|
||||
color("DimGray", 0.75)
|
||||
translate([COL_OD/2,
|
||||
BASE_FACE_T + COL_OD/2,
|
||||
60 + BASE_FACE_H/2 + gz])
|
||||
rotate([0, -90, 0])
|
||||
cable_guide();
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 1 — T-NUT RAIL BASE
|
||||
// ============================================================
|
||||
// Standard 2020 rail T-nut attachment, matching interface used across
|
||||
// all SaltyLab sensor brackets (sensor_rail_brackets.scad convention).
|
||||
// Column socket boss on front face (+Y) receives column bottom.
|
||||
// Column locked with 2× M4 cross-bolts through socket boss.
|
||||
//
|
||||
// Cable exit slot at base of socket directs RPLIDAR USB cable
|
||||
// downward and rearward toward Jetson USB port.
|
||||
//
|
||||
// Print: face-plate flat on bed, PETG, 5 perims, 50 % gyroid.
|
||||
module tnut_base() {
|
||||
// ─────────────────────────────────────────────────────────────
|
||||
module rplidar_ring() {
|
||||
difference() {
|
||||
union() {
|
||||
// ── Face plate (flush against rail outer face, -Y) ───────────
|
||||
translate([-BASE_FACE_W/2, -BASE_FACE_T, 0])
|
||||
cube([BASE_FACE_W, BASE_FACE_T, BASE_FACE_H]);
|
||||
|
||||
// ── T-nut neck (enters rail slot) ────────────────────────────
|
||||
translate([-TNUT_W/2, 0, (BASE_FACE_H - TNUT_L)/2])
|
||||
cube([TNUT_W, SLOT_NECK_H + e, TNUT_L]);
|
||||
|
||||
// ── T-nut body (wider, locks in T-groove) ────────────────────
|
||||
translate([-TNUT_W/2, SLOT_NECK_H - e, (BASE_FACE_H - TNUT_L)/2])
|
||||
cube([TNUT_W, TNUT_H - SLOT_NECK_H + e, TNUT_L]);
|
||||
|
||||
// ── Column socket boss (front face, centred) ─────────────────
|
||||
translate([0, -BASE_FACE_T, BASE_FACE_H/2])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = COL_SOCKET_D, h = BASE_FACE_T + COL_SOCKET_L);
|
||||
}
|
||||
|
||||
// ── Rail clamp bolt bore (M3, centre of face plate) ──────────────
|
||||
translate([0, -BASE_FACE_T - e, BASE_FACE_H/2])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = TNUT_BOLT_D, h = BASE_FACE_T + TNUT_H + 2*e);
|
||||
|
||||
// ── M3 hex nut pocket (inside T-nut body) ────────────────────────
|
||||
translate([0, SLOT_NECK_H + 0.3, BASE_FACE_H/2])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = TNUT_M3_NUT_AF / cos(30),
|
||||
h = TNUT_M3_NUT_H + 0.3, $fn = 6);
|
||||
|
||||
// ── Column socket bore (column inserts from +Y side) ─────────────
|
||||
translate([0, -BASE_FACE_T, BASE_FACE_H/2])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = COL_OD + 0.3, h = BASE_FACE_T + COL_SOCKET_L + e);
|
||||
|
||||
// ── Column lock bolt bores (2× M4, horizontal through socket boss) ─
|
||||
// One bolt from +X, one from -X, on COL_SOCKET_L/2 depth
|
||||
for (lx = [-1, 1])
|
||||
translate([lx * (COL_SOCKET_D/2 + e), COL_SOCKET_L/2, BASE_FACE_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = M4_D, h = COL_SOCKET_D + 2*e,
|
||||
center = true);
|
||||
|
||||
// ── M4 nut pockets (one side of socket boss for each bolt) ────────
|
||||
for (lx = [-1, 1])
|
||||
translate([lx * (COL_SOCKET_D/2 - M4_NUT_H - 1),
|
||||
COL_SOCKET_L/2,
|
||||
BASE_FACE_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = M4_NUT_AF / cos(30),
|
||||
h = M4_NUT_H + 0.5, $fn = 6);
|
||||
|
||||
// ── Cable exit slot (bottom of socket, cable exits downward) ──────
|
||||
translate([0, COL_SOCKET_L * 0.6, BASE_FACE_H/2 - COL_SOCKET_D/2])
|
||||
cube([COL_SLOT_W, COL_SOCKET_D + e, COL_SLOT_H], center = [true, false, false]);
|
||||
|
||||
// ── Lightening pockets in face plate ─────────────────────────────
|
||||
translate([0, -BASE_FACE_T/2, BASE_FACE_H/2])
|
||||
cube([BASE_FACE_W - 12, BASE_FACE_T - 2, BASE_FACE_H - 16],
|
||||
center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 2 — ELEVATION COLUMN
|
||||
// ============================================================
|
||||
// Hollow cylindrical mast (ELEV_H tall) raising the RPLIDAR scan
|
||||
// plane above the chassis body for unobstructed 360° coverage.
|
||||
// Inner bore routes USB cable from scanner to base exit slot.
|
||||
// Bottom peg inserts into tnut_base socket; top peg inserts into
|
||||
// scan_platform socket. Both ends are plain Ø(COL_OD) cylinders,
|
||||
// interference-free slip fit into Ø(COL_OD+0.3) sockets.
|
||||
//
|
||||
// Three longitudinal ribs on outer surface add torsional stiffness
|
||||
// without added diameter. Cable slot on one rib for cable retention.
|
||||
//
|
||||
// Print: standing upright, PETG, 5 perims, 20 % gyroid (hollow).
|
||||
module column() {
|
||||
rib_w = 3.0;
|
||||
rib_h = 2.0; // rib protrusion from column OD
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── Hollow cylinder ───────────────────────────────────────────
|
||||
cylinder(d = COL_OD, h = ELEV_H + COL_SOCKET_L);
|
||||
|
||||
// ── Three stiffening ribs (120° apart) ────────────────────────
|
||||
for (ra = [0, 120, 240])
|
||||
rotate([0, 0, ra])
|
||||
translate([COL_OD/2 - e, -rib_w/2, 0])
|
||||
cube([rib_h + e, rib_w, ELEV_H + COL_SOCKET_L]);
|
||||
}
|
||||
|
||||
// ── Central cable bore (full length) ─────────────────────────────
|
||||
translate([0, 0, -e])
|
||||
cylinder(d = COL_ID, h = ELEV_H + COL_SOCKET_L + 2*e);
|
||||
|
||||
// ── Cable entry slot at column base (aligns with base exit slot) ──
|
||||
translate([-COL_SLOT_W/2, COL_OD/2 - e, -e])
|
||||
cube([COL_SLOT_W, COL_ID/2 + rib_h + 2, COL_SLOT_H + 2]);
|
||||
|
||||
// ── Cable exit slot at column top (USB exits to scanner) ──────────
|
||||
translate([-COL_SLOT_W/2, COL_OD/2 - e,
|
||||
ELEV_H + COL_SOCKET_L - COL_SLOT_H - 2])
|
||||
cube([COL_SLOT_W, COL_ID/2 + rib_h + 2, COL_SLOT_H + 2]);
|
||||
|
||||
// ── Column lock flat (prevents rotation in socket) ────────────────
|
||||
// Two opposed flats at column base & top socket peg
|
||||
for (peg_z = [0, ELEV_H]) {
|
||||
translate([-COL_OD/2 - e, COL_OD/2 - 2.0, peg_z])
|
||||
cube([COL_OD + 2*e, 2.5, COL_SOCKET_L]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 3 — SCAN PLATFORM
|
||||
// ============================================================
|
||||
// Disc that RPLIDAR A1 mounts to. Matches RPLIDAR A1 bolt pattern:
|
||||
// 4× M3 on Ø40 mm bolt circle at 45°/135°/225°/315°.
|
||||
// M3 hex nuts captured in underside pockets (blind, tool-free install).
|
||||
// Column-top socket on underside receives column top peg (Ø25 slip fit).
|
||||
// Motor connector slot on rear edge for JST cable exit.
|
||||
// Vibe ring sits on top face between platform and RPLIDAR (separate part).
|
||||
//
|
||||
// Scan plane (19 mm above mount face) clears platform top by design;
|
||||
// minimum platform OD = RPL_CLEAR_D (82 mm) leaves scan plane open.
|
||||
//
|
||||
// Print: disc face down, PETG, 5 perims, 40 % gyroid.
|
||||
module scan_platform() {
|
||||
difference() {
|
||||
union() {
|
||||
// ── Platform disc ─────────────────────────────────────────────
|
||||
cylinder(d = PLAT_OD, h = PLAT_T);
|
||||
|
||||
// ── Column socket boss (underside, -Z) ────────────────────────
|
||||
translate([0, 0, -PLAT_SOCKET_L])
|
||||
cylinder(d = COL_SOCKET_D, h = PLAT_SOCKET_L + e);
|
||||
}
|
||||
|
||||
// ── Column socket bore (column top peg inserts from below) ────────
|
||||
translate([0, 0, -PLAT_SOCKET_L - e])
|
||||
cylinder(d = PLAT_SOCKET_D, h = PLAT_SOCKET_L + e + 1);
|
||||
|
||||
// ── Column lock bores (2× M4 through socket boss) ─────────────────
|
||||
for (lx = [-1, 1])
|
||||
translate([lx * (COL_SOCKET_D/2 + e), 0, -PLAT_SOCKET_L/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = M4_D, h = COL_SOCKET_D + 2*e, center = true);
|
||||
|
||||
// ── M4 nut pockets (one side socket boss) ─────────────────────────
|
||||
translate([COL_SOCKET_D/2 - M4_NUT_H - 1, 0, -PLAT_SOCKET_L/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = M4_NUT_AF / cos(30), h = M4_NUT_H + 0.5,
|
||||
$fn = 6);
|
||||
|
||||
// ── 4× RPLIDAR mounting bolt holes (M3, Ø40 mm BC at 45°) ────────
|
||||
for (a = [45, 135, 225, 315])
|
||||
translate([RPL_BC_D/2 * cos(a),
|
||||
RPL_BC_D/2 * sin(a), -e])
|
||||
cylinder(d = RPL_BOLT_D, h = PLAT_T + 2*e);
|
||||
|
||||
// ── M3 hex nut pockets on underside (captured, tool-free) ─────────
|
||||
for (a = [45, 135, 225, 315])
|
||||
translate([RPL_BC_D/2 * cos(a),
|
||||
RPL_BC_D/2 * sin(a), -e])
|
||||
cylinder(d = M3_NUT_AF / cos(30),
|
||||
h = M3_NUT_H + 0.5, $fn = 6);
|
||||
|
||||
// ── Motor connector slot (JST rear centreline, 10×6 mm) ──────────
|
||||
translate([0, PLAT_OD/2 - 8, -e])
|
||||
cube([10, 10, PLAT_T + 2*e], center = [true, false, false]);
|
||||
|
||||
// ── USB connector slot (micro-USB, right-rear, 12×6 mm) ──────────
|
||||
translate([PLAT_OD/4, PLAT_OD/2 - 8, -e])
|
||||
cube([12, 10, PLAT_T + 2*e], center = [true, false, false]);
|
||||
|
||||
// ── Lightening pockets (between bolt holes) ────────────────────────
|
||||
for (a = [0, 90, 180, 270])
|
||||
translate([(RPL_BC_D/2 + 10) * cos(a),
|
||||
(RPL_BC_D/2 + 10) * sin(a), -e])
|
||||
cylinder(d = 8, h = PLAT_T + 2*e);
|
||||
|
||||
// ── Central cable bore (USB from scanner routes down column) ──────
|
||||
translate([0, 0, -e])
|
||||
cylinder(d = COL_ID - 2, h = PLAT_T + 2*e);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 4 — VIBRATION ISOLATION RING
|
||||
// ============================================================
|
||||
// Flat ring sits between scan_platform top face and RPLIDAR bottom.
|
||||
// Anti-vibration isolation via 4× M3 silicone FC-style grommets
|
||||
// (Ø6 mm silicone, M3 bore — same type used on flight controllers).
|
||||
//
|
||||
// Bolt stack (bottom → top):
|
||||
// M3 × 30 SHCS → platform (countersunk) → grommet (Ø7 seat) →
|
||||
// ring (4 mm) → RPLIDAR threaded boss (~6 mm engagement)
|
||||
//
|
||||
// Grommet seats are recessed 1.5 mm into ring bottom face so grommets
|
||||
// are captured and self-locating. Ring top face is flat for RPLIDAR.
|
||||
//
|
||||
// Print: flat on bed, PETG or TPU 95A, 3 perims, 20 % infill.
|
||||
// TPU 95A provides additional compliance in axial direction.
|
||||
module vibe_ring() {
|
||||
difference() {
|
||||
// ── Ring body ────────────────────────────────────────────────────
|
||||
cylinder(d = RING_OD, h = RING_H);
|
||||
|
||||
// ── Central bore (cable / connector access) ───────────────────────
|
||||
// Central cutout
|
||||
translate([0, 0, -e])
|
||||
cylinder(d = RING_ID, h = RING_H + 2*e);
|
||||
|
||||
// ── 4× M3 clearance bores on Ø40 mm bolt circle ───────────────────
|
||||
for (a = [45, 135, 225, 315])
|
||||
translate([RPL_BC_D/2 * cos(a),
|
||||
RPL_BC_D/2 * sin(a), -e])
|
||||
cylinder(d = RPL_BOLT_D, h = RING_H + 2*e);
|
||||
|
||||
// ── Grommet seating recesses (bottom face, Ø7 mm × 1.5 mm deep) ──
|
||||
for (a = [45, 135, 225, 315])
|
||||
translate([RPL_BC_D/2 * cos(a),
|
||||
RPL_BC_D/2 * sin(a), -e])
|
||||
cylinder(d = GROMMET_D, h = GROMMET_RECESS + e);
|
||||
|
||||
// ── Motor connector notch (rear centreline, passes through ring) ──
|
||||
translate([0, RING_OD/2 - 6, -e])
|
||||
cube([10, 8, RING_H + 2*e], center = [true, false, false]);
|
||||
|
||||
// ── Lightening arcs ───────────────────────────────────────────────
|
||||
for (a = [0, 90, 180, 270])
|
||||
translate([(RPL_BC_D/2 + 9) * cos(a),
|
||||
(RPL_BC_D/2 + 9) * sin(a), -e])
|
||||
cylinder(d = 7, h = RING_H + 2*e);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 5 — CABLE GUIDE CLIP
|
||||
// ============================================================
|
||||
// Snap-on C-clip that presses onto column ribs to retain USB cable
|
||||
// along column exterior. Cable sits in a semicircular channel;
|
||||
// snap tongue grips the rib. No fasteners — push-fit on rib.
|
||||
// Print multiples: one every ~30 mm along column for clean routing.
|
||||
//
|
||||
// Print: clip-opening face down, PETG, 3 perims, 20 % infill.
|
||||
// Orientation matters — clip opening (-Y face) must face down for bridging.
|
||||
module cable_guide() {
|
||||
snap_t = 1.8; // snap tongue thickness (springy PETG)
|
||||
snap_oc = GUIDE_CABLE_D + 2*GUIDE_T; // channel outer cylinder OD
|
||||
body_h = GUIDE_BODY_H;
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── Clip body (flat plate on column face) ─────────────────────
|
||||
translate([-GUIDE_BODY_W/2, 0, 0])
|
||||
cube([GUIDE_BODY_W, GUIDE_T, body_h]);
|
||||
|
||||
// ── Cable channel (C-shape, opens toward +Y) ──────────────────
|
||||
translate([0, GUIDE_T + snap_oc/2, body_h/2])
|
||||
rotate([0, 90, 0])
|
||||
difference() {
|
||||
cylinder(d = snap_oc, h = GUIDE_BODY_W,
|
||||
center = true);
|
||||
cylinder(d = GUIDE_CABLE_D, h = GUIDE_BODY_W + 2*e,
|
||||
center = true);
|
||||
// Open front slot for cable insertion
|
||||
translate([0, snap_oc/2 + e, 0])
|
||||
cube([GUIDE_CABLE_D * 0.85,
|
||||
snap_oc + 2*e,
|
||||
GUIDE_BODY_W + 2*e], center = true);
|
||||
}
|
||||
|
||||
// ── Snap-fit tongue (grips column rib, -Y side of body) ───────
|
||||
// Two flexible tabs that straddle column rib
|
||||
for (tx = [-GUIDE_BODY_W/2 + 2, GUIDE_BODY_W/2 - 2 - snap_t])
|
||||
translate([tx, -4, 0])
|
||||
cube([snap_t, 4 + GUIDE_T, body_h]);
|
||||
|
||||
// Snap barbs (slight overhang engages rib back edge)
|
||||
for (tx = [-GUIDE_BODY_W/2 + 2, GUIDE_BODY_W/2 - 2 - snap_t])
|
||||
translate([tx + snap_t/2, -4, body_h/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = 2, h = snap_t, center = true);
|
||||
// 4× M3 clearance holes on bolt circle
|
||||
for (a = [45, 135, 225, 315]) {
|
||||
translate([RPL_BC/2 * cos(a), RPL_BC/2 * sin(a), -e])
|
||||
cylinder(d = BOLT_D, h = RING_H + 2*e);
|
||||
}
|
||||
|
||||
// ── Rib slot (column rib passes through clip body) ─────────────────
|
||||
translate([0, -2, body_h/2])
|
||||
cube([3.5, GUIDE_T + 4 + e, body_h - 4], center = true);
|
||||
// Grommet seating recesses — bottom face
|
||||
for (a = [45, 135, 225, 315]) {
|
||||
translate([RPL_BC/2 * cos(a), RPL_BC/2 * sin(a), -e])
|
||||
cylinder(d = GROMMET_D, h = GROMMET_H + e);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────────
|
||||
// Render selector
|
||||
// ─────────────────────────────────────────────────────────────
|
||||
if (RENDER == "ring") {
|
||||
rplidar_ring();
|
||||
|
||||
} else if (RENDER == "assembly") {
|
||||
// Platform stub
|
||||
color("Silver", 0.5)
|
||||
difference() {
|
||||
cylinder(d = 90, h = 8);
|
||||
translate([0, 0, -e]) cylinder(d = 25.4, h = 8 + 2*e);
|
||||
}
|
||||
// Ring floating 8 mm above (grommet gap)
|
||||
color("SkyBlue", 0.9)
|
||||
translate([0, 0, 8 + 8])
|
||||
rplidar_ring();
|
||||
}
|
||||
|
||||
@ -1,463 +1,275 @@
|
||||
// ============================================================
|
||||
// uwb_anchor_mount.scad — Wall/Ceiling UWB Anchor Mount Bracket
|
||||
// Issue: #564 Agent: sl-mechanical Date: 2026-03-14
|
||||
// (supersedes Rev A stem-collar mount — see git history)
|
||||
// uwb_anchor_mount.scad — Stem-Mounted UWB Anchor Rev A
|
||||
// Agent: sl-mechanical 2026-03-01
|
||||
// Closes issues #57, #62
|
||||
// ============================================================
|
||||
// Clamp-on bracket for 2× MaUWB ESP32-S3 anchor modules on
|
||||
// SaltyBot 25 mm OD vertical stem.
|
||||
// Anchors spaced ANCHOR_SPACING = 250 mm apart.
|
||||
//
|
||||
// Parametric wall or ceiling mount bracket for ESP32 UWB Pro anchor.
|
||||
// Designed for fixed-infrastructure deployment: anchors screw into
|
||||
// wall or ceiling drywall/timber with standard M4 or #6 wood screws,
|
||||
// at a user-defined tilt angle so the UWB antenna faces the desired
|
||||
// coverage zone.
|
||||
// Features:
|
||||
// • Split D-collar with M4 clamping bolts + M4 set screw
|
||||
// • Anti-rotation flat tab that keys against a small pin
|
||||
// OR printed key tab that registers on the stem flat (if stem
|
||||
// has a ground flat) — see ANTI_ROT_MODE parameter
|
||||
// • Module bracket: faces outward, tilted 10° from vertical
|
||||
// so antenna clears stem and faces horizon
|
||||
// • USB cable channel (power from Orin via USB-A) on collar
|
||||
// • Tool-free capture: M4 thumbscrews (slot-head, hand-tighten)
|
||||
// • UWB antenna area: NO material within 10 mm of PCB top face
|
||||
//
|
||||
// Architecture:
|
||||
// Wall base → flat backplate with 2× screw holes (wall or ceiling)
|
||||
// Tilt knuckle → single-axis articulating joint; 15° detent steps
|
||||
// locked with M3 nyloc bolt; range 0–90°
|
||||
// Anchor cradle→ U-cradle holding ESP32 UWB Pro PCB on M2.5 standoffs
|
||||
// USB-C channel→ routed groove on tilt arm + exit slot in cradle back wall
|
||||
// Label slot → rear window slot for printed anchor-ID card strip
|
||||
//
|
||||
// Part catalogue:
|
||||
// Part 1 — wall_base() Backplate + 2-ear pivot block + detent arc
|
||||
// Part 2 — tilt_arm() Pivoting arm with knuckle + cradle stub
|
||||
// Part 3 — anchor_cradle() PCB cradle, standoffs, USB-C slot, label window
|
||||
// Part 4 — cable_clip() Snap-on USB-C cable guide for tilt arm
|
||||
// Part 5 — assembly_preview()
|
||||
//
|
||||
// Hardware BOM:
|
||||
// 2× M4 × 30 mm wood screws (or #6 drywall screws) wall fasteners
|
||||
// 1× M3 × 20 mm SHCS + M3 nyloc nut tilt pivot bolt
|
||||
// 4× M2.5 × 8 mm SHCS PCB-to-cradle
|
||||
// 4× M2.5 hex nuts captured in standoffs
|
||||
// 1× USB-C cable anchor power
|
||||
//
|
||||
// ESP32 UWB Pro interface (verify with calipers):
|
||||
// PCB size : UWB_L × UWB_W × UWB_H (55 × 28 × 10 mm default)
|
||||
// Mounting holes : M2.5, 4× corners on UWB_HOLE_X × UWB_HOLE_Y pattern
|
||||
// USB-C port : centred on short edge, UWB_USBC_W × UWB_USBC_H
|
||||
// Antenna area : top face rear half — 10 mm keep-out of bracket material
|
||||
//
|
||||
// Tilt angles (15° detent steps, set TILT_DEG before export):
|
||||
// 0° → horizontal face-up (ceiling, antenna faces down)
|
||||
// 30° → 30° downward (wall near ceiling) [default]
|
||||
// 45° → diagonal (wall mid-height)
|
||||
// 90° → vertical face-out (wall, antenna faces forward)
|
||||
// Components per mount:
|
||||
// 2× collar_half print in PLA/PETG, flat-face-down
|
||||
// 1× module_bracket print in PLA/PETG, flat-face-down
|
||||
//
|
||||
// RENDER options:
|
||||
// "assembly" full assembly at TILT_DEG (default)
|
||||
// "wall_base_stl" Part 1
|
||||
// "tilt_arm_stl" Part 2
|
||||
// "anchor_cradle_stl" Part 3
|
||||
// "cable_clip_stl" Part 4
|
||||
//
|
||||
// Export commands:
|
||||
// openscad uwb_anchor_mount.scad -D 'RENDER="wall_base_stl"' -o uwb_wall_base.stl
|
||||
// openscad uwb_anchor_mount.scad -D 'RENDER="tilt_arm_stl"' -o uwb_tilt_arm.stl
|
||||
// openscad uwb_anchor_mount.scad -D 'RENDER="anchor_cradle_stl"' -o uwb_anchor_cradle.stl
|
||||
// openscad uwb_anchor_mount.scad -D 'RENDER="cable_clip_stl"' -o uwb_cable_clip.stl
|
||||
// "assembly" single mount assembled (default)
|
||||
// "collar_front" front collar half for slicing (×2 per mount × 2 mounts = 4)
|
||||
// "collar_rear" rear collar half
|
||||
// "bracket" module bracket (×2 mounts)
|
||||
// "pair" both mounts on 350 mm stem section
|
||||
// ============================================================
|
||||
|
||||
$fn = 64;
|
||||
e = 0.01;
|
||||
|
||||
// ── Tilt angle (override per anchor, 0–90°, 15° steps) ───────────────────────
|
||||
TILT_DEG = 30;
|
||||
|
||||
// ── ESP32 UWB Pro PCB dimensions (verify with calipers) ──────────────────────
|
||||
UWB_L = 55.0; // PCB length
|
||||
UWB_W = 28.0; // PCB width
|
||||
UWB_H = 10.0; // PCB + components height
|
||||
UWB_HOLE_X = 47.5; // M2.5 hole X span
|
||||
UWB_HOLE_Y = 21.0; // M2.5 hole Y span
|
||||
UWB_USBC_W = 9.5; // USB-C receptacle width
|
||||
UWB_USBC_H = 4.0; // USB-C receptacle height
|
||||
UWB_ANTENNA_L = 20.0; // antenna area at PCB rear (keep-out)
|
||||
|
||||
// ── Wall base geometry ────────────────────────────────────────────────────────
|
||||
BASE_W = 60.0;
|
||||
BASE_H = 50.0;
|
||||
BASE_T = 5.0;
|
||||
BASE_SCREW_D = 4.5; // M4 clearance
|
||||
BASE_SCREW_HD = 8.5; // countersink OD
|
||||
BASE_SCREW_HH = 3.5; // countersink depth
|
||||
BASE_SCREW_SPC = 35.0; // Z span between screw holes
|
||||
KNUCKLE_T = BASE_T + 4.0; // pivot ear depth (Y)
|
||||
|
||||
// ── Tilt arm geometry ─────────────────────────────────────────────────────────
|
||||
ARM_W = 12.0;
|
||||
ARM_T = 5.0;
|
||||
ARM_L = 35.0;
|
||||
PIVOT_D = 3.3; // M3 clearance
|
||||
PIVOT_NUT_AF = 5.5;
|
||||
PIVOT_NUT_H = 2.4;
|
||||
DETENT_D = 3.2; // detent notch diameter
|
||||
DETENT_R = 8.0; // detent notch radius from pivot
|
||||
|
||||
// ── Anchor cradle geometry ────────────────────────────────────────────────────
|
||||
CRADLE_WALL_T = 3.5;
|
||||
CRADLE_BACK_T = 4.0;
|
||||
CRADLE_FLOOR_T = 3.0;
|
||||
CRADLE_LIP_H = 4.0;
|
||||
CRADLE_LIP_T = 2.5;
|
||||
STANDOFF_H = 3.0;
|
||||
STANDOFF_OD = 5.5;
|
||||
LABEL_W = UWB_L - 4.0;
|
||||
LABEL_H = UWB_W * 0.55;
|
||||
LABEL_T = 1.2; // label card thickness
|
||||
|
||||
// ── USB-C cable routing ───────────────────────────────────────────────────────
|
||||
USBC_CHAN_W = 11.0;
|
||||
USBC_CHAN_H = 7.0;
|
||||
|
||||
// ── Cable clip ────────────────────────────────────────────────────────────────
|
||||
CLIP_CABLE_D = 4.5;
|
||||
CLIP_T = 2.0;
|
||||
CLIP_BODY_W = 16.0;
|
||||
CLIP_BODY_H = 10.0;
|
||||
|
||||
// ── Fasteners ─────────────────────────────────────────────────────────────────
|
||||
M2P5_D = 2.7;
|
||||
M3_D = 3.3;
|
||||
M3_NUT_AF = 5.5;
|
||||
M3_NUT_H = 2.4;
|
||||
|
||||
// ============================================================
|
||||
// RENDER DISPATCH
|
||||
// ============================================================
|
||||
RENDER = "assembly";
|
||||
|
||||
if (RENDER == "assembly") assembly_preview();
|
||||
else if (RENDER == "wall_base_stl") wall_base();
|
||||
else if (RENDER == "tilt_arm_stl") tilt_arm();
|
||||
else if (RENDER == "anchor_cradle_stl") anchor_cradle();
|
||||
else if (RENDER == "cable_clip_stl") cable_clip();
|
||||
// ── ⚠ Verify with calipers ───────────────────────────────────
|
||||
MAWB_L = 50.0; // PCB length
|
||||
MAWB_W = 25.0; // PCB width
|
||||
MAWB_H = 10.0; // PCB + components
|
||||
MAWB_HOLE_X = 43.0; // M2 mounting hole X span
|
||||
MAWB_HOLE_Y = 20.0; // M2 mounting hole Y span
|
||||
M2_D = 2.2; // M2 clearance
|
||||
|
||||
// ============================================================
|
||||
// ASSEMBLY PREVIEW
|
||||
// ============================================================
|
||||
module assembly_preview() {
|
||||
// Ghost wall surface
|
||||
%color("Wheat", 0.22)
|
||||
translate([-BASE_W/2, -10, -BASE_H/2])
|
||||
cube([BASE_W, 10, BASE_H + 40]);
|
||||
// ── Stem ─────────────────────────────────────────────────────
|
||||
STEM_OD = 25.0;
|
||||
STEM_BORE = 25.4; // +0.4 clearance
|
||||
WALL = 2.0; // wall thickness (used in thumbscrew recess)
|
||||
|
||||
// Wall base
|
||||
color("OliveDrab", 0.85)
|
||||
wall_base();
|
||||
// ── Collar ───────────────────────────────────────────────────
|
||||
COL_OD = 52.0;
|
||||
COL_H = 30.0; // taller than sensor-head collar for rigidity
|
||||
COL_BOLT_X = 19.0; // M4 bolt CL from stem axis
|
||||
COL_BOLT_D = 4.5; // M4 clearance
|
||||
THUMB_HEAD_D= 8.0; // M4 thumbscrew head OD (slot for access)
|
||||
COL_NUT_W = 7.0; // M4 hex nut A/F
|
||||
COL_NUT_H = 3.4;
|
||||
|
||||
// Tilt arm at TILT_DEG, pivoting at knuckle
|
||||
color("SteelBlue", 0.85)
|
||||
translate([0, KNUCKLE_T, 0])
|
||||
rotate([TILT_DEG, 0, 0])
|
||||
tilt_arm();
|
||||
// Anti-rotation flat tab: a 3 mm wall tab that protrudes radially
|
||||
// and bears against the bracket arm, preventing axial rotation
|
||||
// without needing a stem flat.
|
||||
ANTI_ROT_T = 3.0; // tab thickness (radial)
|
||||
ANTI_ROT_W = 8.0; // tab width (tangential)
|
||||
ANTI_ROT_Z = 4.0; // distance from collar base
|
||||
|
||||
// Anchor cradle at arm end
|
||||
color("DarkSlateGray", 0.85)
|
||||
translate([0, KNUCKLE_T, 0])
|
||||
rotate([TILT_DEG, 0, 0])
|
||||
translate([0, ARM_T, ARM_L])
|
||||
anchor_cradle();
|
||||
// USB cable channel: groove on collar outer surface, runs Z direction
|
||||
// Cable routes from anchor module down to base
|
||||
USB_CHAN_W = 9.0; // channel width (fits USB-A cable Ø6 mm)
|
||||
USB_CHAN_D = 5.0; // channel depth
|
||||
|
||||
// PCB ghost
|
||||
%color("ForestGreen", 0.38)
|
||||
translate([0, KNUCKLE_T, 0])
|
||||
rotate([TILT_DEG, 0, 0])
|
||||
translate([-UWB_L/2,
|
||||
ARM_T + CRADLE_BACK_T,
|
||||
ARM_L + CRADLE_FLOOR_T + STANDOFF_H])
|
||||
cube([UWB_L, UWB_W, UWB_H]);
|
||||
// ── Module bracket ───────────────────────────────────────────
|
||||
ARM_L = 20.0; // arm length from collar OD to bracket face
|
||||
ARM_W = MAWB_W + 6.0; // bracket width (Y, includes side walls)
|
||||
ARM_H = 6.0; // arm thickness (Z)
|
||||
BRKT_TILT = 10.0; // tilt outward from vertical (antenna faces horizon)
|
||||
|
||||
// Cable clip at arm mid-point
|
||||
color("DimGray", 0.70)
|
||||
translate([ARM_W/2, KNUCKLE_T, 0])
|
||||
rotate([TILT_DEG, 0, 0])
|
||||
translate([0, ARM_T + e, ARM_L/2])
|
||||
rotate([0, -90, 90])
|
||||
cable_clip();
|
||||
}
|
||||
BRKT_BACK_T = 3.0; // bracket back wall (module sits against this)
|
||||
BRKT_SIDE_T = 2.0; // bracket side walls
|
||||
|
||||
// ============================================================
|
||||
// PART 1 — WALL BASE
|
||||
// ============================================================
|
||||
// Flat backplate screws to wall or ceiling with 2× countersunk
|
||||
// M4/#6 wood screws on BASE_SCREW_SPC (35 mm) centres.
|
||||
// Two pivot ears straddle the tilt arm; M3 pivot bolt passes through
|
||||
// both ears and arm knuckle.
|
||||
// Detent arc on inner face of +X ear: 7 notches at 15° steps (0–90°)
|
||||
// so tilt can be set without a protractor.
|
||||
// Shallow rear recess accepts a printed installation-zone label.
|
||||
//
|
||||
// Dual-use: flat face to wall (vertical screw axis) or flat face
|
||||
// to ceiling (horizontal screw axis) — same part either way.
|
||||
//
|
||||
// Print: backplate flat on bed, PETG, 5 perims, 40 % gyroid.
|
||||
module wall_base() {
|
||||
ear_h = ARM_W + 3.0;
|
||||
ear_t = 6.0;
|
||||
ear_sep = ARM_W + 1.0;
|
||||
M2_STNDFF = 3.0; // M2 standoff height
|
||||
M2_STNDFF_OD= 4.5;
|
||||
|
||||
// USB port access notch in bracket side wall (8×5 mm)
|
||||
USB_NOTCH_W = 10.0;
|
||||
USB_NOTCH_H = 7.0;
|
||||
|
||||
// ── Spacing ───────────────────────────────────────────────────
|
||||
ANCHOR_SPACING = 250.0; // centre-to-centre Z separation
|
||||
|
||||
$fn = 64;
|
||||
e = 0.01;
|
||||
|
||||
// ─────────────────────────────────────────────────────────────
|
||||
// collar_half(side)
|
||||
// split at Y=0 plane. Bracket arm on front (+Y) half.
|
||||
// Print flat-face-down.
|
||||
// ─────────────────────────────────────────────────────────────
|
||||
module collar_half(side = "front") {
|
||||
y_front = (side == "front");
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── Backplate ────────────────────────────────────────────────
|
||||
translate([-BASE_W/2, -BASE_T, -BASE_H/2])
|
||||
cube([BASE_W, BASE_T, BASE_H]);
|
||||
// D-shaped body
|
||||
intersection() {
|
||||
cylinder(d=COL_OD, h=COL_H);
|
||||
translate([-COL_OD/2, y_front ? 0 : -COL_OD/2, 0])
|
||||
cube([COL_OD, COL_OD/2, COL_H]);
|
||||
}
|
||||
|
||||
// ── Two pivot ears ────────────────────────────────────────────
|
||||
for (ex = [-(ear_sep/2 + ear_t), ear_sep/2])
|
||||
translate([ex, -BASE_T + e, -ear_h/2])
|
||||
cube([ear_t, KNUCKLE_T + e, ear_h]);
|
||||
// Anti-rotation tab (front half only, at +X side)
|
||||
if (y_front) {
|
||||
translate([COL_OD/2, -ANTI_ROT_W/2, ANTI_ROT_Z])
|
||||
cube([ANTI_ROT_T, ANTI_ROT_W,
|
||||
COL_H - ANTI_ROT_Z - 4]);
|
||||
}
|
||||
|
||||
// ── Stiffening gussets ────────────────────────────────────────
|
||||
for (ex = [-(ear_sep/2 + ear_t), ear_sep/2])
|
||||
hull() {
|
||||
translate([ex, -BASE_T, -ear_h/4])
|
||||
cube([ear_t, BASE_T - 1, ear_h/2]);
|
||||
translate([ex + (ex < 0 ? ear_t*0.6 : 0),
|
||||
-BASE_T, -ear_h/6])
|
||||
cube([ear_t * 0.4, 1, ear_h/3]);
|
||||
}
|
||||
// Bracket arm attachment boss (front half only, top centre)
|
||||
if (y_front) {
|
||||
translate([-ARM_W/2, COL_OD/2, COL_H * 0.3])
|
||||
cube([ARM_W, ARM_L, COL_H * 0.4]);
|
||||
}
|
||||
}
|
||||
|
||||
// ── 2× countersunk wall screws ────────────────────────────────────
|
||||
for (sz = [-BASE_SCREW_SPC/2, BASE_SCREW_SPC/2]) {
|
||||
translate([0, -BASE_T - e, sz])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = BASE_SCREW_D, h = BASE_T + 2*e);
|
||||
translate([0, -BASE_T - e, sz])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d1 = BASE_SCREW_HD, d2 = BASE_SCREW_D,
|
||||
h = BASE_SCREW_HH + e);
|
||||
// ── Stem bore ─────────────────────────────────────────
|
||||
translate([0,0,-e])
|
||||
cylinder(d=STEM_BORE, h=COL_H + 2*e);
|
||||
|
||||
// ── M4 clamping bolt holes (Y direction) ──────────────
|
||||
for (bx=[-COL_BOLT_X, COL_BOLT_X]) {
|
||||
translate([bx, y_front ? COL_OD/2 : 0, COL_H/2])
|
||||
rotate([90,0,0])
|
||||
cylinder(d=COL_BOLT_D, h=COL_OD/2 + e);
|
||||
// Thumbscrew head recess on outer face (front only — access side)
|
||||
if (y_front) {
|
||||
translate([bx, COL_OD/2 - WALL, COL_H/2])
|
||||
rotate([90,0,0])
|
||||
cylinder(d=THUMB_HEAD_D, h=8 + e);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Pivot bolt bore (M3, through both ears) ───────────────────────
|
||||
translate([-(ear_sep/2 + ear_t + e), KNUCKLE_T * 0.55, 0])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = PIVOT_D, h = ear_sep + 2*ear_t + 2*e);
|
||||
// ── M4 hex nut pockets (rear half) ────────────────────
|
||||
if (!y_front) {
|
||||
for (bx=[-COL_BOLT_X, COL_BOLT_X]) {
|
||||
translate([bx, -(COL_OD/4 + e), COL_H/2])
|
||||
rotate([90,0,0])
|
||||
cylinder(d=COL_NUT_W/cos(30), h=COL_NUT_H + e,
|
||||
$fn=6);
|
||||
}
|
||||
}
|
||||
|
||||
// ── M3 nyloc nut pocket (outer face of one ear) ───────────────────
|
||||
translate([ear_sep/2 + ear_t - PIVOT_NUT_H - 0.4,
|
||||
KNUCKLE_T * 0.55, 0])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = PIVOT_NUT_AF / cos(30),
|
||||
h = PIVOT_NUT_H + 0.5, $fn = 6);
|
||||
// ── Set screw (height lock, front half) ───────────────
|
||||
if (y_front) {
|
||||
translate([0, COL_OD/2, COL_H * 0.8])
|
||||
rotate([90,0,0])
|
||||
cylinder(d=COL_BOLT_D,
|
||||
h=COL_OD/2 - STEM_BORE/2 + e);
|
||||
}
|
||||
|
||||
// ── Detent arc — 7 notches at 15° steps on +X ear inner face ─────
|
||||
for (da = [0 : 15 : 90])
|
||||
translate([ear_sep/2 - e,
|
||||
KNUCKLE_T * 0.55 + DETENT_R * sin(da),
|
||||
DETENT_R * cos(da)])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = DETENT_D, h = ear_t * 0.45 + e);
|
||||
// ── USB cable routing channel (rear half, −X side) ────
|
||||
if (!y_front) {
|
||||
translate([-COL_OD/2, -USB_CHAN_W/2, -e])
|
||||
cube([USB_CHAN_D, USB_CHAN_W, COL_H + 2*e]);
|
||||
}
|
||||
|
||||
// ── Installation label recess (rear face of backplate) ────────────
|
||||
translate([0, -BASE_T - e, 0])
|
||||
rotate([-90, 0, 0])
|
||||
cube([BASE_W - 12, BASE_H - 16, 1.6], center = true);
|
||||
|
||||
// ── Lightening pocket ─────────────────────────────────────────────
|
||||
translate([0, -BASE_T + 1.5, 0])
|
||||
cube([BASE_W - 14, BASE_T - 3, BASE_H - 20], center = true);
|
||||
// ── M4 hole through arm boss (Z direction, for bracket bolt) ─
|
||||
if (y_front) {
|
||||
for (dx=[-ARM_W/4, ARM_W/4])
|
||||
translate([dx, COL_OD/2 + ARM_L/2, COL_H * 0.35])
|
||||
cylinder(d=COL_BOLT_D, h=COL_H * 0.35 + e);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 2 — TILT ARM
|
||||
// ============================================================
|
||||
// Pivoting arm linking wall_base pivot ears to anchor_cradle.
|
||||
// Knuckle end (Z=0): M3 pivot bore + spring-plunger detent pocket
|
||||
// that indexes into the base ear detent arc notches.
|
||||
// Cradle end (Z=ARM_L): 2× M3 bolt attachment to cradle back wall.
|
||||
// USB-C cable channel runs along outer (+Y) face, full arm length.
|
||||
//
|
||||
// Print: knuckle face flat on bed, PETG, 5 perims, 40 % gyroid.
|
||||
module tilt_arm() {
|
||||
total_h = ARM_L + 10;
|
||||
// ─────────────────────────────────────────────────────────────
|
||||
// module_bracket()
|
||||
// Bolts to collar arm boss. Holds MaUWB PCB facing outward.
|
||||
// Tilted BRKT_TILT° from vertical — antenna clears stem.
|
||||
// Print flat-face-down (back wall on bed).
|
||||
// ─────────────────────────────────────────────────────────────
|
||||
module module_bracket() {
|
||||
bk = BRKT_BACK_T;
|
||||
sd = BRKT_SIDE_T;
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── Arm body ─────────────────────────────────────────────────
|
||||
translate([-ARM_W/2, 0, 0])
|
||||
cube([ARM_W, ARM_T, total_h]);
|
||||
// ── Back wall (mounts to collar arm boss) ─────────
|
||||
cube([ARM_W, bk, MAWB_H + M2_STNDFF + 6]);
|
||||
|
||||
// ── Knuckle boss (rounded pivot end) ─────────────────────────
|
||||
translate([0, ARM_T/2, 0])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = ARM_W, h = ARM_T, center = true);
|
||||
// ── Side walls ────────────────────────────────────
|
||||
for (sx=[0, ARM_W - sd])
|
||||
translate([sx, bk, 0])
|
||||
cube([sd, MAWB_L + 2, MAWB_H + M2_STNDFF + 6]);
|
||||
|
||||
// ── Cradle attach stub (Z = ARM_L) ────────────────────────────
|
||||
translate([-ARM_W/2, 0, ARM_L])
|
||||
cube([ARM_W, ARM_T + CRADLE_BACK_T, ARM_T]);
|
||||
// ── M2 standoff posts (PCB mounts to these) ───────
|
||||
for (hx=[0, MAWB_HOLE_X], hy=[0, MAWB_HOLE_Y])
|
||||
translate([(ARM_W - MAWB_HOLE_X)/2 + hx,
|
||||
bk + (MAWB_L - MAWB_HOLE_Y)/2 + hy,
|
||||
0])
|
||||
cylinder(d=M2_STNDFF_OD, h=M2_STNDFF);
|
||||
}
|
||||
|
||||
// ── M3 pivot bore ─────────────────────────────────────────────────
|
||||
translate([-ARM_W/2 - e, ARM_T/2, 0])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = PIVOT_D, h = ARM_W + 2*e);
|
||||
// ── M2 bores through standoffs ────────────────────────
|
||||
for (hx=[0, MAWB_HOLE_X], hy=[0, MAWB_HOLE_Y])
|
||||
translate([(ARM_W - MAWB_HOLE_X)/2 + hx,
|
||||
bk + (MAWB_L - MAWB_HOLE_Y)/2 + hy,
|
||||
-e])
|
||||
cylinder(d=M2_D, h=M2_STNDFF + e);
|
||||
|
||||
// ── Detent plunger pocket (3 mm spring-ball, outer +Y face) ──────
|
||||
translate([0, ARM_T + e, 0])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = 3.2, h = 4 + e);
|
||||
// ── Antenna clearance cutout in back wall ─────────────
|
||||
// Open slot near top of back wall so antenna is unobstructed
|
||||
translate([sd, -e, M2_STNDFF + 2])
|
||||
cube([ARM_W - 2*sd, bk + 2*e, MAWB_H]);
|
||||
|
||||
// ── USB-C cable channel (outer +Y face, mid-arm length) ───────────
|
||||
translate([-USBC_CHAN_W/2, ARM_T - e, ARM_T + 4])
|
||||
cube([USBC_CHAN_W, USBC_CHAN_H, ARM_L - ARM_T - 8]);
|
||||
// ── USB port access notch on one side wall ────────────
|
||||
translate([-e, bk + 2, M2_STNDFF - 1])
|
||||
cube([sd + 2*e, USB_NOTCH_W, USB_NOTCH_H]);
|
||||
|
||||
// ── Cradle attach bolt holes (2× M3 at cradle stub) ───────────────
|
||||
for (bx = [-ARM_W/4, ARM_W/4])
|
||||
translate([bx, ARM_T/2, ARM_L + ARM_T/2])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d = M3_D, h = ARM_T + CRADLE_BACK_T + 2*e);
|
||||
|
||||
// ── M3 nut pockets (front of cradle stub) ─────────────────────────
|
||||
for (bx = [-ARM_W/4, ARM_W/4])
|
||||
translate([bx, ARM_T/2, ARM_L + ARM_T/2])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = M3_NUT_AF / cos(30),
|
||||
h = M3_NUT_H + 0.5, $fn = 6);
|
||||
|
||||
// ── Lightening pocket ─────────────────────────────────────────────
|
||||
translate([0, ARM_T/2, ARM_L/2])
|
||||
cube([ARM_W - 4, ARM_T - 2, ARM_L - 18], center = true);
|
||||
// ── Mounting holes to collar arm boss (×2) ────────────
|
||||
for (dx=[-ARM_W/4, ARM_W/4])
|
||||
translate([ARM_W/2 + dx, bk + ARM_L/2, -e])
|
||||
cylinder(d=COL_BOLT_D, h=6 + e);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 3 — ANCHOR CRADLE
|
||||
// ============================================================
|
||||
// Open-front U-cradle for ESP32 UWB Pro PCB.
|
||||
// PCB retained on 4× M2.5 standoffs matching UWB_HOLE_X × UWB_HOLE_Y.
|
||||
// Back wall features:
|
||||
// • USB-C exit slot — aligns with PCB USB-C port
|
||||
// • USB-C groove — cable routes from slot toward arm channel
|
||||
// • Label card slot — insert printed strip for anchor ID
|
||||
// • Antenna keep-out — back wall material removed above antenna area
|
||||
// Front lip prevents PCB from sliding forward.
|
||||
// Two attachment tabs bolt to tilt_arm cradle stub.
|
||||
//
|
||||
// Print: back wall flat on bed, PETG, 5 perims, 40 % gyroid.
|
||||
module anchor_cradle() {
|
||||
outer_l = UWB_L + 2*CRADLE_WALL_T;
|
||||
outer_w = UWB_W + CRADLE_FLOOR_T;
|
||||
pcb_z = CRADLE_FLOOR_T + STANDOFF_H;
|
||||
total_z = pcb_z + UWB_H + 2;
|
||||
// ─────────────────────────────────────────────────────────────
|
||||
// single_anchor_assembly()
|
||||
// ─────────────────────────────────────────────────────────────
|
||||
module single_anchor_assembly(show_phantom=false) {
|
||||
// Collar
|
||||
color("SteelBlue", 0.9) collar_half("front");
|
||||
color("CornflowerBlue", 0.9) mirror([0,1,0]) collar_half("rear");
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── Cradle body ───────────────────────────────────────────────
|
||||
translate([-outer_l/2, 0, 0])
|
||||
cube([outer_l, outer_w, total_z]);
|
||||
// Bracket tilted BRKT_TILT° outward from top of arm boss
|
||||
color("LightSteelBlue", 0.85)
|
||||
translate([0, COL_OD/2 + ARM_L, COL_H * 0.3])
|
||||
rotate([BRKT_TILT, 0, 0])
|
||||
translate([-ARM_W/2, 0, 0])
|
||||
module_bracket();
|
||||
|
||||
// ── Front retaining lip ───────────────────────────────────────
|
||||
translate([-outer_l/2, outer_w - CRADLE_LIP_T, 0])
|
||||
cube([outer_l, CRADLE_LIP_T, CRADLE_LIP_H]);
|
||||
|
||||
// ── Arm attachment tabs (behind back wall) ─────────────────────
|
||||
for (tx = [-ARM_W/4, ARM_W/4])
|
||||
translate([tx - 4, -CRADLE_BACK_T, 0])
|
||||
cube([8, CRADLE_BACK_T + 1, total_z]);
|
||||
}
|
||||
|
||||
// ── PCB pocket ────────────────────────────────────────────────────
|
||||
translate([-UWB_L/2, 0, pcb_z])
|
||||
cube([UWB_L, UWB_W + 1, UWB_H + 4]);
|
||||
|
||||
// ── USB-C exit slot (through back wall, aligned to PCB port) ─────
|
||||
translate([0, -CRADLE_BACK_T - e,
|
||||
pcb_z + UWB_H/2 - UWB_USBC_H/2])
|
||||
cube([UWB_USBC_W + 2, CRADLE_BACK_T + 2*e, UWB_USBC_H + 2],
|
||||
center = [true, false, false]);
|
||||
|
||||
// ── USB-C cable routing groove (outer back wall face) ─────────────
|
||||
translate([0, -CRADLE_BACK_T - e, -e])
|
||||
cube([USBC_CHAN_W, USBC_CHAN_H, pcb_z + UWB_H/2 + USBC_CHAN_H],
|
||||
center = [true, false, false]);
|
||||
|
||||
// ── Label card slot (insert from below, rear face upper half) ─────
|
||||
// Paper/laminate card strip identifying this anchor instance
|
||||
translate([0, -CRADLE_BACK_T - e, pcb_z + UWB_H/2])
|
||||
cube([LABEL_W, LABEL_T + 0.3, LABEL_H],
|
||||
center = [true, false, false]);
|
||||
|
||||
// ── Antenna keep-out: remove back wall above antenna area ─────────
|
||||
translate([0, -e, pcb_z + UWB_H - UWB_ANTENNA_L])
|
||||
cube([UWB_L - 4, CRADLE_BACK_T + 2*e, UWB_ANTENNA_L + 4],
|
||||
center = [true, false, false]);
|
||||
|
||||
// ── Arm bolt holes through attachment tabs ────────────────────────
|
||||
for (tx = [-ARM_W/4, ARM_W/4])
|
||||
translate([tx, ARM_T/2 - CRADLE_BACK_T, total_z/2])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d = M3_D, h = ARM_T + CRADLE_BACK_T + 2*e);
|
||||
|
||||
// ── Lightening slots in side walls ────────────────────────────────
|
||||
for (side_x = [-outer_l/2 - e, outer_l/2 - CRADLE_WALL_T - e])
|
||||
translate([side_x, 2, pcb_z + 2])
|
||||
cube([CRADLE_WALL_T + 2*e, UWB_W - 4, UWB_H - 4]);
|
||||
}
|
||||
|
||||
// ── M2.5 standoff bosses (positive, inside cradle floor) ──────────────
|
||||
for (hx = [-UWB_HOLE_X/2, UWB_HOLE_X/2])
|
||||
for (hy = [(outer_w - UWB_W)/2 + (UWB_W - UWB_HOLE_Y)/2,
|
||||
(outer_w - UWB_W)/2 + (UWB_W - UWB_HOLE_Y)/2 + UWB_HOLE_Y])
|
||||
difference() {
|
||||
translate([hx, hy, CRADLE_FLOOR_T - e])
|
||||
cylinder(d = STANDOFF_OD, h = STANDOFF_H + e);
|
||||
translate([hx, hy, CRADLE_FLOOR_T - 2*e])
|
||||
cylinder(d = M2P5_D, h = STANDOFF_H + 4);
|
||||
}
|
||||
// Phantom UWB PCB
|
||||
if (show_phantom)
|
||||
color("ForestGreen", 0.4)
|
||||
translate([-MAWB_L/2,
|
||||
COL_OD/2 + ARM_L + BRKT_BACK_T,
|
||||
COL_H * 0.3 + M2_STNDFF])
|
||||
cube([MAWB_L, MAWB_W, MAWB_H]);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// PART 4 — CABLE CLIP
|
||||
// ============================================================
|
||||
// Snap-on C-clip retaining USB-C cable along tilt arm outer face.
|
||||
// Presses onto ARM_T-wide arm with PETG snap tongues.
|
||||
// Open-front cable channel for push-in cable insertion.
|
||||
// Print ×2–3 per anchor, spaced 25 mm along arm.
|
||||
//
|
||||
// Print: clip-opening face down, PETG, 3 perims, 20 % infill.
|
||||
module cable_clip() {
|
||||
ch_r = CLIP_CABLE_D/2 + CLIP_T;
|
||||
snap_t = 1.6;
|
||||
// ─────────────────────────────────────────────────────────────
|
||||
// Render selector
|
||||
// ─────────────────────────────────────────────────────────────
|
||||
if (RENDER == "assembly") {
|
||||
single_anchor_assembly(show_phantom=true);
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── Body plate ────────────────────────────────────────────────
|
||||
translate([-CLIP_BODY_W/2, 0, 0])
|
||||
cube([CLIP_BODY_W, CLIP_T, CLIP_BODY_H]);
|
||||
} else if (RENDER == "collar_front") {
|
||||
collar_half("front");
|
||||
|
||||
// ── Cable channel (C-shape, opens toward +Y) ─────────────────
|
||||
translate([0, CLIP_T + ch_r, CLIP_BODY_H/2])
|
||||
rotate([0, 90, 0])
|
||||
difference() {
|
||||
cylinder(r = ch_r, h = CLIP_BODY_W, center = true);
|
||||
cylinder(r = CLIP_CABLE_D/2,
|
||||
h = CLIP_BODY_W + 2*e, center = true);
|
||||
// open insertion slot
|
||||
translate([0, ch_r + e, 0])
|
||||
cube([CLIP_CABLE_D * 0.85,
|
||||
ch_r * 2 + 2*e,
|
||||
CLIP_BODY_W + 2*e], center = true);
|
||||
}
|
||||
} else if (RENDER == "collar_rear") {
|
||||
collar_half("rear");
|
||||
|
||||
// ── Snap tongues (straddle arm, -Y side of body) ─────────────
|
||||
for (tx = [-CLIP_BODY_W/2 + 1.5,
|
||||
CLIP_BODY_W/2 - 1.5 - snap_t])
|
||||
translate([tx, -ARM_T - 1, 0])
|
||||
cube([snap_t, ARM_T + 1 + CLIP_T, CLIP_BODY_H]);
|
||||
} else if (RENDER == "bracket") {
|
||||
module_bracket();
|
||||
|
||||
// ── Snap barbs ────────────────────────────────────────────────
|
||||
for (tx = [-CLIP_BODY_W/2 + 1.5,
|
||||
CLIP_BODY_W/2 - 1.5 - snap_t])
|
||||
translate([tx + snap_t/2, -ARM_T - 1, CLIP_BODY_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d = 2, h = snap_t, center = true);
|
||||
}
|
||||
} else if (RENDER == "pair") {
|
||||
// Both anchors at 250 mm spacing on a stem stub
|
||||
color("Silver", 0.2)
|
||||
translate([0, 0, -50])
|
||||
cylinder(d=STEM_OD, h=ANCHOR_SPACING + COL_H + 100);
|
||||
|
||||
// ── Arm slot (arm body passes between tongues) ─────────────────────
|
||||
translate([0, -ARM_T - 1 - e, CLIP_BODY_H/2])
|
||||
cube([CLIP_BODY_W - 6, ARM_T + 2, CLIP_BODY_H - 4], center = true);
|
||||
}
|
||||
// Lower anchor (Z = 0)
|
||||
single_anchor_assembly(show_phantom=true);
|
||||
|
||||
// Upper anchor (Z = ANCHOR_SPACING)
|
||||
translate([0, 0, ANCHOR_SPACING])
|
||||
single_anchor_assembly(show_phantom=true);
|
||||
}
|
||||
|
||||
|
||||
@ -1,30 +0,0 @@
|
||||
; SaltyBot UWB Anchor Firmware — Issue #544
|
||||
; Target: Makerfabs ESP32 UWB Pro (DW3000 chip)
|
||||
;
|
||||
; Library: Makerfabs MaUWB_DW3000
|
||||
; https://github.com/Makerfabs/MaUWB_DW3000
|
||||
;
|
||||
; Flash:
|
||||
; pio run -e anchor0 --target upload (port-side anchor)
|
||||
; pio run -e anchor1 --target upload (starboard anchor)
|
||||
; Monitor:
|
||||
; pio device monitor -e anchor0 -b 115200
|
||||
|
||||
[common]
|
||||
platform = espressif32
|
||||
board = esp32dev
|
||||
framework = arduino
|
||||
monitor_speed = 115200
|
||||
upload_speed = 921600
|
||||
lib_deps =
|
||||
https://github.com/Makerfabs/MaUWB_DW3000.git
|
||||
build_flags =
|
||||
-DCORE_DEBUG_LEVEL=0
|
||||
|
||||
[env:anchor0]
|
||||
extends = common
|
||||
build_flags = ${common.build_flags} -DANCHOR_ID=0
|
||||
|
||||
[env:anchor1]
|
||||
extends = common
|
||||
build_flags = ${common.build_flags} -DANCHOR_ID=1
|
||||
@ -1,413 +0,0 @@
|
||||
/*
|
||||
* uwb_anchor — SaltyBot ESP32 UWB Pro anchor firmware (TWR responder)
|
||||
* Issue #544
|
||||
*
|
||||
* Hardware: Makerfabs ESP32 UWB Pro (DW3000 chip)
|
||||
*
|
||||
* Role
|
||||
* ────
|
||||
* Anchor sits on SaltyBot body, USB-connected to Jetson Orin.
|
||||
* Two anchors per robot (anchor-0 port side, anchor-1 starboard).
|
||||
* Person-worn tags initiate ranging; anchors respond.
|
||||
*
|
||||
* Protocol: Double-Sided TWR (DS-TWR)
|
||||
* ────────────────────────────────────
|
||||
* Tag → Anchor POLL (msg_type 0x01)
|
||||
* Anchor → Tag RESP (msg_type 0x02, payload: T_poll_rx, T_resp_tx)
|
||||
* Tag → Anchor FINAL (msg_type 0x03, payload: Ra, Da, Db timestamps)
|
||||
* Anchor computes range via DS-TWR formula, emits +RANGE on Serial.
|
||||
*
|
||||
* Serial output (115200 8N1, USB-CDC to Jetson)
|
||||
* ──────────────────────────────────────────────
|
||||
* +RANGE:<anchor_id>,<range_mm>,<rssi_dbm>\r\n (on each successful range)
|
||||
*
|
||||
* AT commands (host → anchor)
|
||||
* ───────────────────────────
|
||||
* AT+RANGE? → returns last buffered +RANGE line
|
||||
* AT+RANGE_ADDR=<hex_addr> → pair with specific tag (filter others)
|
||||
* AT+RANGE_ADDR= → clear pairing (accept all tags)
|
||||
* AT+ID? → returns +ID:<anchor_id>
|
||||
*
|
||||
* Pin mapping — Makerfabs ESP32 UWB Pro
|
||||
* ──────────────────────────────────────
|
||||
* SPI SCK 18 SPI MISO 19 SPI MOSI 23
|
||||
* DW CS 21 DW RST 27 DW IRQ 34
|
||||
*
|
||||
* Build
|
||||
* ──────
|
||||
* pio run -e anchor0 --target upload (port side)
|
||||
* pio run -e anchor1 --target upload (starboard)
|
||||
*/
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <SPI.h>
|
||||
#include <math.h>
|
||||
#include "dw3000.h" // Makerfabs MaUWB_DW3000 library
|
||||
|
||||
/* ── Configurable ───────────────────────────────────────────────── */
|
||||
|
||||
#ifndef ANCHOR_ID
|
||||
# define ANCHOR_ID 0 /* 0 = port, 1 = starboard */
|
||||
#endif
|
||||
|
||||
#define SERIAL_BAUD 115200
|
||||
|
||||
/* ── Pin map (Makerfabs ESP32 UWB Pro) ─────────────────────────── */
|
||||
|
||||
#define PIN_SCK 18
|
||||
#define PIN_MISO 19
|
||||
#define PIN_MOSI 23
|
||||
#define PIN_CS 21
|
||||
#define PIN_RST 27
|
||||
#define PIN_IRQ 34
|
||||
|
||||
/* ── DW3000 channel / PHY config ───────────────────────────────── */
|
||||
|
||||
static dwt_config_t dw_cfg = {
|
||||
5, /* channel 5 (6.5 GHz, best penetration) */
|
||||
DWT_PLEN_128, /* preamble length */
|
||||
DWT_PAC8, /* PAC size */
|
||||
9, /* TX preamble code */
|
||||
9, /* RX preamble code */
|
||||
1, /* SFD type (IEEE 802.15.4z) */
|
||||
DWT_BR_6M8, /* data rate 6.8 Mbps */
|
||||
DWT_PHR_MODE_STD, /* standard PHR */
|
||||
DWT_PHR_RATE_DATA,
|
||||
(129 + 8 - 8), /* SFD timeout */
|
||||
DWT_STS_MODE_OFF, /* STS off — standard TWR */
|
||||
DWT_STS_LEN_64,
|
||||
DWT_PDOA_M0, /* no PDoA */
|
||||
};
|
||||
|
||||
/* ── Frame format ──────────────────────────────────────────────── */
|
||||
|
||||
/* Byte layout for all frames:
|
||||
* [0] frame_type (FTYPE_*)
|
||||
* [1] src_id (tag 8-bit addr, or ANCHOR_ID)
|
||||
* [2] dst_id
|
||||
* [3..] payload
|
||||
* (FCS appended automatically by DW3000 — 2 bytes)
|
||||
*/
|
||||
|
||||
#define FTYPE_POLL 0x01
|
||||
#define FTYPE_RESP 0x02
|
||||
#define FTYPE_FINAL 0x03
|
||||
|
||||
#define FRAME_HDR 3
|
||||
#define FCS_LEN 2
|
||||
|
||||
/* RESP payload: T_poll_rx(5 B) + T_resp_tx(5 B) */
|
||||
#define RESP_PAYLOAD 10
|
||||
#define RESP_FRAME_LEN (FRAME_HDR + RESP_PAYLOAD + FCS_LEN)
|
||||
|
||||
/* FINAL payload: Ra(5 B) + Da(5 B) + Db(5 B) */
|
||||
#define FINAL_PAYLOAD 15
|
||||
#define FINAL_FRAME_LEN (FRAME_HDR + FINAL_PAYLOAD + FCS_LEN)
|
||||
|
||||
/* ── Timing ────────────────────────────────────────────────────── */
|
||||
|
||||
/* Turnaround delay: anchor waits 500 µs after poll_rx before tx_resp.
|
||||
* DW3000 tick = 1/(128×499.2e6) ≈ 15.65 ps → 500 µs = ~31.95M ticks.
|
||||
* Stored as uint32 shifted right 8 bits for dwt_setdelayedtrxtime. */
|
||||
#define RESP_TX_DLY_US 500UL
|
||||
#define DWT_TICKS_PER_US 63898UL /* 1µs in DW3000 ticks (×8 prescaler) */
|
||||
#define RESP_TX_DLY_TICKS (RESP_TX_DLY_US * DWT_TICKS_PER_US)
|
||||
|
||||
/* How long anchor listens for FINAL after sending RESP */
|
||||
#define FINAL_RX_TIMEOUT_US 3000
|
||||
|
||||
/* Speed of light (m/s) */
|
||||
#define SPEED_OF_LIGHT 299702547.0
|
||||
|
||||
/* DW3000 40-bit timestamp mask */
|
||||
#define DWT_TS_MASK 0xFFFFFFFFFFULL
|
||||
|
||||
/* Antenna delay (factory default; calibrate per unit for best accuracy) */
|
||||
#define ANT_DELAY 16385
|
||||
|
||||
/* ── Interrupt flags (set in ISR, polled in main) ──────────────── */
|
||||
|
||||
static volatile bool g_rx_ok = false;
|
||||
static volatile bool g_tx_done = false;
|
||||
static volatile bool g_rx_err = false;
|
||||
static volatile bool g_rx_to = false;
|
||||
|
||||
static uint8_t g_rx_buf[128];
|
||||
static uint32_t g_rx_len = 0;
|
||||
|
||||
/* ── State ──────────────────────────────────────────────────────── */
|
||||
|
||||
/* Last successful range (serves AT+RANGE? queries) */
|
||||
static int32_t g_last_range_mm = -1;
|
||||
static char g_last_range_line[72] = {};
|
||||
|
||||
/* Optional tag pairing: 0 = accept all tags */
|
||||
static uint8_t g_paired_tag_id = 0;
|
||||
|
||||
/* ── DW3000 ISR callbacks ───────────────────────────────────────── */
|
||||
|
||||
static void cb_tx_done(const dwt_cb_data_t *) { g_tx_done = true; }
|
||||
|
||||
static void cb_rx_ok(const dwt_cb_data_t *d) {
|
||||
g_rx_len = d->datalength;
|
||||
if (g_rx_len > sizeof(g_rx_buf)) g_rx_len = sizeof(g_rx_buf);
|
||||
dwt_readrxdata(g_rx_buf, g_rx_len, 0);
|
||||
g_rx_ok = true;
|
||||
}
|
||||
|
||||
static void cb_rx_err(const dwt_cb_data_t *) { g_rx_err = true; }
|
||||
static void cb_rx_to(const dwt_cb_data_t *) { g_rx_to = true; }
|
||||
|
||||
/* ── Timestamp helpers ──────────────────────────────────────────── */
|
||||
|
||||
static uint64_t ts_read(const uint8_t *p) {
|
||||
uint64_t v = 0;
|
||||
for (int i = 4; i >= 0; i--) v = (v << 8) | p[i];
|
||||
return v;
|
||||
}
|
||||
|
||||
static void ts_write(uint8_t *p, uint64_t v) {
|
||||
for (int i = 0; i < 5; i++, v >>= 8) p[i] = (uint8_t)(v & 0xFF);
|
||||
}
|
||||
|
||||
static inline uint64_t ts_diff(uint64_t later, uint64_t earlier) {
|
||||
return (later - earlier) & DWT_TS_MASK;
|
||||
}
|
||||
|
||||
static inline double ticks_to_s(uint64_t t) {
|
||||
return (double)t / (128.0 * 499200000.0);
|
||||
}
|
||||
|
||||
/* Estimate receive power from CIR diagnostics (dBm) */
|
||||
static float rx_power_dbm(void) {
|
||||
dwt_rxdiag_t d;
|
||||
dwt_readdiagnostics(&d);
|
||||
if (d.maxGrowthCIR == 0 || d.rxPreamCount == 0) return 0.0f;
|
||||
float f = (float)d.maxGrowthCIR;
|
||||
float n = (float)d.rxPreamCount;
|
||||
return 10.0f * log10f((f * f) / (n * n)) - 121.74f;
|
||||
}
|
||||
|
||||
/* ── AT command handler ─────────────────────────────────────────── */
|
||||
|
||||
static char g_at_buf[64];
|
||||
static int g_at_idx = 0;
|
||||
|
||||
static void at_dispatch(const char *cmd) {
|
||||
if (strcmp(cmd, "AT+RANGE?") == 0) {
|
||||
if (g_last_range_mm >= 0)
|
||||
Serial.println(g_last_range_line);
|
||||
else
|
||||
Serial.println("+RANGE:NO_DATA");
|
||||
|
||||
} else if (strcmp(cmd, "AT+ID?") == 0) {
|
||||
Serial.printf("+ID:%d\r\n", ANCHOR_ID);
|
||||
|
||||
} else if (strncmp(cmd, "AT+RANGE_ADDR=", 14) == 0) {
|
||||
const char *v = cmd + 14;
|
||||
if (*v == '\0') {
|
||||
g_paired_tag_id = 0;
|
||||
Serial.println("+OK:UNPAIRED");
|
||||
} else {
|
||||
g_paired_tag_id = (uint8_t)strtoul(v, nullptr, 0);
|
||||
Serial.printf("+OK:PAIRED=0x%02X\r\n", g_paired_tag_id);
|
||||
}
|
||||
} else {
|
||||
Serial.println("+ERR:UNKNOWN_CMD");
|
||||
}
|
||||
}
|
||||
|
||||
static void serial_poll(void) {
|
||||
while (Serial.available()) {
|
||||
char c = (char)Serial.read();
|
||||
if (c == '\r') continue;
|
||||
if (c == '\n') {
|
||||
g_at_buf[g_at_idx] = '\0';
|
||||
if (g_at_idx > 0) at_dispatch(g_at_buf);
|
||||
g_at_idx = 0;
|
||||
} else if (g_at_idx < (int)(sizeof(g_at_buf) - 1)) {
|
||||
g_at_buf[g_at_idx++] = c;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ── DS-TWR anchor state machine ────────────────────────────────── */
|
||||
|
||||
/*
|
||||
* DS-TWR responder (one shot):
|
||||
* 1. Wait for POLL from tag
|
||||
* 2. Delayed-TX RESP (carry T_poll_rx + scheduled T_resp_tx)
|
||||
* 3. Wait for FINAL from tag (tag embeds Ra, Da, Db)
|
||||
* 4. Compute: Rb = T_final_rx − T_resp_tx
|
||||
* tof = (Ra·Rb − Da·Db) / (Ra+Rb+Da+Db)
|
||||
* range_m = tof × c
|
||||
* 5. Print +RANGE line
|
||||
*/
|
||||
static void twr_cycle(void) {
|
||||
|
||||
/* --- 1. Listen for POLL --- */
|
||||
dwt_setrxtimeout(0);
|
||||
dwt_rxenable(DWT_START_RX_IMMEDIATE);
|
||||
|
||||
g_rx_ok = g_rx_err = false;
|
||||
uint32_t deadline = millis() + 2000;
|
||||
while (!g_rx_ok && !g_rx_err) {
|
||||
serial_poll();
|
||||
if (millis() > deadline) {
|
||||
/* restart RX if we've been stuck */
|
||||
dwt_rxenable(DWT_START_RX_IMMEDIATE);
|
||||
deadline = millis() + 2000;
|
||||
}
|
||||
yield();
|
||||
}
|
||||
if (!g_rx_ok || g_rx_len < FRAME_HDR) return;
|
||||
|
||||
/* validate POLL */
|
||||
if (g_rx_buf[0] != FTYPE_POLL) return;
|
||||
uint8_t tag_id = g_rx_buf[1];
|
||||
if (g_paired_tag_id != 0 && tag_id != g_paired_tag_id) return;
|
||||
|
||||
/* --- 2. Record T_poll_rx --- */
|
||||
uint8_t poll_rx_raw[5];
|
||||
dwt_readrxtimestamp(poll_rx_raw);
|
||||
uint64_t T_poll_rx = ts_read(poll_rx_raw);
|
||||
|
||||
/* Compute delayed TX time: poll_rx + turnaround, aligned to 512-tick grid */
|
||||
uint64_t resp_tx_sched = (T_poll_rx + RESP_TX_DLY_TICKS) & ~0x1FFULL;
|
||||
|
||||
/* Build RESP frame */
|
||||
uint8_t resp[RESP_FRAME_LEN];
|
||||
resp[0] = FTYPE_RESP;
|
||||
resp[1] = ANCHOR_ID;
|
||||
resp[2] = tag_id;
|
||||
ts_write(&resp[3], T_poll_rx); /* T_poll_rx (tag uses this) */
|
||||
ts_write(&resp[8], resp_tx_sched); /* scheduled T_resp_tx */
|
||||
|
||||
dwt_writetxdata(RESP_FRAME_LEN - FCS_LEN, resp, 0);
|
||||
dwt_writetxfctrl(RESP_FRAME_LEN, 0, 1 /*ranging*/);
|
||||
dwt_setdelayedtrxtime((uint32_t)(resp_tx_sched >> 8));
|
||||
|
||||
/* Enable RX after TX to receive FINAL */
|
||||
dwt_setrxaftertxdelay(300);
|
||||
dwt_setrxtimeout(FINAL_RX_TIMEOUT_US);
|
||||
|
||||
/* Fire delayed TX */
|
||||
g_tx_done = g_rx_ok = g_rx_err = g_rx_to = false;
|
||||
if (dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED) != DWT_SUCCESS) {
|
||||
dwt_forcetrxoff();
|
||||
return; /* TX window missed — try next cycle */
|
||||
}
|
||||
|
||||
/* Wait for TX done (short wait, ISR fires fast) */
|
||||
uint32_t t0 = millis();
|
||||
while (!g_tx_done && millis() - t0 < 15) { yield(); }
|
||||
|
||||
/* Read actual T_resp_tx */
|
||||
uint8_t resp_tx_raw[5];
|
||||
dwt_readtxtimestamp(resp_tx_raw);
|
||||
uint64_t T_resp_tx = ts_read(resp_tx_raw);
|
||||
|
||||
/* --- 3. Wait for FINAL --- */
|
||||
t0 = millis();
|
||||
while (!g_rx_ok && !g_rx_err && !g_rx_to && millis() - t0 < 60) {
|
||||
serial_poll();
|
||||
yield();
|
||||
}
|
||||
if (!g_rx_ok || g_rx_len < FRAME_HDR + FINAL_PAYLOAD) return;
|
||||
if (g_rx_buf[0] != FTYPE_FINAL) return;
|
||||
if (g_rx_buf[1] != tag_id) return;
|
||||
|
||||
/* Extract DS-TWR timestamps from FINAL payload */
|
||||
uint64_t Ra = ts_read(&g_rx_buf[3]); /* tag: T_resp_rx − T_poll_tx */
|
||||
uint64_t Da = ts_read(&g_rx_buf[8]); /* tag: T_final_tx − T_resp_rx */
|
||||
/* g_rx_buf[13..17] = Db from tag (cross-check, unused here) */
|
||||
|
||||
/* T_final_rx */
|
||||
uint8_t final_rx_raw[5];
|
||||
dwt_readrxtimestamp(final_rx_raw);
|
||||
uint64_t T_final_rx = ts_read(final_rx_raw);
|
||||
|
||||
/* --- 4. DS-TWR formula --- */
|
||||
uint64_t Rb = ts_diff(T_final_rx, T_resp_tx); /* anchor round-trip */
|
||||
uint64_t Db = ts_diff(T_resp_tx, T_poll_rx); /* anchor turnaround */
|
||||
|
||||
double ra = ticks_to_s(Ra), rb = ticks_to_s(Rb);
|
||||
double da = ticks_to_s(Da), db = ticks_to_s(Db);
|
||||
|
||||
double denom = ra + rb + da + db;
|
||||
if (denom < 1e-15) return;
|
||||
|
||||
double tof = (ra * rb - da * db) / denom;
|
||||
double range_m = tof * SPEED_OF_LIGHT;
|
||||
|
||||
/* Validity window: 0.1 m – 130 m */
|
||||
if (range_m < 0.1 || range_m > 130.0) return;
|
||||
|
||||
int32_t range_mm = (int32_t)(range_m * 1000.0 + 0.5);
|
||||
float rssi = rx_power_dbm();
|
||||
|
||||
/* --- 5. Emit +RANGE --- */
|
||||
snprintf(g_last_range_line, sizeof(g_last_range_line),
|
||||
"+RANGE:%d,%ld,%.1f", ANCHOR_ID, (long)range_mm, rssi);
|
||||
g_last_range_mm = range_mm;
|
||||
Serial.println(g_last_range_line);
|
||||
}
|
||||
|
||||
/* ── Arduino setup ──────────────────────────────────────────────── */
|
||||
|
||||
void setup(void) {
|
||||
Serial.begin(SERIAL_BAUD);
|
||||
delay(300);
|
||||
|
||||
Serial.printf("\r\n[uwb_anchor] anchor_id=%d starting\r\n", ANCHOR_ID);
|
||||
|
||||
SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS);
|
||||
|
||||
/* Hardware reset */
|
||||
pinMode(PIN_RST, OUTPUT);
|
||||
digitalWrite(PIN_RST, LOW);
|
||||
delay(2);
|
||||
pinMode(PIN_RST, INPUT_PULLUP);
|
||||
delay(5);
|
||||
|
||||
/* DW3000 probe + init (Makerfabs MaUWB_DW3000 library) */
|
||||
if (dwt_probe((struct dwt_probe_s *)&dw3000_probe_interf)) {
|
||||
Serial.println("[uwb_anchor] FATAL: DW3000 probe failed — check SPI wiring");
|
||||
for (;;) delay(1000);
|
||||
}
|
||||
|
||||
if (dwt_initialise(DWT_DW_INIT) != DWT_SUCCESS) {
|
||||
Serial.println("[uwb_anchor] FATAL: dwt_initialise failed");
|
||||
for (;;) delay(1000);
|
||||
}
|
||||
|
||||
if (dwt_configure(&dw_cfg) != DWT_SUCCESS) {
|
||||
Serial.println("[uwb_anchor] FATAL: dwt_configure failed");
|
||||
for (;;) delay(1000);
|
||||
}
|
||||
|
||||
dwt_setrxantennadelay(ANT_DELAY);
|
||||
dwt_settxantennadelay(ANT_DELAY);
|
||||
dwt_settxpower(0x0E080222UL); /* max TX power for 120 m range */
|
||||
|
||||
dwt_setcallbacks(cb_tx_done, cb_rx_ok, cb_rx_to, cb_rx_err,
|
||||
nullptr, nullptr, nullptr);
|
||||
dwt_setinterrupt(
|
||||
DWT_INT_TXFRS | DWT_INT_RFCG | DWT_INT_RFTO |
|
||||
DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_ARFE | DWT_INT_CPERR,
|
||||
0, DWT_ENABLE_INT_ONLY);
|
||||
|
||||
attachInterrupt(digitalPinToInterrupt(PIN_IRQ),
|
||||
[]() { dwt_isr(); }, RISING);
|
||||
|
||||
Serial.printf("[uwb_anchor] DW3000 ready ch=%d 6.8Mbps id=%d\r\n",
|
||||
dw_cfg.chan, ANCHOR_ID);
|
||||
Serial.println("[uwb_anchor] Listening for tags...");
|
||||
}
|
||||
|
||||
/* ── Arduino loop ───────────────────────────────────────────────── */
|
||||
|
||||
void loop(void) {
|
||||
serial_poll();
|
||||
twr_cycle();
|
||||
}
|
||||
@ -1,19 +0,0 @@
|
||||
# SaltyBot UWB anchor USB-serial persistent symlinks
|
||||
# Install:
|
||||
# sudo cp 99-uwb-anchors.rules /etc/udev/rules.d/
|
||||
# sudo udevadm control --reload && sudo udevadm trigger
|
||||
#
|
||||
# Find serial numbers:
|
||||
# udevadm info -a /dev/ttyUSB0 | grep ATTRS{serial}
|
||||
#
|
||||
# Fill ANCHOR0_SERIAL and ANCHOR1_SERIAL with the values found above.
|
||||
# Anchor 0 = port side → /dev/uwb-anchor0
|
||||
# Anchor 1 = starboard → /dev/uwb-anchor1
|
||||
|
||||
SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \
|
||||
ATTRS{serial}=="ANCHOR0_SERIAL", \
|
||||
SYMLINK+="uwb-anchor0", MODE="0666"
|
||||
|
||||
SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \
|
||||
ATTRS{serial}=="ANCHOR1_SERIAL", \
|
||||
SYMLINK+="uwb-anchor1", MODE="0666"
|
||||
@ -244,17 +244,4 @@
|
||||
#define AUDIO_BUF_HALF 441u // DMA half-buffer: 20ms at 22050 Hz
|
||||
#define AUDIO_VOLUME_DEFAULT 80u // default volume 0-100
|
||||
|
||||
// --- Gimbal Servo Bus (ST3215, USART3 half-duplex, Issue #547) ---
|
||||
// Half-duplex single-wire on PB10 (USART3_TX, AF7) at 1 Mbps.
|
||||
// USART3 is available: not assigned to any active subsystem.
|
||||
#define SERVO_BUS_UART USART3
|
||||
#define SERVO_BUS_PORT GPIOB
|
||||
#define SERVO_BUS_PIN GPIO_PIN_10 // USART3_TX, AF7
|
||||
#define SERVO_BUS_BAUD 1000000u // 1 Mbps (ST3215 default)
|
||||
#define GIMBAL_PAN_ID 1u // ST3215 servo ID for pan
|
||||
#define GIMBAL_TILT_ID 2u // ST3215 servo ID for tilt
|
||||
#define GIMBAL_TLM_HZ 50u // position feedback rate (Hz)
|
||||
#define GIMBAL_PAN_LIMIT_DEG 180.0f // pan soft limit (deg each side)
|
||||
#define GIMBAL_TILT_LIMIT_DEG 90.0f // tilt soft limit (deg each side)
|
||||
|
||||
#endif // CONFIG_H
|
||||
|
||||
@ -1,72 +0,0 @@
|
||||
#ifndef GIMBAL_H
|
||||
#define GIMBAL_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* gimbal.h — Pan/tilt gimbal controller for ST3215 bus servos (Issue #547)
|
||||
*
|
||||
* Manages dual ST3215 serial bus servos:
|
||||
* Pan servo: ID GIMBAL_PAN_ID (config.h, default 1)
|
||||
* Tilt servo: ID GIMBAL_TILT_ID (config.h, default 2)
|
||||
*
|
||||
* Position units: degrees x10 (int16), matching JLink protocol convention.
|
||||
* e.g. 900 = 90.0°, -450 = -45.0°
|
||||
*
|
||||
* Limits:
|
||||
* Pan: -1800..+1800 (x10 deg) = -180..+180 deg
|
||||
* Tilt: -900..+900 (x10 deg) = -90..+90 deg
|
||||
*
|
||||
* The gimbal_tick() function polls servo feedback at GIMBAL_TLM_HZ (50 Hz).
|
||||
* Alternates reading pan position on even ticks, tilt on odd ticks — each
|
||||
* servo polled at 25 Hz to keep bus utilization low.
|
||||
*/
|
||||
|
||||
typedef struct {
|
||||
/* Command state */
|
||||
int16_t cmd_pan_x10; /* Commanded pan (deg x10) */
|
||||
int16_t cmd_tilt_x10; /* Commanded tilt (deg x10) */
|
||||
uint16_t cmd_speed; /* Servo bus speed (0=max, 1-4095) */
|
||||
bool torque_enabled; /* True when torques are enabled */
|
||||
|
||||
/* Feedback state (updated at ~25 Hz per axis) */
|
||||
int16_t fb_pan_x10; /* Measured pan (deg x10) */
|
||||
int16_t fb_tilt_x10; /* Measured tilt (deg x10) */
|
||||
uint16_t fb_pan_speed; /* Raw speed register, pan servo */
|
||||
uint16_t fb_tilt_speed; /* Raw speed register, tilt servo */
|
||||
|
||||
/* Diagnostics */
|
||||
uint32_t rx_ok; /* Successful position reads */
|
||||
uint32_t rx_err; /* Failed position reads */
|
||||
|
||||
uint32_t _last_tick_ms; /* Internal: last tick timestamp */
|
||||
uint8_t _poll_phase; /* Internal: alternates 0=pan 1=tilt */
|
||||
} gimbal_t;
|
||||
|
||||
/*
|
||||
* gimbal_init(g) — enable torque on both servos, center them.
|
||||
* servo_bus_init() must be called first.
|
||||
*/
|
||||
void gimbal_init(gimbal_t *g);
|
||||
|
||||
/*
|
||||
* gimbal_set_pos(g, pan_x10, tilt_x10, speed) — command a new pan/tilt
|
||||
* position. pan_x10 and tilt_x10 are degrees×10, clamped to servo limits.
|
||||
* speed: 0=max servo speed, 1-4095 = scaled.
|
||||
*/
|
||||
void gimbal_set_pos(gimbal_t *g, int16_t pan_x10, int16_t tilt_x10,
|
||||
uint16_t speed);
|
||||
|
||||
/*
|
||||
* gimbal_torque(g, enable) — enable or disable torque on both servos.
|
||||
*/
|
||||
void gimbal_torque(gimbal_t *g, bool enable);
|
||||
|
||||
/*
|
||||
* gimbal_tick(g, now_ms) — poll servo feedback at GIMBAL_TLM_HZ.
|
||||
* Call every 1 ms from the main loop; function self-throttles.
|
||||
*/
|
||||
void gimbal_tick(gimbal_t *g, uint32_t now_ms);
|
||||
|
||||
#endif /* GIMBAL_H */
|
||||
@ -32,18 +32,16 @@
|
||||
* 0x08 AUDIO - int16 PCM samples (up to 126 samples)
|
||||
* 0x09 SLEEP - no payload; request STOP-mode sleep
|
||||
* 0x0A PID_SAVE - no payload; save current Kp/Ki/Kd to flash (Issue #531)
|
||||
* 0x0B GIMBAL_POS - int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547)
|
||||
* 0x0C SCHED_GET - no payload; reply with TLM_SCHED (Issue #550)
|
||||
* 0x0D SCHED_SET - uint8 num_bands + N*16-byte pid_sched_entry_t (Issue #550)
|
||||
* 0x0E SCHED_SAVE - float kp, ki, kd (12 bytes); save sched+single to flash (Issue #550)
|
||||
*
|
||||
* STM32 to Jetson telemetry:
|
||||
* 0x80 STATUS - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
|
||||
* 0x81 POWER - jlink_tlm_power_t (11 bytes), sent at PM_TLM_HZ
|
||||
* 0x82 BATTERY - jlink_tlm_battery_t (10 bytes, Issue #533)
|
||||
* 0x83 PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531)
|
||||
* 0x84 GIMBAL_STATE - jlink_tlm_gimbal_state_t (10 bytes, Issue #547)
|
||||
* 0x85 SCHED - jlink_tlm_sched_t (1+N*16 bytes), sent on SCHED_GET (Issue #550)
|
||||
* 0x80 STATUS - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
|
||||
* 0x81 POWER - jlink_tlm_power_t (11 bytes), sent at PM_TLM_HZ
|
||||
* 0x82 BATTERY - jlink_tlm_battery_t (10 bytes, Issue #533)
|
||||
* 0x83 PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531)
|
||||
* 0x85 SCHED - jlink_tlm_sched_t (1+N*16 bytes), sent on SCHED_GET (Issue #550)
|
||||
*
|
||||
* Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
|
||||
* when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
|
||||
@ -69,7 +67,6 @@
|
||||
#define JLINK_CMD_AUDIO 0x08u /* PCM audio chunk: int16 samples, up to 126 */
|
||||
#define JLINK_CMD_SLEEP 0x09u /* no payload; request STOP-mode sleep */
|
||||
#define JLINK_CMD_PID_SAVE 0x0Au /* no payload; save Kp/Ki/Kd to flash (Issue #531) */
|
||||
#define JLINK_CMD_GIMBAL_POS 0x0Bu /* int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547) */
|
||||
#define JLINK_CMD_SCHED_GET 0x0Cu /* no payload; reply TLM_SCHED (Issue #550) */
|
||||
#define JLINK_CMD_SCHED_SET 0x0Du /* uint8 num_bands + N*16-byte entries (Issue #550) */
|
||||
#define JLINK_CMD_SCHED_SAVE 0x0Eu /* float kp,ki,kd; save sched+single to flash (Issue #550) */
|
||||
@ -79,7 +76,6 @@
|
||||
#define JLINK_TLM_POWER 0x81u /* jlink_tlm_power_t (11 bytes) */
|
||||
#define JLINK_TLM_BATTERY 0x82u /* jlink_tlm_battery_t (10 bytes, Issue #533) */
|
||||
#define JLINK_TLM_PID_RESULT 0x83u /* jlink_tlm_pid_result_t (13 bytes, Issue #531) */
|
||||
#define JLINK_TLM_GIMBAL_STATE 0x84u /* jlink_tlm_gimbal_state_t (10 bytes, Issue #547) */
|
||||
#define JLINK_TLM_SCHED 0x85u /* jlink_tlm_sched_t (1+N*16 bytes, Issue #550) */
|
||||
|
||||
/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
|
||||
@ -130,17 +126,6 @@ typedef struct __attribute__((packed)) {
|
||||
uint8_t saved_ok; /* 1 = flash write verified, 0 = write failed */
|
||||
} jlink_tlm_pid_result_t; /* 13 bytes */
|
||||
|
||||
/* ---- Telemetry GIMBAL_STATE payload (10 bytes, packed) Issue #547 ---- */
|
||||
/* Sent at GIMBAL_TLM_HZ (50 Hz); reports measured pan/tilt and speed. */
|
||||
typedef struct __attribute__((packed)) {
|
||||
int16_t pan_x10; /* Measured pan angle, deg x10 */
|
||||
int16_t tilt_x10; /* Measured tilt angle, deg x10 */
|
||||
uint16_t pan_speed_raw; /* Present speed register, pan servo */
|
||||
uint16_t tilt_speed_raw; /* Present speed register, tilt servo */
|
||||
uint8_t torque_en; /* 1 = torque enabled */
|
||||
uint8_t rx_err_pct; /* bus error rate 0-100% (rx_err*100/(rx_ok+rx_err)) */
|
||||
} jlink_tlm_gimbal_state_t; /* 10 bytes */
|
||||
|
||||
/* ---- Telemetry SCHED payload (1 + N*16 bytes, packed) Issue #550 ---- */
|
||||
/* Sent in response to JLINK_CMD_SCHED_GET; N = num_bands (1..PID_SCHED_MAX_BANDS). */
|
||||
typedef struct __attribute__((packed)) {
|
||||
@ -175,12 +160,6 @@ typedef struct {
|
||||
/* PID save request - set by JLINK_CMD_PID_SAVE, cleared by main loop (Issue #531) */
|
||||
volatile uint8_t pid_save_req;
|
||||
|
||||
/* Gimbal position command - set by JLINK_CMD_GIMBAL_POS (Issue #547) */
|
||||
volatile uint8_t gimbal_updated; /* set by parser, cleared by main loop */
|
||||
volatile int16_t gimbal_pan_x10; /* pan angle deg x10 */
|
||||
volatile int16_t gimbal_tilt_x10; /* tilt angle deg x10 */
|
||||
volatile uint16_t gimbal_speed; /* servo speed 0-4095 (0=max) */
|
||||
|
||||
/* PID schedule commands (Issue #550) - set by parser, cleared by main loop */
|
||||
volatile uint8_t sched_get_req; /* SCHED_GET: main loop calls jlink_send_sched_telemetry() */
|
||||
volatile uint8_t sched_save_req; /* SCHED_SAVE: main loop calls pid_schedule_flash_save() */
|
||||
@ -212,12 +191,6 @@ void jlink_send_power_telemetry(const jlink_tlm_power_t *power);
|
||||
void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt);
|
||||
void jlink_send_pid_result(const jlink_tlm_pid_result_t *result);
|
||||
|
||||
/*
|
||||
* jlink_send_gimbal_state(state) - transmit JLINK_TLM_GIMBAL_STATE (0x84)
|
||||
* frame (16 bytes) at GIMBAL_TLM_HZ (50 Hz). Issue #547.
|
||||
*/
|
||||
void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state);
|
||||
|
||||
/*
|
||||
* jlink_send_sched_telemetry(tlm) - transmit JLINK_TLM_SCHED (0x85) in
|
||||
* response to SCHED_GET. tlm->num_bands determines actual frame size.
|
||||
|
||||
@ -5,7 +5,7 @@
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* pid_flash — persistent PID storage for Issue #531 (auto-tune).
|
||||
* pid_flash -- persistent PID storage for Issue #531 (auto-tune).
|
||||
*
|
||||
* Stores Kp, Ki, Kd in the last 64 bytes of STM32F722 flash sector 7
|
||||
* (0x0807FFC0). Magic word validates presence of saved params.
|
||||
@ -14,14 +14,22 @@
|
||||
* Flash writes require an erase of the full sector (128KB) before re-writing.
|
||||
* The store address is the very last 64-byte block so future expansion can
|
||||
* grow toward lower addresses within sector 7 without conflict.
|
||||
*
|
||||
* Issue #550 adds pid_sched_flash_t at 0x0807FF40 (128 bytes below the PID
|
||||
* record). Both are written atomically by pid_flash_save_all() in one erase.
|
||||
*
|
||||
* Sector 7 layout:
|
||||
* 0x0807FF40 pid_sched_flash_t (128 bytes) -- schedule (Issue #550)
|
||||
* 0x0807FFC0 pid_flash_t ( 64 bytes) -- single PID (Issue #531)
|
||||
* 0x08080000 sector boundary
|
||||
*/
|
||||
|
||||
#define PID_FLASH_SECTOR FLASH_SECTOR_7
|
||||
#define PID_FLASH_SECTOR FLASH_SECTOR_7
|
||||
#define PID_FLASH_SECTOR_VOLTAGE VOLTAGE_RANGE_3 /* 2.7V-3.6V, 32-bit parallelism */
|
||||
|
||||
/* Sector 7: 128KB at 0x08060000; store in last 64 bytes */
|
||||
/* Sector 7: 128KB at 0x08060000; store single-PID in last 64 bytes */
|
||||
#define PID_FLASH_STORE_ADDR 0x0807FFC0UL
|
||||
#define PID_FLASH_MAGIC 0x534C5401UL /* 'SLT\x01' — version 1 */
|
||||
#define PID_FLASH_MAGIC 0x534C5401UL /* 'SLT\x01' -- version 1 */
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint32_t magic; /* PID_FLASH_MAGIC when valid */
|
||||
@ -32,17 +40,62 @@ typedef struct __attribute__((packed)) {
|
||||
} pid_flash_t;
|
||||
|
||||
/*
|
||||
* pid_flash_load() — read saved PID from flash.
|
||||
* pid_flash_load() -- read saved PID from flash.
|
||||
* Returns true and fills *kp/*ki/*kd if magic is valid.
|
||||
* Returns false if no valid params stored (caller keeps defaults).
|
||||
*/
|
||||
bool pid_flash_load(float *kp, float *ki, float *kd);
|
||||
|
||||
/*
|
||||
* pid_flash_save() — erase sector 7 and write Kp/Ki/Kd.
|
||||
* pid_flash_save() -- erase sector 7 and write Kp/Ki/Kd.
|
||||
* Must not be called while armed (flash erase takes ~1s and stalls the CPU).
|
||||
* Returns true on success.
|
||||
*/
|
||||
bool pid_flash_save(float kp, float ki, float kd);
|
||||
|
||||
/* ==================================================================
|
||||
* PID Gain Schedule flash storage (Issue #550)
|
||||
* ================================================================== */
|
||||
|
||||
#define PID_SCHED_MAX_BANDS 6u
|
||||
#define PID_SCHED_FLASH_ADDR 0x0807FF40UL
|
||||
#define PID_SCHED_MAGIC 0x534C5402UL /* 'SLT\x02' -- version 2 */
|
||||
|
||||
/* One speed band: 16 bytes, IEEE-754 LE floats */
|
||||
typedef struct __attribute__((packed)) {
|
||||
float speed_mps; /* breakpoint velocity (m/s, absolute) */
|
||||
float kp;
|
||||
float ki;
|
||||
float kd;
|
||||
} pid_sched_entry_t; /* 16 bytes */
|
||||
|
||||
/* Flash record: 128 bytes */
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint32_t magic; /* 4 bytes */
|
||||
uint8_t num_bands; /* 1 byte */
|
||||
uint8_t flags; /* 1 byte (reserved) */
|
||||
uint8_t _pad0[2]; /* 2 bytes */
|
||||
pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* 96 bytes */
|
||||
uint8_t _pad1[24]; /* 24 bytes */
|
||||
} pid_sched_flash_t; /* 128 bytes total */
|
||||
|
||||
/*
|
||||
* pid_flash_load_schedule() -- read gain schedule from flash.
|
||||
* Returns true and fills out_entries/out_n if magic valid and num_bands in
|
||||
* [1, PID_SCHED_MAX_BANDS]. Returns false otherwise.
|
||||
* out_entries must have room for PID_SCHED_MAX_BANDS entries.
|
||||
*/
|
||||
bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n);
|
||||
|
||||
/*
|
||||
* pid_flash_save_all() -- erase sector 7 ONCE and write both records:
|
||||
* - pid_sched_flash_t at PID_SCHED_FLASH_ADDR
|
||||
* - pid_flash_t at PID_FLASH_STORE_ADDR
|
||||
* Atomic: one sector erase covers both.
|
||||
* Must not be called while armed (erase takes ~1s).
|
||||
* Returns true on success.
|
||||
*/
|
||||
bool pid_flash_save_all(float kp_single, float ki_single, float kd_single,
|
||||
const pid_sched_entry_t *entries, uint8_t num_bands);
|
||||
|
||||
#endif /* PID_FLASH_H */
|
||||
|
||||
@ -1,97 +0,0 @@
|
||||
#ifndef SERVO_BUS_H
|
||||
#define SERVO_BUS_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* servo_bus.h — Feetech/ST3215 serial bus servo driver (Issue #547)
|
||||
*
|
||||
* Half-duplex single-wire UART protocol at 1 Mbps.
|
||||
* Hardware: USART3 half-duplex on PB10 (USART3_TX, AF7).
|
||||
* No separate RX pin — the TX line is bidirectional with HDSEL.
|
||||
*
|
||||
* Packet format (host to servo):
|
||||
* [0xFF][0xFF][ID][LEN][INSTR][PARAMS...][CKSUM]
|
||||
* CKSUM = ~(ID + LEN + INSTR + PARAMS) & 0xFF
|
||||
*
|
||||
* Response (servo to host):
|
||||
* [0xFF][0xFF][ID][LEN][ERROR][DATA...][CKSUM]
|
||||
*
|
||||
* Key ST3215 registers:
|
||||
* 0x28 Torque Enable (1=on, 0=off)
|
||||
* 0x2A Goal Position L (0-4095)
|
||||
* 0x2B Goal Position H
|
||||
* 0x2E Moving Speed L (0=max, 1-4095)
|
||||
* 0x2F Moving Speed H
|
||||
* 0x38 Present Position L (0-4095)
|
||||
* 0x39 Present Position H
|
||||
* 0x3A Present Speed L (sign+magnitude: bit15=dir)
|
||||
* 0x3B Present Speed H
|
||||
*
|
||||
* Position encoding: 0-4095 maps to 0-360 degrees.
|
||||
* Center (180 deg) = 2048 raw.
|
||||
*/
|
||||
|
||||
/* ST3215 register addresses */
|
||||
#define SB_REG_TORQUE_EN 0x28u
|
||||
#define SB_REG_GOAL_POS_L 0x2Au
|
||||
#define SB_REG_MOVING_SPD_L 0x2Eu
|
||||
#define SB_REG_PRES_POS_L 0x38u
|
||||
#define SB_REG_PRES_SPD_L 0x3Au
|
||||
|
||||
/* ST3215 instructions */
|
||||
#define SB_INSTR_PING 0x01u
|
||||
#define SB_INSTR_READ 0x02u
|
||||
#define SB_INSTR_WRITE 0x03u
|
||||
|
||||
/* Position encoding */
|
||||
#define SB_POS_CENTER 2048u /* 180 deg */
|
||||
#define SB_POS_MAX 4095u /* 360 deg */
|
||||
#define SB_SPEED_MAX 4095u /* counts/sec (0 = max speed) */
|
||||
|
||||
/* Timeout for servo response (ms) */
|
||||
#define SB_RX_TIMEOUT_MS 5u
|
||||
|
||||
/*
|
||||
* servo_bus_init() - configure USART3 in half-duplex mode (PB10, AF7) at
|
||||
* SB_BAUD (1 Mbps, 8N1). Call once at startup before gimbal_init().
|
||||
*/
|
||||
void servo_bus_init(void);
|
||||
|
||||
/*
|
||||
* servo_bus_write_pos(id, raw_pos, speed) - write goal position and moving
|
||||
* speed in a single WRITE DATA packet. raw_pos: 0-4095. speed: 0=max, 1-4095.
|
||||
* Returns true on successful TX (no response expected on write).
|
||||
*/
|
||||
bool servo_bus_write_pos(uint8_t id, uint16_t raw_pos, uint16_t speed);
|
||||
|
||||
/*
|
||||
* servo_bus_write_torque(id, enable) - enable or disable servo torque.
|
||||
*/
|
||||
bool servo_bus_write_torque(uint8_t id, bool enable);
|
||||
|
||||
/*
|
||||
* servo_bus_read_pos(id, raw_pos) - read present position.
|
||||
* Returns true on success; raw_pos is 0-4095.
|
||||
*/
|
||||
bool servo_bus_read_pos(uint8_t id, uint16_t *raw_pos);
|
||||
|
||||
/*
|
||||
* servo_bus_read_speed(id, speed) - read present speed.
|
||||
* speed bit 15 is direction. Returns magnitude in lower 15 bits.
|
||||
*/
|
||||
bool servo_bus_read_speed(uint8_t id, uint16_t *speed);
|
||||
|
||||
/*
|
||||
* servo_bus_deg_to_raw(deg) - convert degree (-180..+180) to raw position.
|
||||
* Center (0 deg) = 2048. Clamps to 0-4095.
|
||||
*/
|
||||
uint16_t servo_bus_deg_to_raw(float deg);
|
||||
|
||||
/*
|
||||
* servo_bus_raw_to_deg(raw) - convert raw position (0-4095) to degree (-180..+180).
|
||||
*/
|
||||
float servo_bus_raw_to_deg(uint16_t raw);
|
||||
|
||||
#endif /* SERVO_BUS_H */
|
||||
@ -1,29 +0,0 @@
|
||||
gimbal_node:
|
||||
ros__parameters:
|
||||
# Serial port connecting to STM32 over JLINK protocol
|
||||
serial_port: "/dev/ttyTHS1"
|
||||
baud_rate: 921600
|
||||
|
||||
# Soft angle limits (degrees, ± from center)
|
||||
pan_limit_deg: 150.0
|
||||
tilt_limit_deg: 45.0
|
||||
|
||||
# Home position (degrees from center)
|
||||
home_pan_deg: 0.0
|
||||
home_tilt_deg: 0.0
|
||||
|
||||
# Motion profile
|
||||
max_speed_deg_s: 90.0 # maximum pan/tilt speed (°/s)
|
||||
accel_deg_s2: 180.0 # trapezoidal acceleration (°/s²)
|
||||
|
||||
# Update rates
|
||||
update_rate_hz: 20.0 # motion tick + JLINK TX rate
|
||||
state_publish_hz: 10.0 # /saltybot/gimbal/state publish rate
|
||||
|
||||
# Serial reconnect
|
||||
reconnect_delay_s: 2.0
|
||||
|
||||
# D435i camera parameters (for look_at 3D→pan/tilt projection)
|
||||
camera_focal_length_px: 600.0
|
||||
image_width_px: 848
|
||||
image_height_px: 480
|
||||
@ -1,50 +0,0 @@
|
||||
"""gimbal.launch.py — Launch saltybot_gimbal node (Issue #548)."""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
pkg = get_package_share_directory("saltybot_gimbal")
|
||||
|
||||
serial_port_arg = DeclareLaunchArgument(
|
||||
"serial_port",
|
||||
default_value="/dev/ttyTHS1",
|
||||
description="JLINK serial port to STM32",
|
||||
)
|
||||
pan_limit_arg = DeclareLaunchArgument(
|
||||
"pan_limit_deg",
|
||||
default_value="150.0",
|
||||
description="Pan soft limit ± degrees",
|
||||
)
|
||||
tilt_limit_arg = DeclareLaunchArgument(
|
||||
"tilt_limit_deg",
|
||||
default_value="45.0",
|
||||
description="Tilt soft limit ± degrees",
|
||||
)
|
||||
|
||||
gimbal_node = Node(
|
||||
package="saltybot_gimbal",
|
||||
executable="gimbal_node",
|
||||
name="gimbal_node",
|
||||
output="screen",
|
||||
parameters=[
|
||||
os.path.join(pkg, "config", "gimbal_params.yaml"),
|
||||
{
|
||||
"serial_port": LaunchConfiguration("serial_port"),
|
||||
"pan_limit_deg": LaunchConfiguration("pan_limit_deg"),
|
||||
"tilt_limit_deg": LaunchConfiguration("tilt_limit_deg"),
|
||||
},
|
||||
],
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
serial_port_arg,
|
||||
pan_limit_arg,
|
||||
tilt_limit_arg,
|
||||
gimbal_node,
|
||||
])
|
||||
@ -1,21 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>saltybot_gimbal</name>
|
||||
<version>1.0.0</version>
|
||||
<description>
|
||||
ROS2 gimbal control node: pan/tilt camera head via JLINK serial to STM32.
|
||||
Smooth trapezoidal motion profiles, configurable limits, look_at 3D projection.
|
||||
Issue #548.
|
||||
</description>
|
||||
<maintainer email="sl-jetson@saltylab.local">sl-jetson</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>std_srvs</depend>
|
||||
|
||||
<test_depend>pytest</test_depend>
|
||||
</package>
|
||||
@ -1,497 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""gimbal_node.py — ROS2 gimbal control node for SaltyBot pan/tilt camera head (Issue #548).
|
||||
|
||||
Controls pan/tilt gimbal via JLINK binary protocol over serial to STM32.
|
||||
Implements smooth trapezoidal motion profiles with configurable axis limits.
|
||||
|
||||
Subscribed topics:
|
||||
/saltybot/gimbal/cmd (geometry_msgs/Vector3) x=pan_deg, y=tilt_deg, z=speed_deg_s
|
||||
/saltybot/gimbal/look_at_target (geometry_msgs/PointStamped) 3D look-at target (camera frame)
|
||||
|
||||
Published topics:
|
||||
/saltybot/gimbal/state (std_msgs/String) JSON: pan_deg, tilt_deg, moving, fault, …
|
||||
/saltybot/gimbal/cmd_echo (geometry_msgs/Vector3) current target cmd (monitoring)
|
||||
|
||||
Services:
|
||||
/saltybot/gimbal/home (std_srvs/Trigger) command gimbal to home position
|
||||
/saltybot/gimbal/look_at (std_srvs/Trigger) execute look-at to last look_at_target
|
||||
|
||||
Parameters (config/gimbal_params.yaml):
|
||||
serial_port /dev/ttyTHS1 JLINK serial device
|
||||
baud_rate 921600
|
||||
pan_limit_deg 150.0 soft limit ± degrees
|
||||
tilt_limit_deg 45.0 soft limit ± degrees
|
||||
home_pan_deg 0.0 home pan position
|
||||
home_tilt_deg 0.0 home tilt position
|
||||
max_speed_deg_s 90.0 default motion speed
|
||||
accel_deg_s2 180.0 trapezoidal acceleration
|
||||
update_rate_hz 20.0 motion profile update rate
|
||||
state_publish_hz 10.0 /saltybot/gimbal/state publish rate
|
||||
reconnect_delay_s 2.0 serial reconnect interval
|
||||
camera_focal_length_px 600.0 D435i focal length (for look_at projection)
|
||||
image_width_px 848
|
||||
image_height_px 480
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import threading
|
||||
import time
|
||||
from typing import Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
import serial
|
||||
|
||||
from geometry_msgs.msg import PointStamped, Vector3
|
||||
from std_msgs.msg import String
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
from .jlink_gimbal import (
|
||||
encode_gimbal_pos,
|
||||
encode_gimbal_home,
|
||||
decode_gimbal_state,
|
||||
TLM_GIMBAL_STATE,
|
||||
GimbalStateFrame,
|
||||
STX, ETX,
|
||||
_crc16_ccitt,
|
||||
)
|
||||
|
||||
|
||||
def _clamp(v: float, lo: float, hi: float) -> float:
|
||||
return max(lo, min(hi, v))
|
||||
|
||||
|
||||
class MotionAxis:
|
||||
"""Single-axis trapezoidal velocity profile.
|
||||
|
||||
Tracks current position and velocity; each tick() call advances the
|
||||
axis toward the target using bounded acceleration and velocity.
|
||||
"""
|
||||
|
||||
def __init__(self, initial_pos: float, max_speed: float, accel: float) -> None:
|
||||
self.pos = initial_pos
|
||||
self.vel = 0.0
|
||||
self.target = initial_pos
|
||||
self.max_speed = max_speed
|
||||
self.accel = accel
|
||||
|
||||
def set_target(self, target: float, speed: Optional[float] = None) -> None:
|
||||
self.target = target
|
||||
if speed is not None:
|
||||
self.max_speed = max(1.0, speed)
|
||||
|
||||
def tick(self, dt: float) -> float:
|
||||
"""Advance one timestep. Returns new position."""
|
||||
error = self.target - self.pos
|
||||
if abs(error) < 0.01:
|
||||
self.pos = self.target
|
||||
self.vel = 0.0
|
||||
return self.pos
|
||||
|
||||
# Deceleration distance at current speed: v²/(2a)
|
||||
decel_dist = (self.vel ** 2) / (2.0 * self.accel) if self.accel > 0 else 0.0
|
||||
direction = 1.0 if error > 0 else -1.0
|
||||
|
||||
if abs(error) <= decel_dist + 0.01:
|
||||
# Deceleration phase
|
||||
desired_vel = direction * math.sqrt(max(0.0, 2.0 * self.accel * abs(error)))
|
||||
else:
|
||||
# Acceleration/cruise phase
|
||||
desired_vel = direction * self.max_speed
|
||||
|
||||
# Apply acceleration limit
|
||||
dv = desired_vel - self.vel
|
||||
dv = _clamp(dv, -self.accel * dt, self.accel * dt)
|
||||
self.vel += dv
|
||||
self.vel = _clamp(self.vel, -self.max_speed, self.max_speed)
|
||||
|
||||
self.pos += self.vel * dt
|
||||
# Don't overshoot
|
||||
if direction > 0 and self.pos > self.target:
|
||||
self.pos = self.target
|
||||
self.vel = 0.0
|
||||
elif direction < 0 and self.pos < self.target:
|
||||
self.pos = self.target
|
||||
self.vel = 0.0
|
||||
|
||||
return self.pos
|
||||
|
||||
@property
|
||||
def is_moving(self) -> bool:
|
||||
return abs(self.target - self.pos) > 0.05 or abs(self.vel) > 0.05
|
||||
|
||||
|
||||
class JLinkGimbalSerial:
|
||||
"""Serial connection manager for JLINK gimbal frames.
|
||||
|
||||
Handles connect/disconnect/reconnect and provides send() and
|
||||
read_state() methods. Thread-safe via internal lock.
|
||||
"""
|
||||
|
||||
_PARSER_WAIT_STX = 0
|
||||
_PARSER_WAIT_CMD = 1
|
||||
_PARSER_WAIT_LEN = 2
|
||||
_PARSER_PAYLOAD = 3
|
||||
_PARSER_CRC_HI = 4
|
||||
_PARSER_CRC_LO = 5
|
||||
_PARSER_WAIT_ETX = 6
|
||||
|
||||
def __init__(self, port: str, baud: int) -> None:
|
||||
self._port = port
|
||||
self._baud = baud
|
||||
self._ser: Optional[serial.Serial] = None
|
||||
self._lock = threading.Lock()
|
||||
self._parser_state = self._PARSER_WAIT_STX
|
||||
self._parser_cmd = 0
|
||||
self._parser_len = 0
|
||||
self._parser_payload = bytearray()
|
||||
self._parser_crc_rcvd = 0
|
||||
|
||||
def connect(self) -> bool:
|
||||
try:
|
||||
self._ser = serial.Serial(
|
||||
port=self._port,
|
||||
baudrate=self._baud,
|
||||
timeout=0.05,
|
||||
bytesize=8,
|
||||
parity=serial.PARITY_NONE,
|
||||
stopbits=1,
|
||||
)
|
||||
self._reset_parser()
|
||||
return True
|
||||
except Exception as e:
|
||||
print(f"[gimbal] serial connect failed ({self._port}): {e}")
|
||||
self._ser = None
|
||||
return False
|
||||
|
||||
def disconnect(self) -> None:
|
||||
with self._lock:
|
||||
if self._ser:
|
||||
self._ser.close()
|
||||
self._ser = None
|
||||
|
||||
def is_connected(self) -> bool:
|
||||
return self._ser is not None and self._ser.is_open
|
||||
|
||||
def send(self, frame: bytes) -> bool:
|
||||
with self._lock:
|
||||
if not self.is_connected():
|
||||
return False
|
||||
try:
|
||||
self._ser.write(frame)
|
||||
return True
|
||||
except Exception as e:
|
||||
print(f"[gimbal] serial write error: {e}")
|
||||
self._ser = None
|
||||
return False
|
||||
|
||||
def read_pending(self) -> Optional[GimbalStateFrame]:
|
||||
"""Drain RX buffer; return GimbalStateFrame if a complete telemetry
|
||||
frame was received, or None if no complete frame is available."""
|
||||
with self._lock:
|
||||
if not self.is_connected():
|
||||
return None
|
||||
try:
|
||||
raw = self._ser.read(self._ser.in_waiting or 1)
|
||||
except Exception:
|
||||
self._ser = None
|
||||
return None
|
||||
|
||||
result = None
|
||||
for b in raw:
|
||||
frame = self._feed(b)
|
||||
if frame is not None:
|
||||
result = frame # keep last complete frame
|
||||
return result
|
||||
|
||||
def _reset_parser(self) -> None:
|
||||
self._parser_state = self._PARSER_WAIT_STX
|
||||
self._parser_payload = bytearray()
|
||||
|
||||
def _feed(self, byte: int) -> Optional[GimbalStateFrame]:
|
||||
s = self._parser_state
|
||||
|
||||
if s == self._PARSER_WAIT_STX:
|
||||
if byte == STX:
|
||||
self._parser_state = self._PARSER_WAIT_CMD
|
||||
return None
|
||||
|
||||
if s == self._PARSER_WAIT_CMD:
|
||||
self._parser_cmd = byte
|
||||
self._parser_state = self._PARSER_WAIT_LEN
|
||||
return None
|
||||
|
||||
if s == self._PARSER_WAIT_LEN:
|
||||
self._parser_len = byte
|
||||
self._parser_payload = bytearray()
|
||||
if byte > 64:
|
||||
self._reset_parser()
|
||||
return None
|
||||
self._parser_state = self._PARSER_PAYLOAD if byte > 0 else self._PARSER_CRC_HI
|
||||
return None
|
||||
|
||||
if s == self._PARSER_PAYLOAD:
|
||||
self._parser_payload.append(byte)
|
||||
if len(self._parser_payload) == self._parser_len:
|
||||
self._parser_state = self._PARSER_CRC_HI
|
||||
return None
|
||||
|
||||
if s == self._PARSER_CRC_HI:
|
||||
self._parser_crc_rcvd = byte << 8
|
||||
self._parser_state = self._PARSER_CRC_LO
|
||||
return None
|
||||
|
||||
if s == self._PARSER_CRC_LO:
|
||||
self._parser_crc_rcvd |= byte
|
||||
self._parser_state = self._PARSER_WAIT_ETX
|
||||
return None
|
||||
|
||||
if s == self._PARSER_WAIT_ETX:
|
||||
self._reset_parser()
|
||||
if byte != ETX:
|
||||
return None
|
||||
crc_data = bytes([self._parser_cmd, self._parser_len]) + self._parser_payload
|
||||
if _crc16_ccitt(crc_data) != self._parser_crc_rcvd:
|
||||
return None
|
||||
if self._parser_cmd == TLM_GIMBAL_STATE:
|
||||
return decode_gimbal_state(bytes(self._parser_payload))
|
||||
return None
|
||||
|
||||
self._reset_parser()
|
||||
return None
|
||||
|
||||
|
||||
class GimbalNode(Node):
|
||||
"""ROS2 gimbal control node: smooth motion profiles + JLINK serial bridge."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("gimbal_node")
|
||||
|
||||
# ── Parameters ─────────────────────────────────────────────────────
|
||||
self.declare_parameter("serial_port", "/dev/ttyTHS1")
|
||||
self.declare_parameter("baud_rate", 921600)
|
||||
self.declare_parameter("pan_limit_deg", 150.0)
|
||||
self.declare_parameter("tilt_limit_deg", 45.0)
|
||||
self.declare_parameter("home_pan_deg", 0.0)
|
||||
self.declare_parameter("home_tilt_deg", 0.0)
|
||||
self.declare_parameter("max_speed_deg_s", 90.0)
|
||||
self.declare_parameter("accel_deg_s2", 180.0)
|
||||
self.declare_parameter("update_rate_hz", 20.0)
|
||||
self.declare_parameter("state_publish_hz", 10.0)
|
||||
self.declare_parameter("reconnect_delay_s", 2.0)
|
||||
self.declare_parameter("camera_focal_length_px", 600.0)
|
||||
self.declare_parameter("image_width_px", 848)
|
||||
self.declare_parameter("image_height_px", 480)
|
||||
|
||||
self._port = self.get_parameter("serial_port").value
|
||||
self._baud = self.get_parameter("baud_rate").value
|
||||
self._pan_limit = self.get_parameter("pan_limit_deg").value
|
||||
self._tilt_limit = self.get_parameter("tilt_limit_deg").value
|
||||
self._home_pan = self.get_parameter("home_pan_deg").value
|
||||
self._home_tilt = self.get_parameter("home_tilt_deg").value
|
||||
self._max_speed = self.get_parameter("max_speed_deg_s").value
|
||||
self._accel = self.get_parameter("accel_deg_s2").value
|
||||
update_hz = self.get_parameter("update_rate_hz").value
|
||||
state_hz = self.get_parameter("state_publish_hz").value
|
||||
self._reconnect_del = self.get_parameter("reconnect_delay_s").value
|
||||
self._focal_px = self.get_parameter("camera_focal_length_px").value
|
||||
self._img_w = self.get_parameter("image_width_px").value
|
||||
self._img_h = self.get_parameter("image_height_px").value
|
||||
|
||||
# ── State ──────────────────────────────────────────────────────────
|
||||
self._pan_axis = MotionAxis(self._home_pan, self._max_speed, self._accel)
|
||||
self._tilt_axis = MotionAxis(self._home_tilt, self._max_speed, self._accel)
|
||||
|
||||
self._hw_state: Optional[GimbalStateFrame] = None
|
||||
self._look_at_target: Optional[PointStamped] = None
|
||||
self._last_cmd_time = time.monotonic()
|
||||
self._reconnect_ts = 0.0
|
||||
self._state_lock = threading.Lock()
|
||||
|
||||
# ── Serial ─────────────────────────────────────────────────────────
|
||||
self._serial = JLinkGimbalSerial(self._port, self._baud)
|
||||
if not self._serial.connect():
|
||||
self.get_logger().warn(
|
||||
f"[gimbal] serial not available on {self._port} — will retry"
|
||||
)
|
||||
|
||||
# ── Subscribers ────────────────────────────────────────────────────
|
||||
qos = QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
)
|
||||
self.create_subscription(Vector3, "/saltybot/gimbal/cmd",
|
||||
self._on_cmd, qos)
|
||||
self.create_subscription(PointStamped, "/saltybot/gimbal/look_at_target",
|
||||
self._on_look_at_target, 10)
|
||||
|
||||
# ── Publishers ─────────────────────────────────────────────────────
|
||||
self._pub_state = self.create_publisher(String, "/saltybot/gimbal/state", 10)
|
||||
self._pub_cmd_echo = self.create_publisher(Vector3, "/saltybot/gimbal/cmd_echo", 10)
|
||||
|
||||
# ── Services ───────────────────────────────────────────────────────
|
||||
self.create_service(Trigger, "/saltybot/gimbal/home", self._svc_home)
|
||||
self.create_service(Trigger, "/saltybot/gimbal/look_at", self._svc_look_at)
|
||||
|
||||
# ── Timers ─────────────────────────────────────────────────────────
|
||||
self.create_timer(1.0 / update_hz, self._motion_tick)
|
||||
self.create_timer(1.0 / state_hz, self._publish_state)
|
||||
|
||||
self.get_logger().info(
|
||||
f"[gimbal] node ready — port={self._port} "
|
||||
f"pan=±{self._pan_limit}° tilt=±{self._tilt_limit}° "
|
||||
f"speed={self._max_speed}°/s accel={self._accel}°/s²"
|
||||
)
|
||||
|
||||
# ── Subscriber callbacks ───────────────────────────────────────────────
|
||||
|
||||
def _on_cmd(self, msg: Vector3) -> None:
|
||||
"""Handle /saltybot/gimbal/cmd (x=pan, y=tilt, z=speed_deg_s)."""
|
||||
pan = _clamp(msg.x, -self._pan_limit, self._pan_limit)
|
||||
tilt = _clamp(msg.y, -self._tilt_limit, self._tilt_limit)
|
||||
speed = msg.z if msg.z > 0.0 else self._max_speed
|
||||
speed = _clamp(speed, 1.0, self._max_speed)
|
||||
with self._state_lock:
|
||||
self._pan_axis.set_target(pan, speed)
|
||||
self._tilt_axis.set_target(tilt, speed)
|
||||
self._last_cmd_time = time.monotonic()
|
||||
|
||||
def _on_look_at_target(self, msg: PointStamped) -> None:
|
||||
"""Cache look-at 3D target for use by /look_at service."""
|
||||
self._look_at_target = msg
|
||||
|
||||
# ── Service callbacks ──────────────────────────────────────────────────
|
||||
|
||||
def _svc_home(self, _req: Trigger.Request, resp: Trigger.Response) -> Trigger.Response:
|
||||
"""Return gimbal to home position."""
|
||||
with self._state_lock:
|
||||
self._pan_axis.set_target(self._home_pan, self._max_speed)
|
||||
self._tilt_axis.set_target(self._home_tilt, self._max_speed)
|
||||
self._serial.send(encode_gimbal_home())
|
||||
resp.success = True
|
||||
resp.message = f"Homing to pan={self._home_pan}° tilt={self._home_tilt}°"
|
||||
self.get_logger().info("[gimbal] home command")
|
||||
return resp
|
||||
|
||||
def _svc_look_at(self, _req: Trigger.Request, resp: Trigger.Response) -> Trigger.Response:
|
||||
"""Convert last look_at_target point to pan/tilt and command motion."""
|
||||
tgt = self._look_at_target
|
||||
if tgt is None:
|
||||
resp.success = False
|
||||
resp.message = "No look_at_target received on /saltybot/gimbal/look_at_target"
|
||||
return resp
|
||||
|
||||
# Project 3D point (camera frame) → pan/tilt angles.
|
||||
# Assumes point is in camera_depth_optical_frame (Z=forward, X=right, Y=down).
|
||||
x, y, z = tgt.point.x, tgt.point.y, tgt.point.z
|
||||
if z <= 0.0:
|
||||
resp.success = False
|
||||
resp.message = f"Invalid look_at_target: z={z:.3f} (must be > 0)"
|
||||
return resp
|
||||
|
||||
pan_deg = math.degrees(math.atan2(x, z))
|
||||
tilt_deg = -math.degrees(math.atan2(y, z)) # negative: down in image = tilt up
|
||||
|
||||
pan_deg = _clamp(pan_deg, -self._pan_limit, self._pan_limit)
|
||||
tilt_deg = _clamp(tilt_deg, -self._tilt_limit, self._tilt_limit)
|
||||
|
||||
with self._state_lock:
|
||||
self._pan_axis.set_target(pan_deg, self._max_speed)
|
||||
self._tilt_axis.set_target(tilt_deg, self._max_speed)
|
||||
|
||||
resp.success = True
|
||||
resp.message = (
|
||||
f"Looking at ({x:.2f}, {y:.2f}, {z:.2f}) → "
|
||||
f"pan={pan_deg:.1f}° tilt={tilt_deg:.1f}°"
|
||||
)
|
||||
self.get_logger().info(f"[gimbal] look_at: {resp.message}")
|
||||
return resp
|
||||
|
||||
# ── Motion tick ────────────────────────────────────────────────────────
|
||||
|
||||
def _motion_tick(self) -> None:
|
||||
"""Advance motion profiles and send JLINK frame at update_rate_hz."""
|
||||
# Reconnect if needed
|
||||
if not self._serial.is_connected():
|
||||
now = time.monotonic()
|
||||
if now - self._reconnect_ts >= self._reconnect_del:
|
||||
self._reconnect_ts = now
|
||||
if self._serial.connect():
|
||||
self.get_logger().info(f"[gimbal] serial reconnected on {self._port}")
|
||||
|
||||
dt = 1.0 / self.get_parameter("update_rate_hz").value
|
||||
|
||||
with self._state_lock:
|
||||
pan = self._pan_axis.tick(dt)
|
||||
tilt = self._tilt_axis.tick(dt)
|
||||
moving = self._pan_axis.is_moving or self._tilt_axis.is_moving
|
||||
speed = max(self._pan_axis.max_speed, self._tilt_axis.max_speed)
|
||||
|
||||
# Send JLINK frame
|
||||
frame = encode_gimbal_pos(pan, tilt, speed)
|
||||
self._serial.send(frame)
|
||||
|
||||
# Read telemetry
|
||||
hw = self._serial.read_pending()
|
||||
if hw is not None:
|
||||
with self._state_lock:
|
||||
self._hw_state = hw
|
||||
|
||||
# Echo command
|
||||
echo = Vector3(x=pan, y=tilt, z=speed)
|
||||
self._pub_cmd_echo.publish(echo)
|
||||
|
||||
# ── State publisher ────────────────────────────────────────────────────
|
||||
|
||||
def _publish_state(self) -> None:
|
||||
"""Publish gimbal state as JSON string."""
|
||||
with self._state_lock:
|
||||
pan = self._pan_axis.pos
|
||||
tilt = self._tilt_axis.pos
|
||||
pan_t = self._pan_axis.target
|
||||
tilt_t = self._tilt_axis.target
|
||||
moving = self._pan_axis.is_moving or self._tilt_axis.is_moving
|
||||
hw = self._hw_state
|
||||
|
||||
state: dict = {
|
||||
"pan_deg": round(pan, 2),
|
||||
"tilt_deg": round(tilt, 2),
|
||||
"pan_target_deg": round(pan_t, 2),
|
||||
"tilt_target_deg": round(tilt_t, 2),
|
||||
"moving": moving,
|
||||
"serial_ok": self._serial.is_connected(),
|
||||
}
|
||||
if hw is not None:
|
||||
state["hw_pan_deg"] = round(hw.pan_deg, 1)
|
||||
state["hw_tilt_deg"] = round(hw.tilt_deg, 1)
|
||||
state["hw_pan_speed_raw"] = hw.pan_speed_raw
|
||||
state["hw_tilt_speed_raw"] = hw.tilt_speed_raw
|
||||
state["hw_torque_en"] = hw.torque_en
|
||||
state["hw_rx_err_pct"] = hw.rx_err_pct
|
||||
|
||||
self._pub_state.publish(String(data=json.dumps(state)))
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
self._serial.disconnect()
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = GimbalNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,141 +0,0 @@
|
||||
"""jlink_gimbal.py — JLINK binary frame codec for gimbal commands (Issue #548).
|
||||
|
||||
Matches the JLINK protocol defined in include/jlink.h (Issue #547 STM32 side).
|
||||
|
||||
Command type (Jetson → STM32):
|
||||
0x0B GIMBAL_POS — int16 pan_x10 + int16 tilt_x10 + uint16 speed (6 bytes)
|
||||
pan_x10 = pan_deg * 10 (±1500 for ±150°)
|
||||
tilt_x10 = tilt_deg * 10 (±450 for ±45°)
|
||||
speed = servo speed register 0–4095 (0 = max)
|
||||
|
||||
Telemetry type (STM32 → Jetson):
|
||||
0x84 GIMBAL_STATE — int16 pan_x10 + int16 tilt_x10 +
|
||||
uint16 pan_speed_raw + uint16 tilt_speed_raw +
|
||||
uint8 torque_en + uint8 rx_err_pct (10 bytes)
|
||||
|
||||
Frame format (shared with stm32_protocol.py):
|
||||
[STX=0x02][CMD][LEN][PAYLOAD...][CRC16_hi][CRC16_lo][ETX=0x03]
|
||||
CRC16-CCITT: poly=0x1021, init=0xFFFF, covers CMD+LEN+PAYLOAD bytes.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import struct
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
# ── Frame constants ────────────────────────────────────────────────────────────
|
||||
|
||||
STX = 0x02
|
||||
ETX = 0x03
|
||||
|
||||
# ── Command / telemetry type codes ─────────────────────────────────────────────
|
||||
|
||||
CMD_GIMBAL_POS = 0x0B # Jetson → STM32: set pan/tilt target
|
||||
TLM_GIMBAL_STATE = 0x84 # STM32 → Jetson: measured state
|
||||
|
||||
# Speed register: 0 = maximum servo speed; 4095 = slowest non-zero speed.
|
||||
# Map deg/s to this register: speed_reg = max(0, 4095 - int(deg_s * 4095 / 360))
|
||||
_MAX_SPEED_DEGS = 360.0 # degrees/sec at speed_reg=0
|
||||
|
||||
|
||||
# ── Parsed telemetry ───────────────────────────────────────────────────────────
|
||||
|
||||
@dataclass
|
||||
class GimbalStateFrame:
|
||||
pan_deg: float # measured pan position (degrees, + = right)
|
||||
tilt_deg: float # measured tilt position (degrees, + = up)
|
||||
pan_speed_raw: int # pan servo present-speed register value
|
||||
tilt_speed_raw: int # tilt servo present-speed register value
|
||||
torque_en: bool # True = servo torque is on
|
||||
rx_err_pct: int # SCS bus error rate 0–100 %
|
||||
|
||||
|
||||
# ── CRC16-CCITT ────────────────────────────────────────────────────────────────
|
||||
|
||||
def _crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int:
|
||||
"""CRC16-CCITT: poly=0x1021, init=0xFFFF, no final XOR."""
|
||||
crc = init
|
||||
for byte in data:
|
||||
crc ^= byte << 8
|
||||
for _ in range(8):
|
||||
crc = ((crc << 1) ^ 0x1021) if (crc & 0x8000) else (crc << 1)
|
||||
crc &= 0xFFFF
|
||||
return crc
|
||||
|
||||
|
||||
# ── Frame builder ──────────────────────────────────────────────────────────────
|
||||
|
||||
def _build_frame(cmd: int, payload: bytes) -> bytes:
|
||||
length = len(payload)
|
||||
header = bytes([cmd, length])
|
||||
crc = _crc16_ccitt(header + payload)
|
||||
return bytes([STX, cmd, length]) + payload + struct.pack(">H", crc) + bytes([ETX])
|
||||
|
||||
|
||||
# ── Speed conversion ───────────────────────────────────────────────────────────
|
||||
|
||||
def degs_to_speed_reg(deg_s: float) -> int:
|
||||
"""Convert degrees/second to servo speed register value.
|
||||
|
||||
speed_reg=0 is maximum speed; speed_reg=4095 is the slowest non-zero.
|
||||
Linear mapping: 0 deg/s → 0 (max), 360 deg/s → 0, clamped.
|
||||
"""
|
||||
if deg_s <= 0.0:
|
||||
return 0 # max speed
|
||||
ratio = min(1.0, deg_s / _MAX_SPEED_DEGS)
|
||||
return max(0, min(4095, int((1.0 - ratio) * 4095)))
|
||||
|
||||
|
||||
def speed_reg_to_degs(reg: int) -> float:
|
||||
"""Convert speed register to approximate degrees/second."""
|
||||
if reg == 0:
|
||||
return _MAX_SPEED_DEGS
|
||||
return (1.0 - reg / 4095.0) * _MAX_SPEED_DEGS
|
||||
|
||||
|
||||
# ── Encoder ────────────────────────────────────────────────────────────────────
|
||||
|
||||
def encode_gimbal_pos(pan_deg: float, tilt_deg: float, speed_deg_s: float) -> bytes:
|
||||
"""Build GIMBAL_POS frame.
|
||||
|
||||
Args:
|
||||
pan_deg: Pan angle (degrees, clamped to ±327°).
|
||||
tilt_deg: Tilt angle (degrees, clamped to ±327°).
|
||||
speed_deg_s: Motion speed (degrees/second; 0 = max).
|
||||
|
||||
Returns:
|
||||
Complete JLINK binary frame (10 bytes total).
|
||||
"""
|
||||
pan_x10 = max(-32767, min(32767, int(round(pan_deg * 10))))
|
||||
tilt_x10 = max(-32767, min(32767, int(round(tilt_deg * 10))))
|
||||
speed = degs_to_speed_reg(speed_deg_s)
|
||||
return _build_frame(CMD_GIMBAL_POS, struct.pack(">hhH", pan_x10, tilt_x10, speed))
|
||||
|
||||
|
||||
def encode_gimbal_home() -> bytes:
|
||||
"""Build GIMBAL_POS frame targeting (0°, 0°) at default speed."""
|
||||
return encode_gimbal_pos(0.0, 0.0, 0.0) # speed=0 → max speed for homing
|
||||
|
||||
|
||||
# ── Decoder ────────────────────────────────────────────────────────────────────
|
||||
|
||||
def decode_gimbal_state(payload: bytes) -> Optional[GimbalStateFrame]:
|
||||
"""Decode GIMBAL_STATE telemetry payload (10 bytes).
|
||||
|
||||
Returns:
|
||||
GimbalStateFrame on success, None if payload is too short.
|
||||
"""
|
||||
if len(payload) < 10:
|
||||
return None
|
||||
pan_x10, tilt_x10, pan_spd, tilt_spd, torque_en, rx_err_pct = (
|
||||
struct.unpack_from(">hhHHBB", payload)
|
||||
)
|
||||
return GimbalStateFrame(
|
||||
pan_deg=pan_x10 / 10.0,
|
||||
tilt_deg=tilt_x10 / 10.0,
|
||||
pan_speed_raw=pan_spd,
|
||||
tilt_speed_raw=tilt_spd,
|
||||
torque_en=bool(torque_en),
|
||||
rx_err_pct=int(rx_err_pct),
|
||||
)
|
||||
@ -1,2 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_gimbal/scripts
|
||||
@ -1,23 +0,0 @@
|
||||
from setuptools import setup
|
||||
|
||||
package_name = 'saltybot_gimbal'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='1.0.0',
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
('share/' + package_name + '/launch', ['launch/gimbal.launch.py']),
|
||||
('share/' + package_name + '/config', ['config/gimbal_params.yaml']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
author='Salty Lab',
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'gimbal_node = saltybot_gimbal.gimbal_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,422 +0,0 @@
|
||||
"""test_gimbal.py — Unit tests for saltybot_gimbal (Issue #548).
|
||||
|
||||
Runs without hardware: all serial I/O is mocked.
|
||||
"""
|
||||
|
||||
import math
|
||||
import struct
|
||||
import sys
|
||||
import types
|
||||
import unittest
|
||||
from unittest.mock import MagicMock
|
||||
|
||||
# ── Stub ROS2 / serial so tests run offline ────────────────────────────────────
|
||||
|
||||
def _make_rclpy_stub():
|
||||
rclpy = types.ModuleType("rclpy")
|
||||
rclpy.init = lambda **_: None
|
||||
rclpy.spin = lambda _: None
|
||||
rclpy.shutdown = lambda: None
|
||||
|
||||
node_mod = types.ModuleType("rclpy.node")
|
||||
class _Node:
|
||||
def __init__(self, *a, **kw): pass
|
||||
def declare_parameter(self, *a, **kw): pass
|
||||
def get_parameter(self, name):
|
||||
defaults = {
|
||||
"serial_port": "/dev/null", "baud_rate": 921600,
|
||||
"pan_limit_deg": 150.0, "tilt_limit_deg": 45.0,
|
||||
"home_pan_deg": 0.0, "home_tilt_deg": 0.0,
|
||||
"max_speed_deg_s": 90.0, "accel_deg_s2": 180.0,
|
||||
"update_rate_hz": 20.0, "state_publish_hz": 10.0,
|
||||
"reconnect_delay_s": 2.0,
|
||||
"camera_focal_length_px": 600.0,
|
||||
"image_width_px": 848, "image_height_px": 480,
|
||||
}
|
||||
m = MagicMock()
|
||||
m.value = defaults.get(name, 0)
|
||||
return m
|
||||
def get_logger(self): return MagicMock()
|
||||
def create_subscription(self, *a, **kw): pass
|
||||
def create_publisher(self, *a, **kw): return MagicMock()
|
||||
def create_service(self, *a, **kw): pass
|
||||
def create_timer(self, *a, **kw): pass
|
||||
def destroy_node(self): pass
|
||||
node_mod.Node = _Node
|
||||
rclpy.node = node_mod
|
||||
|
||||
qos_mod = types.ModuleType("rclpy.qos")
|
||||
qos_mod.QoSProfile = MagicMock(return_value=None)
|
||||
qos_mod.HistoryPolicy = MagicMock()
|
||||
qos_mod.ReliabilityPolicy = MagicMock()
|
||||
rclpy.qos = qos_mod
|
||||
|
||||
return rclpy
|
||||
|
||||
|
||||
def _install_stubs():
|
||||
rclpy_stub = _make_rclpy_stub()
|
||||
sys.modules.setdefault("rclpy", rclpy_stub)
|
||||
sys.modules.setdefault("rclpy.node", rclpy_stub.node)
|
||||
sys.modules.setdefault("rclpy.qos", rclpy_stub.qos)
|
||||
|
||||
geo = types.ModuleType("geometry_msgs")
|
||||
geo_msg = types.ModuleType("geometry_msgs.msg")
|
||||
class _Vector3:
|
||||
def __init__(self, x=0.0, y=0.0, z=0.0): self.x=x; self.y=y; self.z=z
|
||||
class _PointStamped:
|
||||
def __init__(self): self.point = _Vector3()
|
||||
geo_msg.Vector3 = _Vector3
|
||||
geo_msg.PointStamped = _PointStamped
|
||||
geo.msg = geo_msg
|
||||
sys.modules.setdefault("geometry_msgs", geo)
|
||||
sys.modules.setdefault("geometry_msgs.msg", geo_msg)
|
||||
|
||||
std_msgs = types.ModuleType("std_msgs")
|
||||
std_msgs_msg = types.ModuleType("std_msgs.msg")
|
||||
class _String:
|
||||
def __init__(self, data=""): self.data = data
|
||||
std_msgs_msg.String = _String
|
||||
std_msgs.msg = std_msgs_msg
|
||||
sys.modules.setdefault("std_msgs", std_msgs)
|
||||
sys.modules.setdefault("std_msgs.msg", std_msgs_msg)
|
||||
|
||||
std_srvs = types.ModuleType("std_srvs")
|
||||
std_srvs_srv = types.ModuleType("std_srvs.srv")
|
||||
class _Trigger:
|
||||
class Request: pass
|
||||
class Response:
|
||||
success = False
|
||||
message = ""
|
||||
std_srvs_srv.Trigger = _Trigger
|
||||
std_srvs.srv = std_srvs_srv
|
||||
sys.modules.setdefault("std_srvs", std_srvs)
|
||||
sys.modules.setdefault("std_srvs.srv", std_srvs_srv)
|
||||
sys.modules.setdefault("serial", MagicMock())
|
||||
|
||||
|
||||
_install_stubs()
|
||||
|
||||
from saltybot_gimbal.jlink_gimbal import ( # noqa: E402
|
||||
encode_gimbal_pos, encode_gimbal_home, decode_gimbal_state,
|
||||
GimbalStateFrame, CMD_GIMBAL_POS, TLM_GIMBAL_STATE,
|
||||
STX, ETX, _crc16_ccitt, _build_frame,
|
||||
degs_to_speed_reg, speed_reg_to_degs,
|
||||
)
|
||||
from saltybot_gimbal.gimbal_node import MotionAxis, _clamp # noqa: E402
|
||||
|
||||
|
||||
# ── Helper ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
def _parse_frame(raw: bytes):
|
||||
"""Parse a JLINK frame; return (cmd, payload) or raise on error."""
|
||||
assert raw[0] == STX, f"STX expected, got 0x{raw[0]:02x}"
|
||||
cmd = raw[1]
|
||||
length = raw[2]
|
||||
payload = raw[3:3 + length]
|
||||
crc_hi = raw[3 + length]
|
||||
crc_lo = raw[4 + length]
|
||||
crc_rcvd = (crc_hi << 8) | crc_lo
|
||||
assert raw[-1] == ETX, f"ETX expected, got 0x{raw[-1]:02x}"
|
||||
crc_data = bytes([cmd, length]) + payload
|
||||
crc_calc = _crc16_ccitt(crc_data)
|
||||
assert crc_rcvd == crc_calc, (
|
||||
f"CRC mismatch: got {crc_rcvd:#06x}, expected {crc_calc:#06x}"
|
||||
)
|
||||
return cmd, payload
|
||||
|
||||
|
||||
# ── CRC16 Tests ────────────────────────────────────────────────────────────────
|
||||
|
||||
class TestCRC16(unittest.TestCase):
|
||||
|
||||
def test_empty_data(self):
|
||||
self.assertEqual(_crc16_ccitt(b""), 0xFFFF)
|
||||
|
||||
def test_known_value_123456789(self):
|
||||
# Standard CCITT-FFFF check value for "123456789"
|
||||
self.assertEqual(_crc16_ccitt(b"123456789"), 0x29B1)
|
||||
|
||||
def test_single_byte_is_int(self):
|
||||
v = _crc16_ccitt(b"\x0B")
|
||||
self.assertIsInstance(v, int)
|
||||
self.assertLessEqual(v, 0xFFFF)
|
||||
|
||||
def test_different_data_different_crc(self):
|
||||
self.assertNotEqual(_crc16_ccitt(b"\x00\x01"), _crc16_ccitt(b"\x00\x02"))
|
||||
|
||||
|
||||
# ── Speed Conversion Tests ─────────────────────────────────────────────────────
|
||||
|
||||
class TestSpeedConversion(unittest.TestCase):
|
||||
|
||||
def test_zero_speed_is_max(self):
|
||||
self.assertEqual(degs_to_speed_reg(0.0), 0)
|
||||
|
||||
def test_negative_speed_is_max(self):
|
||||
self.assertEqual(degs_to_speed_reg(-10.0), 0)
|
||||
|
||||
def test_360_degs_maps_to_zero(self):
|
||||
# 360°/s → ratio=1 → reg=0 (max)
|
||||
self.assertEqual(degs_to_speed_reg(360.0), 0)
|
||||
|
||||
def test_mid_speed(self):
|
||||
reg = degs_to_speed_reg(90.0)
|
||||
self.assertGreater(reg, 0)
|
||||
self.assertLess(reg, 4095)
|
||||
|
||||
def test_round_trip_approx(self):
|
||||
for deg_s in (30.0, 60.0, 120.0, 200.0):
|
||||
reg = degs_to_speed_reg(deg_s)
|
||||
back = speed_reg_to_degs(reg)
|
||||
self.assertAlmostEqual(back, deg_s, delta=5.0)
|
||||
|
||||
def test_reg_zero_is_max_speed(self):
|
||||
self.assertAlmostEqual(speed_reg_to_degs(0), 360.0)
|
||||
|
||||
|
||||
# ── Encode Tests ───────────────────────────────────────────────────────────────
|
||||
|
||||
class TestEncodeGimbalPos(unittest.TestCase):
|
||||
|
||||
def test_zero_pose(self):
|
||||
raw = encode_gimbal_pos(0.0, 0.0, 90.0)
|
||||
cmd, payload = _parse_frame(raw)
|
||||
self.assertEqual(cmd, CMD_GIMBAL_POS) # 0x0B
|
||||
pan_x10, tilt_x10, speed = struct.unpack(">hhH", payload)
|
||||
self.assertEqual(pan_x10, 0)
|
||||
self.assertEqual(tilt_x10, 0)
|
||||
|
||||
def test_positive_pan_tilt(self):
|
||||
raw = encode_gimbal_pos(30.0, 15.0, 45.0)
|
||||
_, payload = _parse_frame(raw)
|
||||
pan_x10, tilt_x10, _ = struct.unpack(">hhH", payload)
|
||||
self.assertEqual(pan_x10, 300) # 30.0 * 10
|
||||
self.assertEqual(tilt_x10, 150) # 15.0 * 10
|
||||
|
||||
def test_negative_angles(self):
|
||||
raw = encode_gimbal_pos(-90.0, -20.0, 60.0)
|
||||
_, payload = _parse_frame(raw)
|
||||
pan_x10, tilt_x10, _ = struct.unpack(">hhH", payload)
|
||||
self.assertEqual(pan_x10, -900)
|
||||
self.assertEqual(tilt_x10, -200)
|
||||
|
||||
def test_pan_limit_150_deg(self):
|
||||
raw = encode_gimbal_pos(150.0, 0.0, 30.0)
|
||||
_, payload = _parse_frame(raw)
|
||||
pan_x10, _, _ = struct.unpack(">hhH", payload)
|
||||
self.assertEqual(pan_x10, 1500)
|
||||
|
||||
def test_tilt_limit_45_deg(self):
|
||||
raw = encode_gimbal_pos(0.0, 45.0, 30.0)
|
||||
_, payload = _parse_frame(raw)
|
||||
_, tilt_x10, _ = struct.unpack(">hhH", payload)
|
||||
self.assertEqual(tilt_x10, 450)
|
||||
|
||||
def test_extreme_clamped(self):
|
||||
raw = encode_gimbal_pos(9999.0, -9999.0, 0.0)
|
||||
_, payload = _parse_frame(raw)
|
||||
pan_x10, tilt_x10, _ = struct.unpack(">hhH", payload)
|
||||
self.assertEqual(pan_x10, 32767)
|
||||
self.assertEqual(tilt_x10, -32767)
|
||||
|
||||
def test_frame_byte_layout(self):
|
||||
raw = encode_gimbal_pos(10.0, -5.0, 30.0)
|
||||
self.assertEqual(raw[0], STX)
|
||||
self.assertEqual(raw[-1], ETX)
|
||||
self.assertEqual(raw[1], CMD_GIMBAL_POS) # 0x0B
|
||||
self.assertEqual(raw[2], 6) # 6-byte payload
|
||||
|
||||
def test_crc_valid(self):
|
||||
# _parse_frame raises if CRC is bad
|
||||
_parse_frame(encode_gimbal_pos(45.0, -10.0, 120.0))
|
||||
|
||||
|
||||
class TestEncodeGimbalHome(unittest.TestCase):
|
||||
|
||||
def test_homes_to_zero(self):
|
||||
raw = encode_gimbal_home()
|
||||
cmd, payload = _parse_frame(raw)
|
||||
self.assertEqual(cmd, CMD_GIMBAL_POS)
|
||||
pan_x10, tilt_x10, _ = struct.unpack(">hhH", payload)
|
||||
self.assertEqual(pan_x10, 0)
|
||||
self.assertEqual(tilt_x10, 0)
|
||||
|
||||
def test_crc_valid(self):
|
||||
_parse_frame(encode_gimbal_home())
|
||||
|
||||
|
||||
# ── Decode Tests ───────────────────────────────────────────────────────────────
|
||||
|
||||
class TestDecodeGimbalState(unittest.TestCase):
|
||||
|
||||
def _make_payload(self, pan_x10, tilt_x10, pan_spd, tilt_spd,
|
||||
torque_en, rx_err_pct) -> bytes:
|
||||
return struct.pack(">hhHHBB",
|
||||
pan_x10, tilt_x10,
|
||||
pan_spd, tilt_spd,
|
||||
torque_en, rx_err_pct)
|
||||
|
||||
def test_zero_state(self):
|
||||
state = decode_gimbal_state(self._make_payload(0, 0, 0, 0, 0, 0))
|
||||
self.assertIsNotNone(state)
|
||||
self.assertAlmostEqual(state.pan_deg, 0.0)
|
||||
self.assertAlmostEqual(state.tilt_deg, 0.0)
|
||||
self.assertFalse(state.torque_en)
|
||||
self.assertEqual(state.rx_err_pct, 0)
|
||||
|
||||
def test_angles_decode(self):
|
||||
state = decode_gimbal_state(self._make_payload(500, -250, 100, 200, 1, 5))
|
||||
self.assertAlmostEqual(state.pan_deg, 50.0)
|
||||
self.assertAlmostEqual(state.tilt_deg, -25.0)
|
||||
|
||||
def test_torque_en(self):
|
||||
state = decode_gimbal_state(self._make_payload(0, 0, 0, 0, 1, 0))
|
||||
self.assertTrue(state.torque_en)
|
||||
|
||||
def test_torque_off(self):
|
||||
state = decode_gimbal_state(self._make_payload(0, 0, 0, 0, 0, 0))
|
||||
self.assertFalse(state.torque_en)
|
||||
|
||||
def test_speed_raw(self):
|
||||
state = decode_gimbal_state(self._make_payload(0, 0, 1234, 5678, 0, 0))
|
||||
self.assertEqual(state.pan_speed_raw, 1234)
|
||||
self.assertEqual(state.tilt_speed_raw, 5678)
|
||||
|
||||
def test_rx_err_pct(self):
|
||||
state = decode_gimbal_state(self._make_payload(0, 0, 0, 0, 0, 42))
|
||||
self.assertEqual(state.rx_err_pct, 42)
|
||||
|
||||
def test_short_payload_returns_none(self):
|
||||
self.assertIsNone(decode_gimbal_state(b"\x00\x01\x02"))
|
||||
|
||||
def test_empty_payload_returns_none(self):
|
||||
self.assertIsNone(decode_gimbal_state(b""))
|
||||
|
||||
def test_exactly_10_bytes_ok(self):
|
||||
payload = self._make_payload(100, -50, 512, 512, 1, 3)
|
||||
state = decode_gimbal_state(payload)
|
||||
self.assertIsNotNone(state)
|
||||
|
||||
|
||||
# ── MotionAxis Tests ───────────────────────────────────────────────────────────
|
||||
|
||||
class TestMotionAxis(unittest.TestCase):
|
||||
|
||||
def test_starts_at_initial_pos(self):
|
||||
ax = MotionAxis(10.0, 90.0, 180.0)
|
||||
self.assertAlmostEqual(ax.pos, 10.0)
|
||||
|
||||
def test_not_moving_at_rest(self):
|
||||
ax = MotionAxis(0.0, 90.0, 180.0)
|
||||
self.assertFalse(ax.is_moving)
|
||||
|
||||
def test_moves_toward_target(self):
|
||||
ax = MotionAxis(0.0, 90.0, 180.0)
|
||||
ax.set_target(90.0)
|
||||
for _ in range(5):
|
||||
ax.tick(0.05)
|
||||
self.assertGreater(ax.pos, 0.0)
|
||||
|
||||
def test_reaches_target(self):
|
||||
ax = MotionAxis(0.0, 90.0, 180.0)
|
||||
ax.set_target(10.0)
|
||||
for _ in range(200):
|
||||
ax.tick(0.05)
|
||||
self.assertAlmostEqual(ax.pos, 10.0, places=1)
|
||||
self.assertFalse(ax.is_moving)
|
||||
|
||||
def test_negative_target(self):
|
||||
ax = MotionAxis(0.0, 90.0, 180.0)
|
||||
ax.set_target(-20.0)
|
||||
for _ in range(200):
|
||||
ax.tick(0.05)
|
||||
self.assertAlmostEqual(ax.pos, -20.0, places=1)
|
||||
|
||||
def test_faster_speed_reaches_sooner(self):
|
||||
ax_fast = MotionAxis(0.0, 180.0, 360.0)
|
||||
ax_slow = MotionAxis(0.0, 30.0, 360.0)
|
||||
ax_fast.set_target(30.0)
|
||||
ax_slow.set_target(30.0)
|
||||
for _ in range(5):
|
||||
ax_fast.tick(0.05)
|
||||
ax_slow.tick(0.05)
|
||||
self.assertGreater(ax_fast.pos, ax_slow.pos)
|
||||
|
||||
def test_is_moving_while_en_route(self):
|
||||
ax = MotionAxis(0.0, 90.0, 180.0)
|
||||
ax.set_target(60.0)
|
||||
ax.tick(0.05)
|
||||
self.assertTrue(ax.is_moving)
|
||||
|
||||
def test_no_overshoot(self):
|
||||
ax = MotionAxis(0.0, 90.0, 180.0)
|
||||
ax.set_target(5.0)
|
||||
for _ in range(500):
|
||||
ax.tick(0.05)
|
||||
self.assertLessEqual(ax.pos, 5.01)
|
||||
|
||||
def test_set_target_updates_speed(self):
|
||||
ax = MotionAxis(0.0, 90.0, 180.0)
|
||||
ax.set_target(10.0, speed=45.0)
|
||||
self.assertAlmostEqual(ax.max_speed, 45.0)
|
||||
|
||||
|
||||
# ── Clamp Tests ────────────────────────────────────────────────────────────────
|
||||
|
||||
class TestClamp(unittest.TestCase):
|
||||
|
||||
def test_within_range(self):
|
||||
self.assertEqual(_clamp(5.0, 0.0, 10.0), 5.0)
|
||||
|
||||
def test_below_min(self):
|
||||
self.assertEqual(_clamp(-5.0, 0.0, 10.0), 0.0)
|
||||
|
||||
def test_above_max(self):
|
||||
self.assertEqual(_clamp(15.0, 0.0, 10.0), 10.0)
|
||||
|
||||
def test_at_lower_boundary(self):
|
||||
self.assertEqual(_clamp(0.0, 0.0, 10.0), 0.0)
|
||||
|
||||
def test_at_upper_boundary(self):
|
||||
self.assertEqual(_clamp(10.0, 0.0, 10.0), 10.0)
|
||||
|
||||
|
||||
# ── Look-at Projection Tests ───────────────────────────────────────────────────
|
||||
|
||||
class TestLookAtProjection(unittest.TestCase):
|
||||
"""Test the look-at 3D → pan/tilt math (mirrors gimbal_node logic)."""
|
||||
|
||||
def _project(self, x, y, z):
|
||||
pan_deg = math.degrees(math.atan2(x, z))
|
||||
tilt_deg = -math.degrees(math.atan2(y, z))
|
||||
return pan_deg, tilt_deg
|
||||
|
||||
def test_straight_ahead(self):
|
||||
pan, tilt = self._project(0.0, 0.0, 1.0)
|
||||
self.assertAlmostEqual(pan, 0.0, places=5)
|
||||
self.assertAlmostEqual(tilt, 0.0, places=5)
|
||||
|
||||
def test_right_target(self):
|
||||
pan, tilt = self._project(1.0, 0.0, 1.0)
|
||||
self.assertAlmostEqual(pan, 45.0, places=4)
|
||||
self.assertAlmostEqual(tilt, 0.0, places=4)
|
||||
|
||||
def test_up_target(self):
|
||||
# camera frame: negative y = above centre → tilt up (positive)
|
||||
pan, tilt = self._project(0.0, -1.0, 1.0)
|
||||
self.assertAlmostEqual(pan, 0.0, places=4)
|
||||
self.assertAlmostEqual(tilt, 45.0, places=4)
|
||||
|
||||
def test_left_down_target(self):
|
||||
pan, tilt = self._project(-1.0, 1.0, 2.0)
|
||||
self.assertLess(pan, 0.0) # left → negative pan
|
||||
self.assertLess(tilt, 0.0) # below → negative tilt
|
||||
|
||||
def test_90_degree_right(self):
|
||||
pan, _ = self._project(1.0, 0.0, 0.0)
|
||||
self.assertAlmostEqual(pan, 90.0, places=4)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@ -1,36 +0,0 @@
|
||||
head_tracking:
|
||||
ros__parameters:
|
||||
# ── Camera (Intel RealSense D435i) ──────────────────────────────────────
|
||||
image_width: 848 # pixels
|
||||
image_height: 480 # pixels
|
||||
fov_h_deg: 87.0 # horizontal field of view (degrees)
|
||||
fov_v_deg: 58.0 # vertical field of view (degrees)
|
||||
|
||||
# ── PID gains — pan axis ────────────────────────────────────────────────
|
||||
# Output: deg/step. Start conservative; increase kp for faster tracking.
|
||||
pan_kp: 0.08 # proportional gain (deg per deg-of-error per step)
|
||||
pan_ki: 0.002 # integral gain
|
||||
pan_kd: 0.015 # derivative gain
|
||||
|
||||
# ── PID gains — tilt axis ───────────────────────────────────────────────
|
||||
tilt_kp: 0.08
|
||||
tilt_ki: 0.002
|
||||
tilt_kd: 0.015
|
||||
|
||||
# ── Servo limits (degrees from centre) ──────────────────────────────────
|
||||
pan_min_deg: -60.0 # max left
|
||||
pan_max_deg: 60.0 # max right
|
||||
tilt_min_deg: -30.0 # max down
|
||||
tilt_max_deg: 30.0 # max up
|
||||
|
||||
# ── Tracking behaviour ──────────────────────────────────────────────────
|
||||
hold_duration_s: 3.0 # hold last position this long after target loss
|
||||
center_speed_deg_s: 20.0 # degrees/sec to return to centre after hold
|
||||
dead_zone_px: 10.0 # pixel error smaller than this is treated as zero
|
||||
control_hz: 20.0 # PID loop rate
|
||||
|
||||
# ── Target selection ────────────────────────────────────────────────────
|
||||
# score = distance_weight * (1/(1+dist_m)) + confidence_weight * confidence
|
||||
# Highest score wins. If distance_m == 0 (unknown), confidence only.
|
||||
distance_weight: 0.6
|
||||
confidence_weight: 0.4
|
||||
@ -1,35 +0,0 @@
|
||||
"""Launch file for saltybot_head_tracking (Issue #549)."""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
config = os.path.join(
|
||||
get_package_share_directory("saltybot_head_tracking"),
|
||||
"config",
|
||||
"head_tracking_params.yaml",
|
||||
)
|
||||
|
||||
head_tracking_node = Node(
|
||||
package="saltybot_head_tracking",
|
||||
executable="head_tracking",
|
||||
name="head_tracking",
|
||||
parameters=[config],
|
||||
remappings=[
|
||||
# Remap if object detection publishes on a different topic
|
||||
# ("/saltybot/objects", "/saltybot/objects"),
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
# Note: pair with saltybot_pan_tilt node.
|
||||
# pan_tilt_node subscribes to /saltybot/target_track (Point, pixel coords).
|
||||
# head_tracking publishes to /saltybot/gimbal/cmd (Point, angle degrees).
|
||||
# To bridge, remap pan_tilt's subscription:
|
||||
# remappings=[("/saltybot/target_track", "/saltybot/gimbal/cmd")]
|
||||
# in the pan_tilt launch — or update pan_tilt to accept angle setpoints.
|
||||
|
||||
return LaunchDescription([head_tracking_node])
|
||||
@ -1,31 +0,0 @@
|
||||
<?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_head_tracking</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
Person-following head tracking (Issue #549).
|
||||
Subscribes to YOLOv8n person detections, selects the best target
|
||||
(closest / highest confidence), runs dual-axis PID controllers, and
|
||||
publishes pan/tilt angle setpoints to /saltybot/gimbal/cmd.
|
||||
Lost-target behaviour: hold 3 s then centre.
|
||||
</description>
|
||||
<maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>saltybot_object_detection_msgs</depend>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,422 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Person-following head tracking node (Issue #549).
|
||||
|
||||
Subscribes to person detection bounding boxes, selects the best target
|
||||
(closest / highest confidence), runs independent PID controllers for pan
|
||||
and tilt axes, and publishes angle setpoints to /saltybot/gimbal/cmd.
|
||||
|
||||
Subscribed topics:
|
||||
/saltybot/objects (saltybot_object_detection_msgs/DetectedObjectArray)
|
||||
— YOLOv8n detections; this node filters for class_id==0 (person).
|
||||
|
||||
Published topics:
|
||||
/saltybot/gimbal/cmd (geometry_msgs/Point)
|
||||
— x = desired pan angle (deg, +left / −right from centre)
|
||||
— y = desired tilt angle (deg, +up / −down from centre)
|
||||
— z = tracking confidence (0.0 = no target / centering)
|
||||
|
||||
/saltybot/head_tracking/state (std_msgs/String)
|
||||
— JSON status string: state, target_id, pan_deg, tilt_deg, error_pan_px,
|
||||
error_tilt_px, confidence, distance_m
|
||||
|
||||
State machine:
|
||||
IDLE → waiting for first detection
|
||||
TRACKING → person visible, PID active
|
||||
LOST → person gone; hold last angle for hold_duration_s seconds
|
||||
CENTERING → returning to (0°, 0°) after hold expires
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum, auto
|
||||
from typing import Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
|
||||
from geometry_msgs.msg import Point
|
||||
from std_msgs.msg import String
|
||||
|
||||
from saltybot_object_detection_msgs.msg import DetectedObject, DetectedObjectArray
|
||||
|
||||
|
||||
# ── PID controller ────────────────────────────────────────────────────────────
|
||||
|
||||
class PIDController:
|
||||
"""Simple discrete-time PID with anti-windup integral clamp."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
kp: float,
|
||||
ki: float,
|
||||
kd: float,
|
||||
min_output: float,
|
||||
max_output: float,
|
||||
integral_clamp: float = 30.0,
|
||||
) -> None:
|
||||
self.kp = kp
|
||||
self.ki = ki
|
||||
self.kd = kd
|
||||
self.min_output = min_output
|
||||
self.max_output = max_output
|
||||
self.integral_clamp = integral_clamp
|
||||
|
||||
self._integral: float = 0.0
|
||||
self._prev_error: float = 0.0
|
||||
|
||||
def update(self, error: float, dt: float) -> float:
|
||||
"""Compute PID output for the given error and timestep."""
|
||||
if dt <= 0.0:
|
||||
return 0.0
|
||||
|
||||
self._integral += error * dt
|
||||
# Anti-windup: clamp integral term
|
||||
self._integral = max(
|
||||
-self.integral_clamp, min(self.integral_clamp, self._integral)
|
||||
)
|
||||
|
||||
derivative = (error - self._prev_error) / dt
|
||||
self._prev_error = error
|
||||
|
||||
output = (
|
||||
self.kp * error
|
||||
+ self.ki * self._integral
|
||||
+ self.kd * derivative
|
||||
)
|
||||
return max(self.min_output, min(self.max_output, output))
|
||||
|
||||
def reset(self) -> None:
|
||||
self._integral = 0.0
|
||||
self._prev_error = 0.0
|
||||
|
||||
|
||||
# ── Target dataclass ──────────────────────────────────────────────────────────
|
||||
|
||||
@dataclass
|
||||
class Target:
|
||||
"""Best-candidate person in the current detection frame."""
|
||||
track_id: int = -1
|
||||
center_x: float = 0.0 # bbox centre, pixels
|
||||
center_y: float = 0.0
|
||||
confidence: float = 0.0
|
||||
distance_m: float = 0.0 # 0 = unknown
|
||||
bbox_area: float = 0.0
|
||||
stamp: float = 0.0 # rclpy.clock time (seconds)
|
||||
|
||||
|
||||
# ── State machine ─────────────────────────────────────────────────────────────
|
||||
|
||||
class TrackState(Enum):
|
||||
IDLE = auto()
|
||||
TRACKING = auto()
|
||||
LOST = auto()
|
||||
CENTERING = auto()
|
||||
|
||||
|
||||
# ── Node ──────────────────────────────────────────────────────────────────────
|
||||
|
||||
class HeadTrackingNode(Node):
|
||||
"""Person-following head tracking with PID control."""
|
||||
|
||||
_PERSON_CLASS_ID = 0 # COCO class 0 = "person"
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("head_tracking")
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────────
|
||||
self.declare_parameter("image_width", 848) # D435i default
|
||||
self.declare_parameter("image_height", 480)
|
||||
self.declare_parameter("fov_h_deg", 87.0) # D435i horizontal FOV
|
||||
self.declare_parameter("fov_v_deg", 58.0) # D435i vertical FOV
|
||||
|
||||
# PID gains — pan axis
|
||||
self.declare_parameter("pan_kp", 0.08) # deg / px
|
||||
self.declare_parameter("pan_ki", 0.002)
|
||||
self.declare_parameter("pan_kd", 0.015)
|
||||
|
||||
# PID gains — tilt axis
|
||||
self.declare_parameter("tilt_kp", 0.08)
|
||||
self.declare_parameter("tilt_ki", 0.002)
|
||||
self.declare_parameter("tilt_kd", 0.015)
|
||||
|
||||
# Servo limits
|
||||
self.declare_parameter("pan_min_deg", -60.0)
|
||||
self.declare_parameter("pan_max_deg", 60.0)
|
||||
self.declare_parameter("tilt_min_deg", -30.0)
|
||||
self.declare_parameter("tilt_max_deg", 30.0)
|
||||
|
||||
# Tracking behaviour
|
||||
self.declare_parameter("hold_duration_s", 3.0) # hold after loss
|
||||
self.declare_parameter("center_speed_deg_s", 20.0) # centering speed
|
||||
self.declare_parameter("dead_zone_px", 10.0) # ignore small errors
|
||||
self.declare_parameter("control_hz", 20.0) # PID loop rate
|
||||
|
||||
# Target selection weights
|
||||
self.declare_parameter("distance_weight", 0.6) # vs confidence
|
||||
self.declare_parameter("confidence_weight", 0.4)
|
||||
|
||||
self._img_w = self.get_parameter("image_width").value
|
||||
self._img_h = self.get_parameter("image_height").value
|
||||
self._fov_h = self.get_parameter("fov_h_deg").value
|
||||
self._fov_v = self.get_parameter("fov_v_deg").value
|
||||
|
||||
pan_kp = self.get_parameter("pan_kp").value
|
||||
pan_ki = self.get_parameter("pan_ki").value
|
||||
pan_kd = self.get_parameter("pan_kd").value
|
||||
tilt_kp = self.get_parameter("tilt_kp").value
|
||||
tilt_ki = self.get_parameter("tilt_ki").value
|
||||
tilt_kd = self.get_parameter("tilt_kd").value
|
||||
|
||||
pan_min = self.get_parameter("pan_min_deg").value
|
||||
pan_max = self.get_parameter("pan_max_deg").value
|
||||
tilt_min = self.get_parameter("tilt_min_deg").value
|
||||
tilt_max = self.get_parameter("tilt_max_deg").value
|
||||
|
||||
self._hold_duration = self.get_parameter("hold_duration_s").value
|
||||
self._center_speed = self.get_parameter("center_speed_deg_s").value
|
||||
self._dead_zone = self.get_parameter("dead_zone_px").value
|
||||
self._control_hz = self.get_parameter("control_hz").value
|
||||
self._dist_weight = self.get_parameter("distance_weight").value
|
||||
self._conf_weight = self.get_parameter("confidence_weight").value
|
||||
|
||||
# ── PID controllers ───────────────────────────────────────────────────
|
||||
self._pid_pan = PIDController(pan_kp, pan_ki, pan_kd, pan_min, pan_max)
|
||||
self._pid_tilt = PIDController(tilt_kp, tilt_ki, tilt_kd, tilt_min, tilt_max)
|
||||
|
||||
# ── State ─────────────────────────────────────────────────────────────
|
||||
self._state: TrackState = TrackState.IDLE
|
||||
self._target: Optional[Target] = None
|
||||
self._last_seen: float = 0.0 # wall-clock time of last detection
|
||||
|
||||
# Desired angle setpoint (absolute, degrees from centre)
|
||||
self._pan_deg: float = 0.0
|
||||
self._tilt_deg: float = 0.0
|
||||
|
||||
# ── Subscriptions ─────────────────────────────────────────────────────
|
||||
sensor_qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
)
|
||||
self._sub_objects = self.create_subscription(
|
||||
DetectedObjectArray,
|
||||
"/saltybot/objects",
|
||||
self._on_detections,
|
||||
sensor_qos,
|
||||
)
|
||||
|
||||
# ── Publishers ────────────────────────────────────────────────────────
|
||||
self._pub_cmd = self.create_publisher(
|
||||
Point, "/saltybot/gimbal/cmd", 10
|
||||
)
|
||||
self._pub_state = self.create_publisher(
|
||||
String, "/saltybot/head_tracking/state", 10
|
||||
)
|
||||
|
||||
# ── Control loop timer ────────────────────────────────────────────────
|
||||
dt = 1.0 / self._control_hz
|
||||
self._prev_time: float = self.get_clock().now().nanoseconds * 1e-9
|
||||
self.create_timer(dt, self._control_loop)
|
||||
|
||||
self.get_logger().info(
|
||||
f"HeadTrackingNode ready — "
|
||||
f"image={self._img_w}×{self._img_h} "
|
||||
f"FOV={self._fov_h:.0f}°×{self._fov_v:.0f}° "
|
||||
f"hold={self._hold_duration:.1f}s "
|
||||
f"rate={self._control_hz:.0f}Hz"
|
||||
)
|
||||
|
||||
# ── Detection callback ────────────────────────────────────────────────────
|
||||
|
||||
def _on_detections(self, msg: DetectedObjectArray) -> None:
|
||||
"""Process incoming object detections and select best person target."""
|
||||
persons = [
|
||||
obj for obj in msg.objects
|
||||
if obj.class_id == self._PERSON_CLASS_ID
|
||||
]
|
||||
if not persons:
|
||||
return
|
||||
|
||||
best = self._select_target(persons)
|
||||
if best is None:
|
||||
return
|
||||
|
||||
now = self.get_clock().now().nanoseconds * 1e-9
|
||||
self._target = best
|
||||
self._target.stamp = now
|
||||
self._last_seen = now
|
||||
|
||||
if self._state in (TrackState.IDLE, TrackState.LOST, TrackState.CENTERING):
|
||||
self._state = TrackState.TRACKING
|
||||
# Don't reset PIDs on re-acquisition — avoids jerk
|
||||
self.get_logger().info(
|
||||
f"Target acquired: id={best.track_id} "
|
||||
f"conf={best.confidence:.2f} dist={best.distance_m:.1f}m"
|
||||
)
|
||||
|
||||
def _select_target(self, persons: list) -> Optional[Target]:
|
||||
"""Select highest-priority person: closest first, then confidence.
|
||||
|
||||
Scoring:
|
||||
score = dist_weight * dist_score + conf_weight * confidence
|
||||
dist_score = 1 / (1 + distance_m) — higher = closer
|
||||
If distance_m == 0 (unknown), use confidence only.
|
||||
"""
|
||||
best_score = -1.0
|
||||
best_obj: Optional[DetectedObject] = None
|
||||
|
||||
for obj in persons:
|
||||
dist_m = obj.distance_m
|
||||
conf = obj.confidence
|
||||
|
||||
if dist_m > 0.0:
|
||||
dist_score = 1.0 / (1.0 + dist_m)
|
||||
score = self._dist_weight * dist_score + self._conf_weight * conf
|
||||
else:
|
||||
score = conf # distance unknown — use confidence only
|
||||
|
||||
if score > best_score:
|
||||
best_score = score
|
||||
best_obj = obj
|
||||
|
||||
if best_obj is None:
|
||||
return None
|
||||
|
||||
# Extract bbox centre in pixel coordinates
|
||||
cx = best_obj.bbox.center.position.x
|
||||
cy = best_obj.bbox.center.position.y
|
||||
area = best_obj.bbox.size_x * best_obj.bbox.size_y
|
||||
|
||||
return Target(
|
||||
track_id=0, # DetectedObject has no track_id; use 0
|
||||
center_x=cx,
|
||||
center_y=cy,
|
||||
confidence=best_obj.confidence,
|
||||
distance_m=best_obj.distance_m,
|
||||
bbox_area=area,
|
||||
)
|
||||
|
||||
# ── Control loop ──────────────────────────────────────────────────────────
|
||||
|
||||
def _control_loop(self) -> None:
|
||||
"""20 Hz PID update and gimbal command publish."""
|
||||
now = self.get_clock().now().nanoseconds * 1e-9
|
||||
dt = now - self._prev_time
|
||||
self._prev_time = now
|
||||
|
||||
if dt <= 0.0:
|
||||
return
|
||||
|
||||
time_since_seen = now - self._last_seen
|
||||
|
||||
# ── State transitions ──────────────────────────────────────────────
|
||||
if self._state == TrackState.TRACKING:
|
||||
if time_since_seen > (2.0 / self._control_hz):
|
||||
# No new detection in the last ~2 control cycles → lost
|
||||
self._state = TrackState.LOST
|
||||
self._last_seen = now # reset hold timer
|
||||
self.get_logger().info("Target lost — holding position")
|
||||
|
||||
elif self._state == TrackState.LOST:
|
||||
if time_since_seen >= self._hold_duration:
|
||||
self._state = TrackState.CENTERING
|
||||
self._pid_pan.reset()
|
||||
self._pid_tilt.reset()
|
||||
self.get_logger().info("Hold expired — centering")
|
||||
|
||||
# ── PID update ────────────────────────────────────────────────────
|
||||
error_pan_px = 0.0
|
||||
error_tilt_px = 0.0
|
||||
confidence = 0.0
|
||||
|
||||
if self._state == TrackState.TRACKING and self._target is not None:
|
||||
cx = self._target.center_x
|
||||
cy = self._target.center_y
|
||||
confidence = self._target.confidence
|
||||
|
||||
# Pixel error: positive = target right of centre (→ pan right = negative pan)
|
||||
error_pan_px = cx - self._img_w / 2.0
|
||||
error_tilt_px = cy - self._img_h / 2.0
|
||||
|
||||
# Dead-zone: don't chase tiny errors
|
||||
if abs(error_pan_px) < self._dead_zone:
|
||||
error_pan_px = 0.0
|
||||
if abs(error_tilt_px) < self._dead_zone:
|
||||
error_tilt_px = 0.0
|
||||
|
||||
# Convert pixel error → angle error (degrees)
|
||||
# px_per_deg = image_dim / fov_deg
|
||||
err_pan_deg = error_pan_px / (self._img_w / self._fov_h)
|
||||
err_tilt_deg = error_tilt_px / (self._img_h / self._fov_v)
|
||||
|
||||
# PID output: delta angle to apply this timestep
|
||||
# Pan: positive error (target right) → decrease pan (turn right)
|
||||
# Tilt: positive error (target below) → decrease tilt (look down)
|
||||
pan_delta = self._pid_pan.update(-err_pan_deg, dt)
|
||||
tilt_delta = self._pid_tilt.update(-err_tilt_deg, dt)
|
||||
|
||||
pan_min = self.get_parameter("pan_min_deg").value
|
||||
pan_max = self.get_parameter("pan_max_deg").value
|
||||
tilt_min = self.get_parameter("tilt_min_deg").value
|
||||
tilt_max = self.get_parameter("tilt_max_deg").value
|
||||
|
||||
self._pan_deg = max(pan_min, min(pan_max, self._pan_deg + pan_delta))
|
||||
self._tilt_deg = max(tilt_min, min(tilt_max, self._tilt_deg + tilt_delta))
|
||||
|
||||
elif self._state == TrackState.CENTERING:
|
||||
# Drive angles back toward zero at bounded speed
|
||||
center_step = self._center_speed * dt
|
||||
|
||||
if abs(self._pan_deg) < center_step:
|
||||
self._pan_deg = 0.0
|
||||
else:
|
||||
self._pan_deg -= math.copysign(center_step, self._pan_deg)
|
||||
|
||||
if abs(self._tilt_deg) < center_step:
|
||||
self._tilt_deg = 0.0
|
||||
else:
|
||||
self._tilt_deg -= math.copysign(center_step, self._tilt_deg)
|
||||
|
||||
if self._pan_deg == 0.0 and self._tilt_deg == 0.0:
|
||||
self._state = TrackState.IDLE
|
||||
self.get_logger().info("Centered — IDLE")
|
||||
|
||||
# ── Publish gimbal command ─────────────────────────────────────────
|
||||
cmd = Point()
|
||||
cmd.x = self._pan_deg
|
||||
cmd.y = self._tilt_deg
|
||||
cmd.z = confidence
|
||||
self._pub_cmd.publish(cmd)
|
||||
|
||||
# ── Publish state ─────────────────────────────────────────────────
|
||||
state_data = {
|
||||
"state": self._state.name,
|
||||
"pan_deg": round(self._pan_deg, 2),
|
||||
"tilt_deg": round(self._tilt_deg, 2),
|
||||
"error_pan_px": round(error_pan_px, 1),
|
||||
"error_tilt_px": round(error_tilt_px, 1),
|
||||
"confidence": round(confidence, 3),
|
||||
"distance_m": round(self._target.distance_m, 2) if self._target else 0.0,
|
||||
"time_since_seen_s": round(now - self._last_seen, 2),
|
||||
}
|
||||
self._pub_state.publish(String(data=json.dumps(state_data)))
|
||||
|
||||
|
||||
# ── Entry point ───────────────────────────────────────────────────────────────
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = HeadTrackingNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,5 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_head_tracking
|
||||
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_head_tracking
|
||||
@ -1,32 +0,0 @@
|
||||
from setuptools import setup, find_packages
|
||||
|
||||
package_name = 'saltybot_head_tracking'
|
||||
|
||||
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', [
|
||||
'launch/head_tracking.launch.py',
|
||||
]),
|
||||
('share/' + package_name + '/config', [
|
||||
'config/head_tracking_params.yaml',
|
||||
]),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='sl-perception',
|
||||
maintainer_email='sl-perception@saltylab.local',
|
||||
description='Person-following head tracking with PID control (Issue #549)',
|
||||
license='MIT',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'head_tracking = saltybot_head_tracking.head_tracking_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,66 +0,0 @@
|
||||
sensor_health_node:
|
||||
ros__parameters:
|
||||
# Publish rate for DiagnosticArray and JSON summary
|
||||
check_hz: 1.0
|
||||
|
||||
# MQTT broker for saltybot/health topic
|
||||
mqtt_broker: "localhost"
|
||||
mqtt_port: 1883
|
||||
mqtt_topic: "saltybot/health"
|
||||
mqtt_enabled: true
|
||||
|
||||
# Per-sensor thresholds and configuration
|
||||
# Each entry: topic, name, warn_s (WARN threshold), error_s (ERROR threshold), critical
|
||||
#
|
||||
# critical=true: system cannot operate without this sensor
|
||||
# warn_s: topic age (s) that triggers WARN level
|
||||
# error_s: topic age (s) that triggers ERROR level
|
||||
|
||||
sensors:
|
||||
- topic: "/camera/color/image_raw"
|
||||
name: "camera_color"
|
||||
warn_s: 1.0
|
||||
error_s: 3.0
|
||||
critical: true
|
||||
|
||||
- topic: "/camera/depth/image_rect_raw"
|
||||
name: "camera_depth"
|
||||
warn_s: 1.0
|
||||
error_s: 3.0
|
||||
critical: true
|
||||
|
||||
- topic: "/camera/color/camera_info"
|
||||
name: "camera_info"
|
||||
warn_s: 2.0
|
||||
error_s: 5.0
|
||||
critical: false
|
||||
|
||||
- topic: "/scan"
|
||||
name: "lidar"
|
||||
warn_s: 1.0
|
||||
error_s: 3.0
|
||||
critical: true
|
||||
|
||||
- topic: "/imu/data"
|
||||
name: "imu"
|
||||
warn_s: 0.5
|
||||
error_s: 2.0
|
||||
critical: true
|
||||
|
||||
- topic: "/saltybot/uwb/range"
|
||||
name: "uwb"
|
||||
warn_s: 2.0
|
||||
error_s: 5.0
|
||||
critical: false
|
||||
|
||||
- topic: "/saltybot/battery"
|
||||
name: "battery"
|
||||
warn_s: 3.0
|
||||
error_s: 8.0
|
||||
critical: false
|
||||
|
||||
- topic: "/saltybot/motor_daemon/status"
|
||||
name: "motor_daemon"
|
||||
warn_s: 2.0
|
||||
error_s: 5.0
|
||||
critical: true
|
||||
@ -1,50 +0,0 @@
|
||||
"""sensor_health.launch.py — Launch sensor health monitor node (Issue #566)."""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
pkg = get_package_share_directory("saltybot_health_monitor")
|
||||
|
||||
check_hz_arg = DeclareLaunchArgument(
|
||||
"check_hz",
|
||||
default_value="1.0",
|
||||
description="Health check publish rate (Hz)",
|
||||
)
|
||||
mqtt_broker_arg = DeclareLaunchArgument(
|
||||
"mqtt_broker",
|
||||
default_value="localhost",
|
||||
description="MQTT broker hostname",
|
||||
)
|
||||
mqtt_enabled_arg = DeclareLaunchArgument(
|
||||
"mqtt_enabled",
|
||||
default_value="true",
|
||||
description="Enable MQTT publishing to saltybot/health",
|
||||
)
|
||||
|
||||
sensor_health_node = Node(
|
||||
package="saltybot_health_monitor",
|
||||
executable="sensor_health_node",
|
||||
name="sensor_health_node",
|
||||
output="screen",
|
||||
parameters=[
|
||||
os.path.join(pkg, "config", "sensor_health_params.yaml"),
|
||||
{
|
||||
"check_hz": LaunchConfiguration("check_hz"),
|
||||
"mqtt_broker": LaunchConfiguration("mqtt_broker"),
|
||||
"mqtt_enabled": LaunchConfiguration("mqtt_enabled"),
|
||||
},
|
||||
],
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
check_hz_arg,
|
||||
mqtt_broker_arg,
|
||||
mqtt_enabled_arg,
|
||||
sensor_health_node,
|
||||
])
|
||||
@ -1,24 +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_health_monitor</name>
|
||||
<version>0.2.0</version>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
ROS2 health monitor for SaltyBot. Tracks node heartbeats and sensor topic
|
||||
staleness. Publishes DiagnosticArray on /saltybot/diagnostics and MQTT on
|
||||
saltybot/health. Issue #566.
|
||||
ROS2 system health monitor for SaltyBot. Central node that monitors heartbeats
|
||||
from all critical nodes, detects when nodes go down (>5s silent), triggers
|
||||
auto-restart, publishes /saltybot/system_health JSON, and alerts face display
|
||||
on critical failures.
|
||||
</description>
|
||||
<maintainer email="sl-jetson@saltylab.local">sl-jetson</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
<maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>diagnostic_msgs</depend>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<test_depend>pytest</test_depend>
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
|
||||
@ -1,385 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""sensor_health_node.py — ROS2 sensor topic health monitor (Issue #566).
|
||||
|
||||
Monitors all sensor topics for staleness. Each sensor is checked against
|
||||
configurable WARN and ERROR timeouts. Results are published as a ROS2
|
||||
DiagnosticArray and as an MQTT JSON summary.
|
||||
|
||||
Monitored topics (configurable via sensor_health_params.yaml):
|
||||
/camera/color/image_raw — RealSense colour stream
|
||||
/camera/depth/image_rect_raw — RealSense depth stream
|
||||
/camera/color/camera_info — RealSense camera info
|
||||
/scan — LiDAR 2-D scan
|
||||
/imu/data — IMU (BNO055 via JLink)
|
||||
/saltybot/uwb/range — UWB ranging
|
||||
/saltybot/battery — Battery telemetry (JSON string)
|
||||
/saltybot/motor_daemon/status — Motor daemon status
|
||||
|
||||
Published topics:
|
||||
/saltybot/diagnostics (diagnostic_msgs/DiagnosticArray) — full per-sensor status
|
||||
/saltybot/sensor_health (std_msgs/String) — JSON summary
|
||||
|
||||
MQTT:
|
||||
Topic: saltybot/health
|
||||
Payload: JSON {timestamp, overall, sensors:{name: {status, age_s, hz}}}
|
||||
|
||||
Parameters (config/sensor_health_params.yaml):
|
||||
check_hz 1.0 Health check publish rate
|
||||
mqtt_broker localhost
|
||||
mqtt_port 1883
|
||||
mqtt_topic saltybot/health
|
||||
mqtt_enabled true
|
||||
sensors list of {topic, name, warn_s, error_s, critical}
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import time
|
||||
import threading
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Dict, List, Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import (
|
||||
DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
)
|
||||
|
||||
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
|
||||
from sensor_msgs.msg import CameraInfo, Image, Imu, LaserScan
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
# ── Diagnostic level aliases ───────────────────────────────────────────────────
|
||||
|
||||
OK = DiagnosticStatus.OK # 0
|
||||
WARN = DiagnosticStatus.WARN # 1
|
||||
ERROR = DiagnosticStatus.ERROR # 2
|
||||
|
||||
_LEVEL_NAMES = {OK: "OK", WARN: "WARN", ERROR: "ERROR"}
|
||||
|
||||
# ── Default sensor configuration ──────────────────────────────────────────────
|
||||
|
||||
DEFAULT_SENSORS: List[dict] = [
|
||||
{"topic": "/camera/color/image_raw", "name": "camera_color", "warn_s": 1.0, "error_s": 3.0, "critical": True},
|
||||
{"topic": "/camera/depth/image_rect_raw", "name": "camera_depth", "warn_s": 1.0, "error_s": 3.0, "critical": True},
|
||||
{"topic": "/camera/color/camera_info", "name": "camera_info", "warn_s": 2.0, "error_s": 5.0, "critical": False},
|
||||
{"topic": "/scan", "name": "lidar", "warn_s": 1.0, "error_s": 3.0, "critical": True},
|
||||
{"topic": "/imu/data", "name": "imu", "warn_s": 0.5, "error_s": 2.0, "critical": True},
|
||||
{"topic": "/saltybot/uwb/range", "name": "uwb", "warn_s": 2.0, "error_s": 5.0, "critical": False},
|
||||
{"topic": "/saltybot/battery", "name": "battery", "warn_s": 3.0, "error_s": 8.0, "critical": False},
|
||||
{"topic": "/saltybot/motor_daemon/status", "name": "motor_daemon", "warn_s": 2.0, "error_s": 5.0, "critical": True},
|
||||
]
|
||||
|
||||
|
||||
# ── SensorWatcher ─────────────────────────────────────────────────────────────
|
||||
|
||||
class SensorWatcher:
|
||||
"""Tracks message receipt timestamps and rate for a single topic.
|
||||
|
||||
Thread-safe: callback may fire from any executor thread.
|
||||
"""
|
||||
|
||||
def __init__(self, topic: str, name: str,
|
||||
warn_s: float, error_s: float, critical: bool) -> None:
|
||||
self.topic = topic
|
||||
self.name = name
|
||||
self.warn_s = warn_s
|
||||
self.error_s = error_s
|
||||
self.critical = critical
|
||||
|
||||
self._last_rx: float = 0.0 # monotonic; 0 = never received
|
||||
self._count: int = 0
|
||||
self._rate_hz: float = 0.0
|
||||
self._rate_ts: float = time.monotonic()
|
||||
self._rate_cnt: int = 0
|
||||
self._lock = threading.Lock()
|
||||
|
||||
# ── Callback (called from subscription) ────────────────────────────────
|
||||
|
||||
def on_message(self, _msg) -> None:
|
||||
"""Record receipt of any message on this topic."""
|
||||
now = time.monotonic()
|
||||
with self._lock:
|
||||
self._last_rx = now
|
||||
self._count += 1
|
||||
self._rate_cnt += 1
|
||||
# Update rate estimate every ~2 s
|
||||
elapsed = now - self._rate_ts
|
||||
if elapsed >= 2.0:
|
||||
self._rate_hz = self._rate_cnt / elapsed
|
||||
self._rate_cnt = 0
|
||||
self._rate_ts = now
|
||||
|
||||
# ── Status query ───────────────────────────────────────────────────────
|
||||
|
||||
def age_s(self, now: Optional[float] = None) -> float:
|
||||
"""Seconds since last message (∞ if never received)."""
|
||||
if now is None:
|
||||
now = time.monotonic()
|
||||
with self._lock:
|
||||
if self._last_rx == 0.0:
|
||||
return float("inf")
|
||||
return now - self._last_rx
|
||||
|
||||
def rate_hz(self) -> float:
|
||||
with self._lock:
|
||||
return self._rate_hz
|
||||
|
||||
def msg_count(self) -> int:
|
||||
with self._lock:
|
||||
return self._count
|
||||
|
||||
def level(self, now: Optional[float] = None) -> int:
|
||||
age = self.age_s(now)
|
||||
if age >= self.error_s:
|
||||
return ERROR
|
||||
if age >= self.warn_s:
|
||||
return WARN
|
||||
return OK
|
||||
|
||||
def diagnostic_status(self, now: Optional[float] = None) -> DiagnosticStatus:
|
||||
if now is None:
|
||||
now = time.monotonic()
|
||||
age = self.age_s(now)
|
||||
lvl = self.level(now)
|
||||
hz = self.rate_hz()
|
||||
cnt = self.msg_count()
|
||||
|
||||
if age == float("inf"):
|
||||
msg = f"ERROR: no data received on {self.topic}"
|
||||
elif lvl == ERROR:
|
||||
msg = f"ERROR: stale {age:.1f}s (threshold {self.error_s:.1f}s)"
|
||||
elif lvl == WARN:
|
||||
msg = f"WARN: stale {age:.1f}s (threshold {self.warn_s:.1f}s)"
|
||||
else:
|
||||
msg = f"OK ({hz:.1f} Hz)"
|
||||
|
||||
age_str = "inf" if age == float("inf") else f"{age:.2f}"
|
||||
status = DiagnosticStatus()
|
||||
status.level = lvl
|
||||
status.name = self.name
|
||||
status.message = msg
|
||||
status.hardware_id = self.topic
|
||||
status.values = [
|
||||
KeyValue(key="topic", value=self.topic),
|
||||
KeyValue(key="age_s", value=age_str),
|
||||
KeyValue(key="rate_hz", value=f"{hz:.2f}"),
|
||||
KeyValue(key="count", value=str(cnt)),
|
||||
KeyValue(key="warn_s", value=str(self.warn_s)),
|
||||
KeyValue(key="error_s", value=str(self.error_s)),
|
||||
KeyValue(key="critical", value=str(self.critical)),
|
||||
]
|
||||
return status
|
||||
|
||||
def summary_dict(self, now: Optional[float] = None) -> dict:
|
||||
if now is None:
|
||||
now = time.monotonic()
|
||||
age = self.age_s(now)
|
||||
return {
|
||||
"status": _LEVEL_NAMES[self.level(now)],
|
||||
"age_s": round(age, 2) if age != float("inf") else None,
|
||||
"rate_hz": round(self.rate_hz(), 2),
|
||||
"count": self.msg_count(),
|
||||
"critical": self.critical,
|
||||
}
|
||||
|
||||
|
||||
# ── SensorHealthNode ───────────────────────────────────────────────────────────
|
||||
|
||||
class SensorHealthNode(Node):
|
||||
"""Monitor all sensor topics; publish DiagnosticArray + MQTT JSON."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sensor_health_node")
|
||||
|
||||
# ── Parameters ─────────────────────────────────────────────────────
|
||||
self.declare_parameter("check_hz", 1.0)
|
||||
self.declare_parameter("mqtt_broker", "localhost")
|
||||
self.declare_parameter("mqtt_port", 1883)
|
||||
self.declare_parameter("mqtt_topic", "saltybot/health")
|
||||
self.declare_parameter("mqtt_enabled", True)
|
||||
|
||||
check_hz = self.get_parameter("check_hz").value
|
||||
self._mqtt_broker = self.get_parameter("mqtt_broker").value
|
||||
self._mqtt_port = int(self.get_parameter("mqtt_port").value)
|
||||
self._mqtt_topic = self.get_parameter("mqtt_topic").value
|
||||
mqtt_enabled = self.get_parameter("mqtt_enabled").value
|
||||
|
||||
# ── Build sensor watchers ───────────────────────────────────────────
|
||||
self._watchers: Dict[str, SensorWatcher] = {}
|
||||
for cfg in DEFAULT_SENSORS:
|
||||
w = SensorWatcher(
|
||||
topic = cfg["topic"],
|
||||
name = cfg["name"],
|
||||
warn_s = float(cfg.get("warn_s", 1.0)),
|
||||
error_s = float(cfg.get("error_s", 3.0)),
|
||||
critical = bool(cfg.get("critical", False)),
|
||||
)
|
||||
self._watchers[cfg["name"]] = w
|
||||
|
||||
# ── Subscriptions — one per sensor type ────────────────────────────
|
||||
# Best-effort QoS for sensor data (sensors may use BEST_EFFORT publishers)
|
||||
be_qos = QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST, depth=1,
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
durability=DurabilityPolicy.VOLATILE,
|
||||
)
|
||||
rel_qos = QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST, depth=5,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
)
|
||||
|
||||
self._subscribe(Image, "/camera/color/image_raw", "camera_color", be_qos)
|
||||
self._subscribe(Image, "/camera/depth/image_rect_raw", "camera_depth", be_qos)
|
||||
self._subscribe(CameraInfo, "/camera/color/camera_info", "camera_info", be_qos)
|
||||
self._subscribe(LaserScan, "/scan", "lidar", be_qos)
|
||||
self._subscribe(Imu, "/imu/data", "imu", be_qos)
|
||||
self._subscribe(String, "/saltybot/uwb/range", "uwb", rel_qos)
|
||||
self._subscribe(String, "/saltybot/battery", "battery", rel_qos)
|
||||
self._subscribe(String, "/saltybot/motor_daemon/status", "motor_daemon", rel_qos)
|
||||
|
||||
# ── Publishers ─────────────────────────────────────────────────────
|
||||
self._pub_diag = self.create_publisher(
|
||||
DiagnosticArray, "/saltybot/diagnostics", 10)
|
||||
self._pub_health = self.create_publisher(
|
||||
String, "/saltybot/sensor_health", 10)
|
||||
|
||||
# ── MQTT ───────────────────────────────────────────────────────────
|
||||
self._mqtt_client = None
|
||||
if mqtt_enabled:
|
||||
self._connect_mqtt()
|
||||
|
||||
# ── Timer ──────────────────────────────────────────────────────────
|
||||
self.create_timer(1.0 / max(0.1, check_hz), self._publish_diagnostics)
|
||||
|
||||
sensor_list = ", ".join(self._watchers.keys())
|
||||
self.get_logger().info(
|
||||
f"[sensor_health] monitoring {len(self._watchers)} sensors: {sensor_list}"
|
||||
)
|
||||
|
||||
# ── Subscription helper ────────────────────────────────────────────────
|
||||
|
||||
def _subscribe(self, msg_type, topic: str, name: str, qos) -> None:
|
||||
if name not in self._watchers:
|
||||
return
|
||||
watcher = self._watchers[name]
|
||||
self.create_subscription(msg_type, topic, watcher.on_message, qos)
|
||||
|
||||
# ── MQTT ───────────────────────────────────────────────────────────────
|
||||
|
||||
def _connect_mqtt(self) -> None:
|
||||
try:
|
||||
import paho.mqtt.client as mqtt # type: ignore
|
||||
self._mqtt_client = mqtt.Client(client_id="saltybot_sensor_health")
|
||||
self._mqtt_client.on_connect = self._on_mqtt_connect
|
||||
self._mqtt_client.connect_async(self._mqtt_broker, self._mqtt_port, 60)
|
||||
self._mqtt_client.loop_start()
|
||||
except ImportError:
|
||||
self.get_logger().warn(
|
||||
"[sensor_health] paho-mqtt not installed — MQTT publishing disabled"
|
||||
)
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f"[sensor_health] MQTT connect failed: {e}")
|
||||
|
||||
def _on_mqtt_connect(self, client, userdata, flags, rc) -> None:
|
||||
if rc == 0:
|
||||
self.get_logger().info(
|
||||
f"[sensor_health] MQTT connected to {self._mqtt_broker}:{self._mqtt_port}"
|
||||
)
|
||||
else:
|
||||
self.get_logger().warn(f"[sensor_health] MQTT connect rc={rc}")
|
||||
|
||||
def _publish_mqtt(self, payload: str) -> None:
|
||||
if self._mqtt_client is None:
|
||||
return
|
||||
try:
|
||||
self._mqtt_client.publish(self._mqtt_topic, payload, qos=0, retain=True)
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f"[sensor_health] MQTT publish error: {e}")
|
||||
|
||||
# ── Diagnostic publisher ───────────────────────────────────────────────
|
||||
|
||||
def _publish_diagnostics(self) -> None:
|
||||
now = time.monotonic()
|
||||
wall = time.time()
|
||||
|
||||
# Build DiagnosticArray
|
||||
diag_array = DiagnosticArray()
|
||||
diag_array.header.stamp = self.get_clock().now().to_msg()
|
||||
|
||||
sensor_summaries: dict = {}
|
||||
worst_level = OK
|
||||
|
||||
for name, watcher in self._watchers.items():
|
||||
ds = watcher.diagnostic_status(now)
|
||||
diag_array.status.append(ds)
|
||||
|
||||
if ds.level > worst_level:
|
||||
worst_level = ds.level
|
||||
|
||||
sensor_summaries[name] = watcher.summary_dict(now)
|
||||
|
||||
# Overall system status
|
||||
n_error = sum(1 for w in self._watchers.values() if w.level(now) == ERROR)
|
||||
n_warn = sum(1 for w in self._watchers.values() if w.level(now) == WARN)
|
||||
crit_err = [n for n, w in self._watchers.items()
|
||||
if w.critical and w.level(now) == ERROR]
|
||||
|
||||
overall = "OK"
|
||||
if crit_err:
|
||||
overall = "ERROR"
|
||||
elif n_error > 0:
|
||||
overall = "ERROR"
|
||||
elif n_warn > 0:
|
||||
overall = "WARN"
|
||||
|
||||
# Publish DiagnosticArray
|
||||
self._pub_diag.publish(diag_array)
|
||||
|
||||
# JSON summary
|
||||
summary = {
|
||||
"timestamp": wall,
|
||||
"overall": overall,
|
||||
"n_ok": len(self._watchers) - n_error - n_warn,
|
||||
"n_warn": n_warn,
|
||||
"n_error": n_error,
|
||||
"critical_err": crit_err,
|
||||
"sensors": sensor_summaries,
|
||||
}
|
||||
payload = json.dumps(summary)
|
||||
self._pub_health.publish(String(data=payload))
|
||||
self._publish_mqtt(payload)
|
||||
|
||||
# Log on transitions or errors
|
||||
if worst_level >= ERROR:
|
||||
self.get_logger().warn(
|
||||
f"[sensor_health] {overall}: {n_error} error(s), {n_warn} warn(s)"
|
||||
+ (f" — critical: {crit_err}" if crit_err else "")
|
||||
)
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
if self._mqtt_client is not None:
|
||||
try:
|
||||
self._mqtt_client.loop_stop()
|
||||
self._mqtt_client.disconnect()
|
||||
except Exception:
|
||||
pass
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = SensorHealthNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -4,34 +4,27 @@ package_name = "saltybot_health_monitor"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.2.0",
|
||||
version="0.1.0",
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
|
||||
(f"share/{package_name}", ["package.xml"]),
|
||||
(f"share/{package_name}/launch", [
|
||||
"launch/health_monitor.launch.py",
|
||||
"launch/sensor_health.launch.py",
|
||||
]),
|
||||
(f"share/{package_name}/config", [
|
||||
"config/health_config.yaml",
|
||||
"config/sensor_health_params.yaml",
|
||||
]),
|
||||
(f"share/{package_name}/launch", ["launch/health_monitor.launch.py"]),
|
||||
(f"share/{package_name}/config", ["config/health_config.yaml"]),
|
||||
],
|
||||
install_requires=["setuptools", "pyyaml", "paho-mqtt"],
|
||||
install_requires=["setuptools", "pyyaml"],
|
||||
zip_safe=True,
|
||||
maintainer="sl-jetson",
|
||||
maintainer_email="sl-jetson@saltylab.local",
|
||||
maintainer="sl-controls",
|
||||
maintainer_email="sl-controls@saltylab.local",
|
||||
description=(
|
||||
"System health monitor: node heartbeats + sensor topic staleness "
|
||||
"detection with DiagnosticArray and MQTT (Issue #566)"
|
||||
"System health monitor: tracks node heartbeats, detects down nodes, "
|
||||
"triggers auto-restart, publishes system health status"
|
||||
),
|
||||
license="Apache-2.0",
|
||||
license="MIT",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"health_monitor_node = saltybot_health_monitor.health_monitor_node:main",
|
||||
"sensor_health_node = saltybot_health_monitor.sensor_health_node:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
@ -1,344 +0,0 @@
|
||||
"""test_sensor_health.py — Unit tests for sensor_health_node (Issue #566).
|
||||
|
||||
Runs entirely offline: no ROS2 runtime, no hardware, no MQTT broker required.
|
||||
"""
|
||||
|
||||
import json
|
||||
import sys
|
||||
import time
|
||||
import types
|
||||
import unittest
|
||||
from unittest.mock import MagicMock, patch
|
||||
|
||||
# ── Stub ROS2 and sensor_msgs so tests run offline ────────────────────────────
|
||||
|
||||
def _install_stubs():
|
||||
# rclpy
|
||||
rclpy = types.ModuleType("rclpy")
|
||||
rclpy.init = lambda **_: None
|
||||
rclpy.spin = lambda _: None
|
||||
rclpy.shutdown = lambda: None
|
||||
|
||||
node_mod = types.ModuleType("rclpy.node")
|
||||
class _Node:
|
||||
def __init__(self, *a, **kw): pass
|
||||
def declare_parameter(self, *a, **kw): pass
|
||||
def get_parameter(self, name):
|
||||
defaults = {
|
||||
"check_hz": 1.0, "mqtt_broker": "localhost",
|
||||
"mqtt_port": 1883, "mqtt_topic": "saltybot/health",
|
||||
"mqtt_enabled": False,
|
||||
}
|
||||
m = MagicMock(); m.value = defaults.get(name, False)
|
||||
return m
|
||||
def get_logger(self): return MagicMock()
|
||||
def get_clock(self): return MagicMock()
|
||||
def create_subscription(self, *a, **kw): pass
|
||||
def create_publisher(self, *a, **kw): return MagicMock()
|
||||
def create_timer(self, *a, **kw): pass
|
||||
def destroy_node(self): pass
|
||||
node_mod.Node = _Node
|
||||
rclpy.node = node_mod
|
||||
|
||||
qos_mod = types.ModuleType("rclpy.qos")
|
||||
for attr in ("QoSProfile", "HistoryPolicy", "ReliabilityPolicy", "DurabilityPolicy"):
|
||||
setattr(qos_mod, attr, MagicMock())
|
||||
rclpy.qos = qos_mod
|
||||
|
||||
sys.modules.setdefault("rclpy", rclpy)
|
||||
sys.modules.setdefault("rclpy.node", rclpy.node)
|
||||
sys.modules.setdefault("rclpy.qos", rclpy.qos)
|
||||
|
||||
# diagnostic_msgs
|
||||
diag = types.ModuleType("diagnostic_msgs")
|
||||
diag_msg = types.ModuleType("diagnostic_msgs.msg")
|
||||
|
||||
class _DiagStatus:
|
||||
OK = 0
|
||||
WARN = 1
|
||||
ERROR = 2
|
||||
def __init__(self):
|
||||
self.level = 0; self.name = ""; self.message = ""
|
||||
self.hardware_id = ""; self.values = []
|
||||
|
||||
class _DiagArray:
|
||||
def __init__(self):
|
||||
self.header = MagicMock(); self.status = []
|
||||
|
||||
class _KeyValue:
|
||||
def __init__(self, key="", value=""):
|
||||
self.key = key; self.value = value
|
||||
|
||||
diag_msg.DiagnosticStatus = _DiagStatus
|
||||
diag_msg.DiagnosticArray = _DiagArray
|
||||
diag_msg.KeyValue = _KeyValue
|
||||
diag.msg = diag_msg
|
||||
sys.modules.setdefault("diagnostic_msgs", diag)
|
||||
sys.modules.setdefault("diagnostic_msgs.msg", diag_msg)
|
||||
|
||||
# sensor_msgs
|
||||
sens = types.ModuleType("sensor_msgs")
|
||||
sens_msg = types.ModuleType("sensor_msgs.msg")
|
||||
for cls_name in ("Image", "CameraInfo", "Imu", "LaserScan"):
|
||||
setattr(sens_msg, cls_name, MagicMock)
|
||||
sens.msg = sens_msg
|
||||
sys.modules.setdefault("sensor_msgs", sens)
|
||||
sys.modules.setdefault("sensor_msgs.msg", sens_msg)
|
||||
|
||||
# std_msgs
|
||||
std = types.ModuleType("std_msgs")
|
||||
std_msg = types.ModuleType("std_msgs.msg")
|
||||
class _String:
|
||||
def __init__(self, data=""): self.data = data
|
||||
std_msg.String = _String
|
||||
std.msg = std_msg
|
||||
sys.modules.setdefault("std_msgs", std)
|
||||
sys.modules.setdefault("std_msgs.msg", std_msg)
|
||||
|
||||
|
||||
_install_stubs()
|
||||
|
||||
from saltybot_health_monitor.sensor_health_node import ( # noqa: E402
|
||||
SensorWatcher, OK, WARN, ERROR, _LEVEL_NAMES,
|
||||
)
|
||||
|
||||
|
||||
# ── SensorWatcher: initial state ──────────────────────────────────────────────
|
||||
|
||||
class TestSensorWatcherInitial(unittest.TestCase):
|
||||
|
||||
def setUp(self):
|
||||
self.w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=True)
|
||||
|
||||
def test_initial_age_is_inf(self):
|
||||
self.assertEqual(self.w.age_s(), float("inf"))
|
||||
|
||||
def test_initial_level_is_error(self):
|
||||
# Never received → age=inf ≥ error_s → ERROR
|
||||
self.assertEqual(self.w.level(), ERROR)
|
||||
|
||||
def test_initial_count_zero(self):
|
||||
self.assertEqual(self.w.msg_count(), 0)
|
||||
|
||||
def test_initial_rate_zero(self):
|
||||
self.assertAlmostEqual(self.w.rate_hz(), 0.0)
|
||||
|
||||
def test_name_stored(self):
|
||||
self.assertEqual(self.w.name, "lidar")
|
||||
|
||||
def test_topic_stored(self):
|
||||
self.assertEqual(self.w.topic, "/scan")
|
||||
|
||||
def test_critical_stored(self):
|
||||
self.assertTrue(self.w.critical)
|
||||
|
||||
|
||||
# ── SensorWatcher: after receiving messages ───────────────────────────────────
|
||||
|
||||
class TestSensorWatcherAfterMessage(unittest.TestCase):
|
||||
|
||||
def setUp(self):
|
||||
self.w = SensorWatcher("/imu/data", "imu", warn_s=0.5, error_s=2.0, critical=True)
|
||||
self.w.on_message(None) # simulate message receipt
|
||||
|
||||
def test_age_is_small_after_message(self):
|
||||
self.assertLess(self.w.age_s(), 0.1)
|
||||
|
||||
def test_level_ok_immediately_after(self):
|
||||
self.assertEqual(self.w.level(), OK)
|
||||
|
||||
def test_count_increments(self):
|
||||
self.w.on_message(None)
|
||||
self.assertEqual(self.w.msg_count(), 2)
|
||||
|
||||
def test_multiple_messages(self):
|
||||
for _ in range(10):
|
||||
self.w.on_message(None)
|
||||
self.assertEqual(self.w.msg_count(), 11)
|
||||
|
||||
|
||||
# ── SensorWatcher: staleness thresholds ──────────────────────────────────────
|
||||
|
||||
class TestSensorWatcherStaleness(unittest.TestCase):
|
||||
|
||||
def _make_stale(self, seconds_ago: float) -> SensorWatcher:
|
||||
"""Return a watcher whose last_rx was `seconds_ago` seconds in the past."""
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
w.on_message(None)
|
||||
# Backdate the last_rx directly
|
||||
with w._lock:
|
||||
w._last_rx -= seconds_ago
|
||||
return w
|
||||
|
||||
def test_ok_when_fresh(self):
|
||||
w = self._make_stale(0.5)
|
||||
self.assertEqual(w.level(), OK)
|
||||
|
||||
def test_warn_at_warn_threshold(self):
|
||||
w = self._make_stale(1.1)
|
||||
self.assertEqual(w.level(), WARN)
|
||||
|
||||
def test_error_at_error_threshold(self):
|
||||
w = self._make_stale(3.1)
|
||||
self.assertEqual(w.level(), ERROR)
|
||||
|
||||
def test_age_matches_backdated_time(self):
|
||||
w = self._make_stale(2.0)
|
||||
self.assertAlmostEqual(w.age_s(), 2.0, delta=0.1)
|
||||
|
||||
def test_warn_level_between_thresholds(self):
|
||||
w = self._make_stale(2.0) # 1.0 < 2.0 < 3.0
|
||||
self.assertEqual(w.level(), WARN)
|
||||
|
||||
|
||||
# ── SensorWatcher: diagnostic_status output ──────────────────────────────────
|
||||
|
||||
class TestSensorWatcherDiagStatus(unittest.TestCase):
|
||||
|
||||
def test_never_received_is_error(self):
|
||||
w = SensorWatcher("/camera/color/image_raw", "camera_color",
|
||||
warn_s=1.0, error_s=3.0, critical=True)
|
||||
ds = w.diagnostic_status()
|
||||
self.assertEqual(ds.level, ERROR)
|
||||
self.assertIn("no data", ds.message)
|
||||
|
||||
def test_ok_status_message(self):
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
w.on_message(None)
|
||||
ds = w.diagnostic_status()
|
||||
self.assertEqual(ds.level, OK)
|
||||
self.assertIn("OK", ds.message)
|
||||
|
||||
def test_warn_status_message(self):
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
w.on_message(None)
|
||||
with w._lock:
|
||||
w._last_rx -= 1.5
|
||||
ds = w.diagnostic_status()
|
||||
self.assertEqual(ds.level, WARN)
|
||||
self.assertIn("WARN", ds.message)
|
||||
|
||||
def test_hardware_id_is_topic(self):
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
w.on_message(None)
|
||||
ds = w.diagnostic_status()
|
||||
self.assertEqual(ds.hardware_id, "/scan")
|
||||
|
||||
def test_kv_keys_present(self):
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
w.on_message(None)
|
||||
ds = w.diagnostic_status()
|
||||
kv_keys = {kv.key for kv in ds.values}
|
||||
for expected in ("topic", "age_s", "rate_hz", "count", "warn_s", "error_s"):
|
||||
self.assertIn(expected, kv_keys)
|
||||
|
||||
def test_age_inf_displayed_as_inf(self):
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
ds = w.diagnostic_status()
|
||||
kv = {kv.key: kv.value for kv in ds.values}
|
||||
self.assertEqual(kv["age_s"], "inf")
|
||||
|
||||
|
||||
# ── SensorWatcher: summary_dict output ───────────────────────────────────────
|
||||
|
||||
class TestSensorWatcherSummaryDict(unittest.TestCase):
|
||||
|
||||
def test_never_received_age_is_none(self):
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
d = w.summary_dict()
|
||||
self.assertIsNone(d["age_s"])
|
||||
self.assertEqual(d["status"], "ERROR")
|
||||
|
||||
def test_ok_status_string(self):
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
w.on_message(None)
|
||||
d = w.summary_dict()
|
||||
self.assertEqual(d["status"], "OK")
|
||||
|
||||
def test_warn_status_string(self):
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
w.on_message(None)
|
||||
with w._lock:
|
||||
w._last_rx -= 1.5
|
||||
d = w.summary_dict()
|
||||
self.assertEqual(d["status"], "WARN")
|
||||
|
||||
def test_error_status_string(self):
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
w.on_message(None)
|
||||
with w._lock:
|
||||
w._last_rx -= 5.0
|
||||
d = w.summary_dict()
|
||||
self.assertEqual(d["status"], "ERROR")
|
||||
|
||||
def test_age_rounded(self):
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
w.on_message(None)
|
||||
d = w.summary_dict()
|
||||
self.assertIsInstance(d["age_s"], float)
|
||||
|
||||
def test_critical_flag(self):
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=True)
|
||||
self.assertTrue(w.summary_dict()["critical"])
|
||||
|
||||
def test_non_critical_flag(self):
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
self.assertFalse(w.summary_dict()["critical"])
|
||||
|
||||
def test_count_in_summary(self):
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
for _ in range(5):
|
||||
w.on_message(None)
|
||||
self.assertEqual(w.summary_dict()["count"], 5)
|
||||
|
||||
|
||||
# ── Level name mapping ─────────────────────────────────────────────────────────
|
||||
|
||||
class TestLevelNames(unittest.TestCase):
|
||||
|
||||
def test_ok_name(self):
|
||||
self.assertEqual(_LEVEL_NAMES[OK], "OK")
|
||||
|
||||
def test_warn_name(self):
|
||||
self.assertEqual(_LEVEL_NAMES[WARN], "WARN")
|
||||
|
||||
def test_error_name(self):
|
||||
self.assertEqual(_LEVEL_NAMES[ERROR], "ERROR")
|
||||
|
||||
|
||||
# ── Thread safety ─────────────────────────────────────────────────────────────
|
||||
|
||||
class TestSensorWatcherThreadSafety(unittest.TestCase):
|
||||
|
||||
def test_concurrent_messages(self):
|
||||
import threading
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
N = 500
|
||||
threads = [threading.Thread(target=w.on_message, args=(None,)) for _ in range(N)]
|
||||
for t in threads:
|
||||
t.start()
|
||||
for t in threads:
|
||||
t.join()
|
||||
self.assertEqual(w.msg_count(), N)
|
||||
|
||||
def test_concurrent_reads(self):
|
||||
import threading
|
||||
w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
|
||||
w.on_message(None)
|
||||
errors = []
|
||||
def read_loop():
|
||||
for _ in range(100):
|
||||
try:
|
||||
w.level()
|
||||
w.age_s()
|
||||
w.rate_hz()
|
||||
except Exception as e:
|
||||
errors.append(e)
|
||||
threads = [threading.Thread(target=read_loop) for _ in range(5)]
|
||||
for t in threads: t.start()
|
||||
for t in threads: t.join()
|
||||
self.assertEqual(errors, [])
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@ -1,58 +0,0 @@
|
||||
# uwb_position_params.yaml — UWB position node configuration (Issue #546)
|
||||
#
|
||||
# ROS2 Python node: saltybot_uwb_position
|
||||
# Serial: ESP32 UWB tag → Jetson via USB-CDC (JSON per line)
|
||||
#
|
||||
# Usage:
|
||||
# ros2 launch saltybot_uwb_position uwb_position.launch.py
|
||||
#
|
||||
# JSON protocol (ESP32 → Jetson):
|
||||
# Full frame (preferred):
|
||||
# {"ts": 123456, "ranges": [{"id": 0, "d_mm": 1500, "rssi": -65.0}, ...]}
|
||||
# Per-anchor:
|
||||
# {"id": 0, "d_mm": 1500, "rssi": -65.0}
|
||||
# Both "d_mm" and "range_mm" accepted for the range field.
|
||||
|
||||
uwb_position:
|
||||
ros__parameters:
|
||||
|
||||
# ── Serial (USB-CDC from ESP32 DW3000 tag) ──────────────────────────────
|
||||
serial_port: /dev/ttyUSB0 # udev rule: /dev/ttyUWB_TAG recommended
|
||||
baudrate: 115200
|
||||
|
||||
# ── Anchor definitions ──────────────────────────────────────────────────
|
||||
# anchor_ids: integer list of anchor IDs
|
||||
# anchor_positions: flat float list, 3 values per anchor [x, y, z] in
|
||||
# the map frame (metres). Length must be 3 × len(anchor_ids).
|
||||
#
|
||||
# Example: 4-anchor rectangular room layout, anchors at 2 m height
|
||||
# Corner NW (0,0), NE (5,0), SE (5,5), SW (0,5)
|
||||
anchor_ids: [0, 1, 2, 3]
|
||||
anchor_positions: [
|
||||
0.0, 0.0, 2.0, # anchor 0 — NW corner
|
||||
5.0, 0.0, 2.0, # anchor 1 — NE corner
|
||||
5.0, 5.0, 2.0, # anchor 2 — SE corner
|
||||
0.0, 5.0, 2.0, # anchor 3 — SW corner
|
||||
]
|
||||
|
||||
# ── Trilateration mode ──────────────────────────────────────────────────
|
||||
solve_z: false # false = 2D (robot_z fixed), true = full 3D (needs 4+ anchors)
|
||||
robot_z: 0.0 # m — robot floor height in map frame (used when solve_z=false)
|
||||
|
||||
# ── Validity & timing ───────────────────────────────────────────────────
|
||||
publish_rate: 10.0 # Hz — /saltybot/uwb/pose + /saltybot/uwb/status
|
||||
range_timeout_s: 1.5 # s — discard anchor range after this age
|
||||
min_range_m: 0.05 # m — minimum valid range
|
||||
max_range_m: 30.0 # m — maximum valid range (DW3000: up to 120 m, use conservative)
|
||||
|
||||
# ── Outlier rejection ───────────────────────────────────────────────────
|
||||
outlier_threshold_m: 0.40 # m — residual above this → outlier
|
||||
outlier_strikes_max: 5 # consecutive outlier frames before anchor is flagged
|
||||
|
||||
# ── Kalman filter ───────────────────────────────────────────────────────
|
||||
kf_process_noise: 0.05 # Q — lower = smoother but slower to respond
|
||||
kf_meas_noise: 0.10 # R — higher = trust KF prediction more than measurement
|
||||
|
||||
# ── TF2 frames ──────────────────────────────────────────────────────────
|
||||
map_frame: map # parent frame
|
||||
uwb_frame: uwb_link # child frame (robot UWB tag position)
|
||||
@ -1,35 +0,0 @@
|
||||
"""Launch file for saltybot_uwb_position (Issue #546)."""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
config_path = os.path.join(
|
||||
get_package_share_directory("saltybot_uwb_position"),
|
||||
"config",
|
||||
"uwb_position_params.yaml",
|
||||
)
|
||||
|
||||
port_arg = DeclareLaunchArgument(
|
||||
"serial_port",
|
||||
default_value="/dev/ttyUSB0",
|
||||
description="USB serial port for ESP32 UWB tag (e.g. /dev/ttyUWB_TAG)",
|
||||
)
|
||||
|
||||
uwb_node = Node(
|
||||
package="saltybot_uwb_position",
|
||||
executable="uwb_position",
|
||||
name="uwb_position",
|
||||
parameters=[
|
||||
config_path,
|
||||
{"serial_port": LaunchConfiguration("serial_port")},
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
return LaunchDescription([port_arg, uwb_node])
|
||||
@ -1,36 +0,0 @@
|
||||
<?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_uwb_position</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
ROS2 UWB position node for Jetson (Issue #546).
|
||||
Reads JSON range data from an ESP32 DW3000 UWB tag over USB serial,
|
||||
trilaterates from 3+ fixed infrastructure anchors, publishes
|
||||
PoseStamped on /saltybot/uwb/pose, per-anchor UwbRange on
|
||||
/saltybot/uwb/range/<id>, JSON diagnostics on /saltybot/uwb/status,
|
||||
and broadcasts the uwb_link→map TF2 transform.
|
||||
</description>
|
||||
<maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>saltybot_uwb_msgs</depend>
|
||||
|
||||
<!-- numpy is a system dep on Jetson (python3-numpy) -->
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,254 +0,0 @@
|
||||
"""
|
||||
trilateration.py — Pure-math helpers for UWB trilateration (Issue #546).
|
||||
|
||||
No ROS2 dependencies — importable in unit tests without a ROS2 install.
|
||||
|
||||
Algorithm: linear least-squares from N ≥ 3 anchors
|
||||
────────────────────────────────────────────────────
|
||||
Given anchors A_i = (x_i, y_i, z_i) with measured ranges r_i, the robot
|
||||
position p = (x, y, z) satisfies:
|
||||
|
||||
(x - x_i)² + (y - y_i)² + (z - z_i)² = r_i²
|
||||
|
||||
Subtract the equation for anchor 0 from each subsequent equation to
|
||||
linearise:
|
||||
|
||||
2(x_i - x_0)x + 2(y_i - y_0)y + 2(z_i - z_0)z
|
||||
= r_0² - r_i² + ‖a_i‖² - ‖a_0‖²
|
||||
|
||||
This yields A·p = b where A is (N-1)×3 and b is (N-1)×1.
|
||||
Solve with numpy least-squares (lstsq).
|
||||
|
||||
2D mode (solve_z=False):
|
||||
z is fixed (robot_z parameter, default 0).
|
||||
Reduce to 2 unknowns (x, y): subtract z terms from b, drop the z column.
|
||||
Needs N ≥ 3 anchors.
|
||||
|
||||
Outlier rejection:
|
||||
Compute residual for each anchor: |r_meas - ‖p - a_i‖|.
|
||||
Reject anchors with residual > threshold. Repeat if enough remain.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
AnchorPos = Tuple[float, float, float] # (x, y, z) in map frame (metres)
|
||||
RangeM = float # measured range (metres)
|
||||
|
||||
|
||||
# ── Core trilateration ────────────────────────────────────────────────────────
|
||||
|
||||
def trilaterate(
|
||||
anchors: List[AnchorPos],
|
||||
ranges: List[RangeM],
|
||||
fixed_z: Optional[float] = None,
|
||||
) -> Tuple[float, float, float]:
|
||||
"""Least-squares trilateration from N ≥ 3 anchor ranges.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
anchors : list of (x, y, z) anchor positions in the map frame (metres)
|
||||
ranges : measured distances from robot to each anchor (metres)
|
||||
fixed_z : if not None, treat robot z as this value and solve 2D only
|
||||
|
||||
Returns
|
||||
-------
|
||||
(x, y, z) : estimated robot position in the map frame (metres).
|
||||
When fixed_z is given, z = fixed_z.
|
||||
|
||||
Raises
|
||||
------
|
||||
ValueError : fewer than 3 anchors provided
|
||||
RuntimeError: linear system is rank-deficient (e.g., collinear anchors)
|
||||
"""
|
||||
n = len(anchors)
|
||||
if n < 3:
|
||||
raise ValueError(f"Need ≥ 3 anchors for trilateration, got {n}")
|
||||
if len(ranges) != n:
|
||||
raise ValueError("len(anchors) != len(ranges)")
|
||||
|
||||
a = np.array(anchors, dtype=np.float64) # (N, 3)
|
||||
r = np.array(ranges, dtype=np.float64) # (N,)
|
||||
|
||||
# Reference anchor (index 0)
|
||||
a0 = a[0]
|
||||
r0 = r[0]
|
||||
norm_a0_sq = float(np.dot(a0, a0))
|
||||
|
||||
if fixed_z is not None:
|
||||
# 2D mode: solve for (x, y), z is known
|
||||
z = fixed_z
|
||||
A_rows = []
|
||||
b_rows = []
|
||||
for i in range(1, n):
|
||||
ai = a[i]
|
||||
ri = r[i]
|
||||
norm_ai_sq = float(np.dot(ai, ai))
|
||||
# rhs adjusted for known z
|
||||
rhs = (r0**2 - ri**2 + norm_ai_sq - norm_a0_sq
|
||||
+ 2.0 * (ai[2] - a0[2]) * z)
|
||||
A_rows.append([2.0 * (ai[0] - a0[0]),
|
||||
2.0 * (ai[1] - a0[1])])
|
||||
b_rows.append(rhs)
|
||||
|
||||
A_mat = np.array(A_rows, dtype=np.float64)
|
||||
b_vec = np.array(b_rows, dtype=np.float64)
|
||||
result, _, rank, _ = np.linalg.lstsq(A_mat, b_vec, rcond=None)
|
||||
if rank < 2:
|
||||
raise RuntimeError("Rank-deficient 2D trilateration system — collinear anchors?")
|
||||
return float(result[0]), float(result[1]), z
|
||||
|
||||
else:
|
||||
# 3D mode: solve for (x, y, z) — needs N ≥ 4 for well-conditioned solve
|
||||
A_rows = []
|
||||
b_rows = []
|
||||
for i in range(1, n):
|
||||
ai = a[i]
|
||||
ri = r[i]
|
||||
norm_ai_sq = float(np.dot(ai, ai))
|
||||
rhs = r0**2 - ri**2 + norm_ai_sq - norm_a0_sq
|
||||
A_rows.append([2.0 * (ai[0] - a0[0]),
|
||||
2.0 * (ai[1] - a0[1]),
|
||||
2.0 * (ai[2] - a0[2])])
|
||||
b_rows.append(rhs)
|
||||
|
||||
A_mat = np.array(A_rows, dtype=np.float64)
|
||||
b_vec = np.array(b_rows, dtype=np.float64)
|
||||
result, _, rank, _ = np.linalg.lstsq(A_mat, b_vec, rcond=None)
|
||||
if rank < 3:
|
||||
raise RuntimeError("Rank-deficient 3D trilateration system — coplanar anchors?")
|
||||
return float(result[0]), float(result[1]), float(result[2])
|
||||
|
||||
|
||||
# ── Outlier rejection ─────────────────────────────────────────────────────────
|
||||
|
||||
def reject_outliers(
|
||||
anchors: List[AnchorPos],
|
||||
ranges: List[RangeM],
|
||||
position: Tuple[float, float, float],
|
||||
threshold_m: float = 0.4,
|
||||
) -> List[int]:
|
||||
"""Return indices of inlier anchors (residual ≤ threshold_m).
|
||||
|
||||
Parameters
|
||||
----------
|
||||
anchors : anchor positions in map frame
|
||||
ranges : measured ranges (metres)
|
||||
position : current position estimate (x, y, z)
|
||||
threshold_m : max allowable range residual to be an inlier
|
||||
|
||||
Returns
|
||||
-------
|
||||
inlier_indices : sorted list of indices whose residual is within threshold
|
||||
"""
|
||||
px, py, pz = position
|
||||
inliers = []
|
||||
for i, (anchor, r_meas) in enumerate(zip(anchors, ranges)):
|
||||
ax, ay, az = anchor
|
||||
r_pred = math.sqrt((px - ax)**2 + (py - ay)**2 + (pz - az)**2)
|
||||
residual = abs(r_meas - r_pred)
|
||||
if residual <= threshold_m:
|
||||
inliers.append(i)
|
||||
return inliers
|
||||
|
||||
|
||||
def residuals(
|
||||
anchors: List[AnchorPos],
|
||||
ranges: List[RangeM],
|
||||
position: Tuple[float, float, float],
|
||||
) -> List[float]:
|
||||
"""Compute per-anchor range residual (metres) for diagnostics."""
|
||||
px, py, pz = position
|
||||
res = []
|
||||
for anchor, r_meas in zip(anchors, ranges):
|
||||
ax, ay, az = anchor
|
||||
r_pred = math.sqrt((px - ax)**2 + (py - ay)**2 + (pz - az)**2)
|
||||
res.append(abs(r_meas - r_pred))
|
||||
return res
|
||||
|
||||
|
||||
# ── 3D Kalman filter (constant-velocity, position-only observations) ──────────
|
||||
|
||||
class KalmanFilter3D:
|
||||
"""Constant-velocity Kalman filter for 3D UWB position.
|
||||
|
||||
State: [x, y, z, vx, vy, vz]ᵀ
|
||||
Observation: [x, y, z] (position only)
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
process_noise: float = 0.1,
|
||||
measurement_noise: float = 0.15,
|
||||
dt: float = 0.1,
|
||||
) -> None:
|
||||
self._q = process_noise
|
||||
self._r = measurement_noise
|
||||
self._dt = dt
|
||||
self._x = np.zeros(6)
|
||||
self._P = np.diag([1.0, 1.0, 1.0, 0.5, 0.5, 0.5])
|
||||
self._init = False
|
||||
|
||||
def _build_F(self, dt: float) -> np.ndarray:
|
||||
F = np.eye(6)
|
||||
F[0, 3] = dt
|
||||
F[1, 4] = dt
|
||||
F[2, 5] = dt
|
||||
return F
|
||||
|
||||
def _build_Q(self, dt: float) -> np.ndarray:
|
||||
q = self._q
|
||||
dt2 = dt * dt
|
||||
dt3 = dt2 * dt
|
||||
dt4 = dt3 * dt
|
||||
Q = np.zeros((6, 6))
|
||||
# Position-velocity cross terms (constant white-noise model)
|
||||
for i in range(3):
|
||||
Q[i, i ] = q * dt4 / 4.0
|
||||
Q[i, i+3] = q * dt3 / 2.0
|
||||
Q[i+3, i ] = q * dt3 / 2.0
|
||||
Q[i+3, i+3] = q * dt2
|
||||
return Q
|
||||
|
||||
def predict(self, dt: float | None = None) -> None:
|
||||
if dt is not None:
|
||||
self._dt = dt
|
||||
F = self._build_F(self._dt)
|
||||
Q = self._build_Q(self._dt)
|
||||
self._x = F @ self._x
|
||||
self._P = F @ self._P @ F.T + Q
|
||||
|
||||
def update(self, x_m: float, y_m: float, z_m: float) -> None:
|
||||
if not self._init:
|
||||
self._x[:3] = [x_m, y_m, z_m]
|
||||
self._init = True
|
||||
return
|
||||
|
||||
H = np.zeros((3, 6))
|
||||
H[0, 0] = 1.0
|
||||
H[1, 1] = 1.0
|
||||
H[2, 2] = 1.0
|
||||
R = np.eye(3) * self._r
|
||||
|
||||
z_vec = np.array([x_m, y_m, z_m])
|
||||
innov = z_vec - H @ self._x
|
||||
S = H @ self._P @ H.T + R
|
||||
K = self._P @ H.T @ np.linalg.inv(S)
|
||||
self._x = self._x + K @ innov
|
||||
self._P = (np.eye(6) - K @ H) @ self._P
|
||||
|
||||
def position(self) -> Tuple[float, float, float]:
|
||||
return float(self._x[0]), float(self._x[1]), float(self._x[2])
|
||||
|
||||
def velocity(self) -> Tuple[float, float, float]:
|
||||
return float(self._x[3]), float(self._x[4]), float(self._x[5])
|
||||
|
||||
def reset(self) -> None:
|
||||
self._x = np.zeros(6)
|
||||
self._P = np.diag([1.0, 1.0, 1.0, 0.5, 0.5, 0.5])
|
||||
self._init = False
|
||||
@ -1,487 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
uwb_position_node.py — ROS2 UWB position node for Jetson (Issue #546).
|
||||
|
||||
Reads JSON range measurements from an ESP32 UWB tag (DW3000) over USB
|
||||
serial, trilaterates position from 3+ fixed infrastructure anchors, and
|
||||
publishes the robot's absolute position in the map frame.
|
||||
|
||||
Serial protocol (one JSON object per line, UTF-8):
|
||||
────────────────────────────────────────────────────
|
||||
Tag → Jetson (full frame, all anchors at once):
|
||||
{"ts": 123456, "ranges": [
|
||||
{"id": 0, "d_mm": 1500, "rssi": -65.0},
|
||||
{"id": 1, "d_mm": 2100, "rssi": -68.0},
|
||||
{"id": 2, "d_mm": 1800, "rssi": -70.0}
|
||||
]}
|
||||
|
||||
Alternate (per-anchor, one line per measurement):
|
||||
{"id": 0, "d_mm": 1500, "rssi": -65.0}
|
||||
|
||||
Both "d_mm" and "range_mm" are accepted.
|
||||
|
||||
Anchor positions:
|
||||
─────────────────
|
||||
Fixed anchors are configured in uwb_position_params.yaml as flat arrays:
|
||||
anchor_ids: [0, 1, 2, 3]
|
||||
anchor_positions: [x0, y0, z0, x1, y1, z1, ...] # metres in map frame
|
||||
|
||||
Publishes:
|
||||
──────────
|
||||
/saltybot/uwb/pose (geometry_msgs/PoseStamped) 10 Hz
|
||||
/saltybot/uwb/range/<id> (saltybot_uwb_msgs/UwbRange) per anchor, on arrival
|
||||
/saltybot/uwb/status (std_msgs/String) 10 Hz JSON diagnostics
|
||||
|
||||
TF2:
|
||||
────
|
||||
Broadcasts uwb_link → map from /saltybot/uwb/pose position.
|
||||
|
||||
Outlier rejection:
|
||||
──────────────────
|
||||
After initial trilateration, anchors with range residual > outlier_threshold_m
|
||||
are dropped. If ≥ 3 inliers remain (2D mode) the position is re-solved.
|
||||
Anchors rejected in N_strikes_max consecutive frames are flagged in status.
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
import threading
|
||||
import time
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
|
||||
from geometry_msgs.msg import (
|
||||
PoseStamped, TransformStamped,
|
||||
Quaternion,
|
||||
)
|
||||
from std_msgs.msg import Header, String
|
||||
from tf2_ros import TransformBroadcaster
|
||||
|
||||
from saltybot_uwb_msgs.msg import UwbRange
|
||||
|
||||
from saltybot_uwb_position.trilateration import (
|
||||
trilaterate,
|
||||
reject_outliers,
|
||||
residuals,
|
||||
KalmanFilter3D,
|
||||
)
|
||||
|
||||
try:
|
||||
import serial
|
||||
_SERIAL_OK = True
|
||||
except ImportError:
|
||||
_SERIAL_OK = False
|
||||
|
||||
|
||||
# ── Serial reader thread ──────────────────────────────────────────────────────
|
||||
|
||||
class SerialJsonReader(threading.Thread):
|
||||
"""Background thread: reads newline-delimited JSON from a serial port."""
|
||||
|
||||
def __init__(self, port: str, baudrate: int, callback, logger) -> None:
|
||||
super().__init__(daemon=True)
|
||||
self._port = port
|
||||
self._baudrate = baudrate
|
||||
self._callback = callback
|
||||
self._logger = logger
|
||||
self._running = False
|
||||
self._ser = None
|
||||
|
||||
def run(self) -> None:
|
||||
self._running = True
|
||||
while self._running:
|
||||
try:
|
||||
self._ser = serial.Serial(
|
||||
self._port, self._baudrate, timeout=1.0
|
||||
)
|
||||
self._logger.info(f"UWB serial: opened {self._port} @ {self._baudrate}")
|
||||
self._read_loop()
|
||||
except Exception as exc:
|
||||
self._logger.warning(
|
||||
f"UWB serial error ({self._port}): {exc} — retry in 2 s"
|
||||
)
|
||||
if self._ser and self._ser.is_open:
|
||||
self._ser.close()
|
||||
time.sleep(2.0)
|
||||
|
||||
def _read_loop(self) -> None:
|
||||
while self._running:
|
||||
try:
|
||||
raw = self._ser.readline()
|
||||
if not raw:
|
||||
continue
|
||||
line = raw.decode("utf-8", errors="replace").strip()
|
||||
if not line:
|
||||
continue
|
||||
try:
|
||||
obj = json.loads(line)
|
||||
self._callback(obj)
|
||||
except json.JSONDecodeError:
|
||||
pass # ignore malformed lines silently
|
||||
except Exception as exc:
|
||||
self._logger.warning(f"UWB read error: {exc}")
|
||||
break
|
||||
|
||||
def stop(self) -> None:
|
||||
self._running = False
|
||||
if self._ser and self._ser.is_open:
|
||||
self._ser.close()
|
||||
|
||||
|
||||
# ── Node ──────────────────────────────────────────────────────────────────────
|
||||
|
||||
class UwbPositionNode(Node):
|
||||
"""ROS2 UWB position node — trilateration from 3+ fixed anchors."""
|
||||
|
||||
_MIN_ANCHORS_2D = 3
|
||||
_MIN_ANCHORS_3D = 4
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("uwb_position")
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────────
|
||||
self.declare_parameter("serial_port", "/dev/ttyUSB0")
|
||||
self.declare_parameter("baudrate", 115200)
|
||||
|
||||
# Anchor configuration: flat arrays, 3 floats per anchor
|
||||
self.declare_parameter("anchor_ids", [0, 1, 2])
|
||||
self.declare_parameter("anchor_positions", [
|
||||
0.0, 0.0, 2.0, # anchor 0: x, y, z (metres in map frame)
|
||||
5.0, 0.0, 2.0, # anchor 1
|
||||
5.0, 5.0, 2.0, # anchor 2
|
||||
])
|
||||
|
||||
self.declare_parameter("solve_z", False) # 2D mode by default
|
||||
self.declare_parameter("robot_z", 0.0) # m — robot floor height
|
||||
self.declare_parameter("publish_rate", 10.0) # Hz — pose + status
|
||||
self.declare_parameter("range_timeout_s", 1.5) # s — stale range threshold
|
||||
self.declare_parameter("max_range_m", 30.0) # m — validity max
|
||||
self.declare_parameter("min_range_m", 0.05) # m — validity min
|
||||
self.declare_parameter("outlier_threshold_m", 0.4) # m — RANSAC threshold
|
||||
self.declare_parameter("outlier_strikes_max", 5) # frames before flagging
|
||||
self.declare_parameter("kf_process_noise", 0.05) # Kalman Q
|
||||
self.declare_parameter("kf_meas_noise", 0.10) # Kalman R
|
||||
self.declare_parameter("map_frame", "map")
|
||||
self.declare_parameter("uwb_frame", "uwb_link")
|
||||
|
||||
# Load params
|
||||
self._port = self.get_parameter("serial_port").value
|
||||
self._baudrate = self.get_parameter("baudrate").value
|
||||
self._solve_z = self.get_parameter("solve_z").value
|
||||
self._robot_z = self.get_parameter("robot_z").value
|
||||
self._publish_rate = self.get_parameter("publish_rate").value
|
||||
self._timeout = self.get_parameter("range_timeout_s").value
|
||||
self._max_r = self.get_parameter("max_range_m").value
|
||||
self._min_r = self.get_parameter("min_range_m").value
|
||||
self._outlier_thr = self.get_parameter("outlier_threshold_m").value
|
||||
self._strikes_max = self.get_parameter("outlier_strikes_max").value
|
||||
self._map_frame = self.get_parameter("map_frame").value
|
||||
self._uwb_frame = self.get_parameter("uwb_frame").value
|
||||
|
||||
# Parse anchor config
|
||||
anchor_ids_raw = self.get_parameter("anchor_ids").value
|
||||
anchor_pos_flat = self.get_parameter("anchor_positions").value
|
||||
self._anchor_ids, self._anchor_positions = self._parse_anchors(
|
||||
anchor_ids_raw, anchor_pos_flat
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"UWB anchors: {[f'#{i}@({p[0]:.1f},{p[1]:.1f},{p[2]:.1f})' for i,p in zip(self._anchor_ids, self._anchor_positions)]}"
|
||||
)
|
||||
|
||||
# ── State ─────────────────────────────────────────────────────────────
|
||||
self._lock = threading.Lock()
|
||||
# anchor_id → (range_m, rssi, timestamp_monotonic)
|
||||
self._ranges: Dict[int, Tuple[float, float, float]] = {}
|
||||
self._strikes: Dict[int, int] = {aid: 0 for aid in self._anchor_ids}
|
||||
self._last_fix: Optional[Tuple[float, float, float]] = None
|
||||
self._has_fix = False
|
||||
self._fix_age = 0.0
|
||||
|
||||
# Kalman filter
|
||||
self._kf = KalmanFilter3D(
|
||||
process_noise=self.get_parameter("kf_process_noise").value,
|
||||
measurement_noise=self.get_parameter("kf_meas_noise").value,
|
||||
dt=1.0 / self._publish_rate,
|
||||
)
|
||||
|
||||
# Per-anchor outlier publishers (pre-create for configured anchors)
|
||||
sensor_qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
)
|
||||
self._range_pubs: Dict[int, object] = {}
|
||||
for aid in self._anchor_ids:
|
||||
self._range_pubs[aid] = self.create_publisher(
|
||||
UwbRange, f"/saltybot/uwb/range/{aid}", sensor_qos
|
||||
)
|
||||
|
||||
# Main publishers
|
||||
self._pose_pub = self.create_publisher(PoseStamped, "/saltybot/uwb/pose", 10)
|
||||
self._status_pub = self.create_publisher(String, "/saltybot/uwb/status", 10)
|
||||
|
||||
# TF2
|
||||
self._tf_broadcaster = TransformBroadcaster(self)
|
||||
|
||||
# ── Serial reader ──────────────────────────────────────────────────────
|
||||
if _SERIAL_OK:
|
||||
self._reader = SerialJsonReader(
|
||||
self._port, self._baudrate,
|
||||
callback=self._on_serial_json,
|
||||
logger=self.get_logger(),
|
||||
)
|
||||
self._reader.start()
|
||||
else:
|
||||
self.get_logger().warning(
|
||||
"pyserial not installed — serial I/O disabled (simulation mode)"
|
||||
)
|
||||
|
||||
# ── Publish timer ──────────────────────────────────────────────────────
|
||||
self._prev_publish_t = self.get_clock().now().nanoseconds * 1e-9
|
||||
self.create_timer(1.0 / self._publish_rate, self._publish_cb)
|
||||
|
||||
self.get_logger().info(
|
||||
f"UwbPositionNode ready — port={self._port} "
|
||||
f"anchors={len(self._anchor_ids)} "
|
||||
f"mode={'3D' if self._solve_z else '2D'} "
|
||||
f"rate={self._publish_rate:.0f}Hz"
|
||||
)
|
||||
|
||||
# ── Anchor config parsing ─────────────────────────────────────────────────
|
||||
|
||||
def _parse_anchors(
|
||||
self,
|
||||
ids_raw,
|
||||
pos_flat,
|
||||
):
|
||||
ids = list(ids_raw)
|
||||
n = len(ids)
|
||||
if len(pos_flat) < n * 3:
|
||||
raise ValueError(
|
||||
f"anchor_positions must have 3×anchor_ids entries. "
|
||||
f"Got {len(pos_flat)} values for {n} anchors."
|
||||
)
|
||||
positions = []
|
||||
for i in range(n):
|
||||
base = i * 3
|
||||
positions.append((
|
||||
float(pos_flat[base]),
|
||||
float(pos_flat[base + 1]),
|
||||
float(pos_flat[base + 2]),
|
||||
))
|
||||
return ids, positions
|
||||
|
||||
# ── Serial JSON callback (called from reader thread) ──────────────────────
|
||||
|
||||
def _on_serial_json(self, obj: dict) -> None:
|
||||
"""Parse incoming JSON and update range table."""
|
||||
now = time.monotonic()
|
||||
|
||||
if "ranges" in obj:
|
||||
# Full-frame format: {"ts": ..., "ranges": [{id, d_mm, rssi}, ...]}
|
||||
for entry in obj["ranges"]:
|
||||
self._ingest_range_entry(entry, now)
|
||||
else:
|
||||
# Per-anchor format: {"id": 0, "d_mm": 1500, "rssi": -65.0}
|
||||
self._ingest_range_entry(obj, now)
|
||||
|
||||
def _ingest_range_entry(self, entry: dict, timestamp: float) -> None:
|
||||
"""Store a single anchor range measurement after validity checks."""
|
||||
try:
|
||||
anchor_id = int(entry.get("id", entry.get("anchor_id", -1)))
|
||||
# Accept both "d_mm" and "range_mm"
|
||||
raw_mm = int(entry.get("d_mm", entry.get("range_mm", 0)))
|
||||
rssi = float(entry.get("rssi", 0.0))
|
||||
except (KeyError, TypeError, ValueError):
|
||||
return
|
||||
|
||||
if anchor_id not in self._anchor_ids:
|
||||
return # unknown / unconfigured anchor
|
||||
|
||||
range_m = raw_mm / 1000.0
|
||||
if range_m < self._min_r or range_m > self._max_r:
|
||||
return # outside validity window
|
||||
|
||||
with self._lock:
|
||||
self._ranges[anchor_id] = (range_m, rssi, timestamp)
|
||||
|
||||
# Publish per-anchor range topic immediately
|
||||
self._publish_anchor_range(anchor_id, range_m, raw_mm, rssi)
|
||||
|
||||
# ── Per-anchor range publisher ────────────────────────────────────────────
|
||||
|
||||
def _publish_anchor_range(
|
||||
self, anchor_id: int, range_m: float, raw_mm: int, rssi: float
|
||||
) -> None:
|
||||
if anchor_id not in self._range_pubs:
|
||||
return
|
||||
msg = UwbRange()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = self._map_frame
|
||||
msg.anchor_id = anchor_id
|
||||
msg.range_m = float(range_m)
|
||||
msg.raw_mm = int(raw_mm)
|
||||
msg.rssi = float(rssi)
|
||||
msg.tag_id = ""
|
||||
self._range_pubs[anchor_id].publish(msg)
|
||||
|
||||
# ── Main publish callback (10 Hz) ─────────────────────────────────────────
|
||||
|
||||
def _publish_cb(self) -> None:
|
||||
now_ros = self.get_clock().now()
|
||||
now_mono = time.monotonic()
|
||||
|
||||
# Collect fresh ranges
|
||||
with self._lock:
|
||||
fresh = {
|
||||
aid: entry
|
||||
for aid, entry in self._ranges.items()
|
||||
if (now_mono - entry[2]) <= self._timeout
|
||||
}
|
||||
|
||||
# Check minimum anchor count for chosen mode
|
||||
min_anch = self._MIN_ANCHORS_3D if self._solve_z else self._MIN_ANCHORS_2D
|
||||
state = "no_fix"
|
||||
pos = None
|
||||
used_ids: List[int] = []
|
||||
res_map: Dict[int, float] = {}
|
||||
|
||||
if len(fresh) >= min_anch:
|
||||
active_ids = list(fresh.keys())
|
||||
anch_pos = [self._anchor_positions[self._anchor_ids.index(i)]
|
||||
for i in active_ids]
|
||||
anch_r = [fresh[i][0] for i in active_ids]
|
||||
|
||||
try:
|
||||
fixed_z = None if self._solve_z else self._robot_z
|
||||
raw_pos = trilaterate(anch_pos, anch_r, fixed_z=fixed_z)
|
||||
|
||||
# Outlier rejection
|
||||
inlier_idx = reject_outliers(
|
||||
anch_pos, anch_r, raw_pos,
|
||||
threshold_m=self._outlier_thr,
|
||||
)
|
||||
res_all = residuals(anch_pos, anch_r, raw_pos)
|
||||
for k, aid in enumerate(active_ids):
|
||||
res_map[aid] = round(res_all[k], 3)
|
||||
|
||||
# Update consecutive strike counters
|
||||
for k, aid in enumerate(active_ids):
|
||||
if k in inlier_idx:
|
||||
self._strikes[aid] = 0
|
||||
else:
|
||||
self._strikes[aid] = self._strikes.get(aid, 0) + 1
|
||||
|
||||
# Re-solve with inliers if any were rejected
|
||||
if len(inlier_idx) < len(active_ids) and len(inlier_idx) >= min_anch:
|
||||
inlier_anch = [anch_pos[k] for k in inlier_idx]
|
||||
inlier_r = [anch_r[k] for k in inlier_idx]
|
||||
raw_pos = trilaterate(inlier_anch, inlier_r, fixed_z=fixed_z)
|
||||
|
||||
used_ids = [active_ids[k] for k in inlier_idx]
|
||||
pos = raw_pos
|
||||
|
||||
# Kalman predict + update
|
||||
dt = now_ros.nanoseconds * 1e-9 - self._prev_publish_t
|
||||
self._kf.predict(dt=dt)
|
||||
self._kf.update(pos[0], pos[1], pos[2])
|
||||
pos = self._kf.position()
|
||||
|
||||
self._last_fix = pos
|
||||
self._has_fix = True
|
||||
self._fix_age = 0.0
|
||||
state = "ok" if len(used_ids) >= min_anch else "degraded"
|
||||
|
||||
except (ValueError, RuntimeError) as exc:
|
||||
self.get_logger().warning(f"Trilateration failed: {exc}")
|
||||
state = "degraded"
|
||||
|
||||
elif self._has_fix:
|
||||
# KF predict-only: coast on last known position
|
||||
dt = now_ros.nanoseconds * 1e-9 - self._prev_publish_t
|
||||
self._kf.predict(dt=dt)
|
||||
pos = self._kf.position()
|
||||
self._fix_age += 1.0 / self._publish_rate
|
||||
state = "degraded" if self._fix_age < 5.0 else "no_fix"
|
||||
|
||||
self._prev_publish_t = now_ros.nanoseconds * 1e-9
|
||||
|
||||
if pos is None:
|
||||
self._publish_status(state, 0, [], {})
|
||||
return
|
||||
|
||||
x, y, z = pos
|
||||
|
||||
# ── Publish PoseStamped ────────────────────────────────────────────
|
||||
pose_msg = PoseStamped()
|
||||
pose_msg.header.stamp = now_ros.to_msg()
|
||||
pose_msg.header.frame_id = self._map_frame
|
||||
pose_msg.pose.position.x = x
|
||||
pose_msg.pose.position.y = y
|
||||
pose_msg.pose.position.z = z
|
||||
pose_msg.pose.orientation.w = 1.0 # no orientation from UWB alone
|
||||
self._pose_pub.publish(pose_msg)
|
||||
|
||||
# ── TF2: uwb_link → map ───────────────────────────────────────────
|
||||
tf_msg = TransformStamped()
|
||||
tf_msg.header.stamp = now_ros.to_msg()
|
||||
tf_msg.header.frame_id = self._map_frame
|
||||
tf_msg.child_frame_id = self._uwb_frame
|
||||
tf_msg.transform.translation.x = x
|
||||
tf_msg.transform.translation.y = y
|
||||
tf_msg.transform.translation.z = z
|
||||
tf_msg.transform.rotation.w = 1.0
|
||||
self._tf_broadcaster.sendTransform(tf_msg)
|
||||
|
||||
# ── Diagnostics ───────────────────────────────────────────────────
|
||||
self._publish_status(state, len(used_ids), used_ids, res_map)
|
||||
|
||||
# ── Status publisher ──────────────────────────────────────────────────────
|
||||
|
||||
def _publish_status(
|
||||
self,
|
||||
state: str,
|
||||
n_anchors: int,
|
||||
used_ids: List[int],
|
||||
res_map: Dict[int, float],
|
||||
) -> None:
|
||||
# Flag anchors with too many consecutive outlier strikes
|
||||
flagged = [
|
||||
aid for aid, s in self._strikes.items() if s >= self._strikes_max
|
||||
]
|
||||
pos = self._kf.position() if self._has_fix else None
|
||||
status = {
|
||||
"state": state,
|
||||
"active_anchors": n_anchors,
|
||||
"used_anchor_ids": used_ids,
|
||||
"flagged_anchors": flagged,
|
||||
"position": {
|
||||
"x": round(pos[0], 3),
|
||||
"y": round(pos[1], 3),
|
||||
"z": round(pos[2], 3),
|
||||
} if pos else None,
|
||||
"residuals_m": res_map,
|
||||
"fix_age_s": round(self._fix_age, 2),
|
||||
}
|
||||
self._status_pub.publish(String(data=json.dumps(status)))
|
||||
|
||||
|
||||
# ── Entry point ───────────────────────────────────────────────────────────────
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = UwbPositionNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.try_shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,5 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_uwb_position
|
||||
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_uwb_position
|
||||
@ -1,32 +0,0 @@
|
||||
import os
|
||||
from glob import glob
|
||||
from setuptools import setup
|
||||
|
||||
package_name = "saltybot_uwb_position"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages",
|
||||
["resource/" + package_name]),
|
||||
("share/" + package_name, ["package.xml"]),
|
||||
(os.path.join("share", package_name, "launch"),
|
||||
glob("launch/*.py")),
|
||||
(os.path.join("share", package_name, "config"),
|
||||
glob("config/*.yaml")),
|
||||
],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer="sl-perception",
|
||||
maintainer_email="sl-perception@saltylab.local",
|
||||
description="ROS2 UWB position node — JSON serial bridge with trilateration (Issue #546)",
|
||||
license="MIT",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"uwb_position = saltybot_uwb_position.uwb_position_node:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,448 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
voice_commander.py — Phone-based voice command interface for SaltyBot (Issue #553)
|
||||
|
||||
Runs on Android/Termux. Listens for the wake word 'Hey Salty', transcribes speech
|
||||
via OpenAI Whisper (local), parses robot commands, and publishes to ROS2 topic
|
||||
/saltybot/voice/cmd via WebSocket bridge to Jetson Orin. Confirms commands via
|
||||
termux-tts-speak.
|
||||
|
||||
Supported commands:
|
||||
go forward / go back / go left / go right
|
||||
stop / halt
|
||||
follow me
|
||||
go home
|
||||
look at me
|
||||
|
||||
Usage:
|
||||
python3 phone/voice_commander.py [OPTIONS]
|
||||
|
||||
Options:
|
||||
--host HOST Jetson IP or hostname (default: 192.168.1.100)
|
||||
--port PORT rosbridge WebSocket port (default: 9090)
|
||||
--model MODEL Whisper model size: tiny/base/small (default: base)
|
||||
--threshold FLOAT Wake word match threshold 0.0-1.0 (default: 0.6)
|
||||
--record-sec FLOAT Seconds to record after wake word (default: 3.0)
|
||||
--no-tts Disable TTS confirmation
|
||||
--debug Verbose logging
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import logging
|
||||
import os
|
||||
import subprocess
|
||||
import sys
|
||||
import tempfile
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
# ── Optional ROS2 ────────────────────────────────────────────────────────────
|
||||
try:
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
ROS2_AVAILABLE = True
|
||||
except ImportError:
|
||||
ROS2_AVAILABLE = False
|
||||
|
||||
# ── Whisper (local STT) ───────────────────────────────────────────────────────
|
||||
try:
|
||||
import whisper
|
||||
WHISPER_AVAILABLE = True
|
||||
except ImportError:
|
||||
WHISPER_AVAILABLE = False
|
||||
|
||||
# ── WebSocket client ──────────────────────────────────────────────────────────
|
||||
try:
|
||||
import websocket # websocket-client
|
||||
WS_AVAILABLE = True
|
||||
except ImportError:
|
||||
WS_AVAILABLE = False
|
||||
|
||||
# ── Constants ────────────────────────────────────────────────────────────────
|
||||
WAKE_WORD = "hey salty"
|
||||
VOICE_CMD_TOPIC = "/saltybot/voice/cmd"
|
||||
SAMPLE_RATE = 16000 # Hz required by Whisper
|
||||
WAKE_RECORD_SEC = 1.5 # short clip to check for wake word
|
||||
CMD_RECORD_SEC = 3.0 # command clip length after wake word
|
||||
RECONNECT_DELAY = 5.0 # seconds between WS reconnects
|
||||
CHUNK_BYTES = 4096
|
||||
|
||||
|
||||
class Command(Enum):
|
||||
GO_FORWARD = "go_forward"
|
||||
GO_BACK = "go_back"
|
||||
GO_LEFT = "go_left"
|
||||
GO_RIGHT = "go_right"
|
||||
STOP = "stop"
|
||||
FOLLOW_ME = "follow_me"
|
||||
GO_HOME = "go_home"
|
||||
LOOK_AT_ME = "look_at_me"
|
||||
UNKNOWN = "unknown"
|
||||
|
||||
|
||||
# ── Command parsing ───────────────────────────────────────────────────────────
|
||||
|
||||
# Each entry: (list_of_trigger_phrases, Command)
|
||||
COMMAND_TABLE = [
|
||||
(["go forward", "move forward", "forward", "ahead", "go straight"], Command.GO_FORWARD),
|
||||
(["go back", "go backward", "move back", "reverse", "back up"], Command.GO_BACK),
|
||||
(["go left", "turn left", "move left", "left"], Command.GO_LEFT),
|
||||
(["go right", "turn right", "move right", "right"], Command.GO_RIGHT),
|
||||
(["stop", "halt", "freeze", "stay", "stand by"], Command.STOP),
|
||||
(["follow me", "come here", "come with me", "follow"], Command.FOLLOW_ME),
|
||||
(["go home", "return home", "return to base", "dock"], Command.GO_HOME),
|
||||
(["look at me", "face me", "look here", "turn to me"], Command.LOOK_AT_ME),
|
||||
]
|
||||
|
||||
TTS_CONFIRMATIONS = {
|
||||
Command.GO_FORWARD: "Going forward",
|
||||
Command.GO_BACK: "Going back",
|
||||
Command.GO_LEFT: "Turning left",
|
||||
Command.GO_RIGHT: "Turning right",
|
||||
Command.STOP: "Stopping",
|
||||
Command.FOLLOW_ME: "Following you",
|
||||
Command.GO_HOME: "Heading home",
|
||||
Command.LOOK_AT_ME: "Looking at you",
|
||||
Command.UNKNOWN: "Sorry, I didn't understand that",
|
||||
}
|
||||
|
||||
|
||||
def parse_command(text: str) -> Command:
|
||||
"""Match transcribed text against command table. Returns best match or UNKNOWN."""
|
||||
text = text.lower().strip()
|
||||
for phrases, cmd in COMMAND_TABLE:
|
||||
for phrase in phrases:
|
||||
if phrase in text:
|
||||
return cmd
|
||||
return Command.UNKNOWN
|
||||
|
||||
|
||||
def contains_wake_word(text: str, threshold: float = 0.6) -> bool:
|
||||
"""Check if transcribed text contains the wake word (fuzzy match)."""
|
||||
text = text.lower().strip()
|
||||
if WAKE_WORD in text:
|
||||
return True
|
||||
# Simple token overlap fallback
|
||||
wake_tokens = set(WAKE_WORD.split())
|
||||
text_tokens = set(text.split())
|
||||
overlap = len(wake_tokens & text_tokens) / len(wake_tokens)
|
||||
return overlap >= threshold
|
||||
|
||||
|
||||
# ── Audio capture via termux-microphone-record ────────────────────────────────
|
||||
|
||||
def record_audio(duration_sec: float, output_path: str) -> bool:
|
||||
"""
|
||||
Record audio using termux-microphone-record.
|
||||
Saves a 16 kHz mono WAV to output_path.
|
||||
Returns True on success.
|
||||
"""
|
||||
# termux-microphone-record writes to a file; we start, wait, then stop.
|
||||
try:
|
||||
subprocess.run(
|
||||
[
|
||||
"termux-microphone-record",
|
||||
"-l", str(int(duration_sec)), # duration in seconds
|
||||
"-r", str(SAMPLE_RATE), # sample rate
|
||||
"-c", "1", # mono
|
||||
"-e", "aac", # encoding (aac is reliable on Android)
|
||||
"-f", output_path,
|
||||
],
|
||||
check=True,
|
||||
timeout=duration_sec + 5,
|
||||
capture_output=True,
|
||||
)
|
||||
return Path(output_path).exists()
|
||||
except (subprocess.CalledProcessError, subprocess.TimeoutExpired, FileNotFoundError) as e:
|
||||
logging.debug("termux-microphone-record error: %s", e)
|
||||
return False
|
||||
|
||||
|
||||
def tts_speak(text: str) -> None:
|
||||
"""Speak text via termux-tts-speak (non-blocking)."""
|
||||
try:
|
||||
subprocess.Popen(
|
||||
["termux-tts-speak", text],
|
||||
stdout=subprocess.DEVNULL,
|
||||
stderr=subprocess.DEVNULL,
|
||||
)
|
||||
except FileNotFoundError:
|
||||
logging.debug("termux-tts-speak not available")
|
||||
|
||||
|
||||
# ── Whisper STT ───────────────────────────────────────────────────────────────
|
||||
|
||||
class WhisperSTT:
|
||||
"""Thin wrapper around local Whisper model."""
|
||||
|
||||
def __init__(self, model_size: str = "base"):
|
||||
if not WHISPER_AVAILABLE:
|
||||
raise RuntimeError(
|
||||
"openai-whisper not installed. Run: pip install openai-whisper"
|
||||
)
|
||||
logging.info("Loading Whisper model '%s'...", model_size)
|
||||
self.model = whisper.load_model(model_size)
|
||||
logging.info("Whisper model loaded.")
|
||||
|
||||
def transcribe(self, audio_path: str) -> str:
|
||||
"""Transcribe audio file, return lowercase text."""
|
||||
try:
|
||||
result = self.model.transcribe(audio_path, language="en", fp16=False)
|
||||
text = result.get("text", "").strip()
|
||||
logging.debug("Whisper transcription: '%s'", text)
|
||||
return text
|
||||
except Exception as e:
|
||||
logging.warning("Whisper transcription failed: %s", e)
|
||||
return ""
|
||||
|
||||
|
||||
# ── Publisher backends ────────────────────────────────────────────────────────
|
||||
|
||||
class ROS2Publisher:
|
||||
"""Publish voice commands as std_msgs/String on /saltybot/voice/cmd."""
|
||||
|
||||
def __init__(self):
|
||||
rclpy.init()
|
||||
self._node = Node("voice_commander")
|
||||
self._pub = self._node.create_publisher(String, VOICE_CMD_TOPIC, 10)
|
||||
self._spin_thread = threading.Thread(
|
||||
target=lambda: rclpy.spin(self._node), daemon=True
|
||||
)
|
||||
self._spin_thread.start()
|
||||
|
||||
def publish(self, cmd: Command, raw_text: str) -> None:
|
||||
payload = json.dumps({"command": cmd.value, "raw": raw_text, "ts": time.time()})
|
||||
msg = String()
|
||||
msg.data = payload
|
||||
self._pub.publish(msg)
|
||||
logging.info("ROS2 published: %s", payload)
|
||||
|
||||
def shutdown(self) -> None:
|
||||
self._node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
class WebSocketPublisher:
|
||||
"""Publish voice commands over rosbridge WebSocket protocol."""
|
||||
|
||||
def __init__(self, host: str, port: int):
|
||||
self.uri = f"ws://{host}:{port}"
|
||||
self._ws: Optional[websocket.WebSocket] = None
|
||||
self._lock = threading.Lock()
|
||||
self._connect()
|
||||
|
||||
def _connect(self) -> None:
|
||||
try:
|
||||
ws = websocket.WebSocket()
|
||||
ws.connect(self.uri, timeout=5)
|
||||
with self._lock:
|
||||
self._ws = ws
|
||||
logging.info("WebSocket connected to %s", self.uri)
|
||||
except Exception as e:
|
||||
logging.warning("WebSocket connect failed (%s): %s", self.uri, e)
|
||||
self._ws = None
|
||||
|
||||
def _ensure_connected(self) -> bool:
|
||||
if self._ws is not None:
|
||||
return True
|
||||
self._connect()
|
||||
return self._ws is not None
|
||||
|
||||
def publish(self, cmd: Command, raw_text: str) -> None:
|
||||
payload = json.dumps({"command": cmd.value, "raw": raw_text, "ts": time.time()})
|
||||
# rosbridge advertise + publish message
|
||||
advertise_msg = json.dumps({
|
||||
"op": "advertise",
|
||||
"topic": VOICE_CMD_TOPIC,
|
||||
"type": "std_msgs/String",
|
||||
})
|
||||
publish_msg = json.dumps({
|
||||
"op": "publish",
|
||||
"topic": VOICE_CMD_TOPIC,
|
||||
"msg": {"data": payload},
|
||||
})
|
||||
with self._lock:
|
||||
if not self._ensure_connected():
|
||||
logging.error("Cannot publish — WebSocket not connected.")
|
||||
return
|
||||
try:
|
||||
self._ws.send(advertise_msg)
|
||||
self._ws.send(publish_msg)
|
||||
logging.info("WS published: %s", payload)
|
||||
except Exception as e:
|
||||
logging.warning("WebSocket send failed: %s", e)
|
||||
self._ws = None
|
||||
|
||||
def shutdown(self) -> None:
|
||||
with self._lock:
|
||||
if self._ws:
|
||||
try:
|
||||
self._ws.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._ws = None
|
||||
|
||||
|
||||
# ── Main listener loop ────────────────────────────────────────────────────────
|
||||
|
||||
@dataclass
|
||||
class VoiceCommanderConfig:
|
||||
host: str = "192.168.1.100"
|
||||
port: int = 9090
|
||||
model: str = "base"
|
||||
wake_threshold: float = 0.6
|
||||
record_sec: float = CMD_RECORD_SEC
|
||||
no_tts: bool = False
|
||||
debug: bool = False
|
||||
|
||||
|
||||
class VoiceCommander:
|
||||
"""
|
||||
Main voice command loop.
|
||||
|
||||
State machine:
|
||||
IDLE → record short clip → check for wake word
|
||||
WAKE_DETECTED → record command clip → transcribe → parse → publish → confirm
|
||||
"""
|
||||
|
||||
def __init__(self, config: VoiceCommanderConfig):
|
||||
self.cfg = config
|
||||
self._running = False
|
||||
|
||||
# STT
|
||||
self._stt = WhisperSTT(model_size=config.model)
|
||||
|
||||
# Publisher
|
||||
if ROS2_AVAILABLE:
|
||||
logging.info("Using ROS2 publisher backend.")
|
||||
self._pub: ROS2Publisher | WebSocketPublisher = ROS2Publisher()
|
||||
elif WS_AVAILABLE:
|
||||
logging.info("Using WebSocket publisher backend (%s:%d).", config.host, config.port)
|
||||
self._pub = WebSocketPublisher(config.host, config.port)
|
||||
else:
|
||||
raise RuntimeError(
|
||||
"No publisher backend available. "
|
||||
"Install rclpy (ROS2) or websocket-client: pip install websocket-client"
|
||||
)
|
||||
|
||||
# ── lifecycle ──────────────────────────────────────────────────────────────
|
||||
|
||||
def start(self) -> None:
|
||||
self._running = True
|
||||
logging.info("Voice commander started. Listening for '%s'...", WAKE_WORD)
|
||||
if not self.cfg.no_tts:
|
||||
tts_speak("Hey Salty is ready")
|
||||
try:
|
||||
self._listen_loop()
|
||||
except KeyboardInterrupt:
|
||||
logging.info("Interrupted.")
|
||||
finally:
|
||||
self.stop()
|
||||
|
||||
def stop(self) -> None:
|
||||
self._running = False
|
||||
self._pub.shutdown()
|
||||
logging.info("Voice commander stopped.")
|
||||
|
||||
# ── main loop ─────────────────────────────────────────────────────────────
|
||||
|
||||
def _listen_loop(self) -> None:
|
||||
"""Continuously poll for wake word then capture command."""
|
||||
with tempfile.TemporaryDirectory() as tmpdir:
|
||||
wake_audio = os.path.join(tmpdir, "wake.aac")
|
||||
cmd_audio = os.path.join(tmpdir, "cmd.aac")
|
||||
|
||||
while self._running:
|
||||
# 1. Record short clip for wake word detection
|
||||
logging.debug("Recording %.1fs for wake word...", WAKE_RECORD_SEC)
|
||||
if not record_audio(WAKE_RECORD_SEC, wake_audio):
|
||||
logging.debug("Wake clip recording failed, retrying.")
|
||||
time.sleep(0.5)
|
||||
continue
|
||||
|
||||
wake_text = self._stt.transcribe(wake_audio)
|
||||
if not wake_text:
|
||||
continue
|
||||
|
||||
if not contains_wake_word(wake_text, self.cfg.wake_threshold):
|
||||
logging.debug("No wake word in: '%s'", wake_text)
|
||||
continue
|
||||
|
||||
# 2. Wake word detected — acknowledge and record command
|
||||
logging.info("Wake word detected! Recording command...")
|
||||
if not self.cfg.no_tts:
|
||||
tts_speak("Yes?")
|
||||
|
||||
if not record_audio(self.cfg.record_sec, cmd_audio):
|
||||
logging.warning("Command clip recording failed.")
|
||||
continue
|
||||
|
||||
cmd_text = self._stt.transcribe(cmd_audio)
|
||||
if not cmd_text:
|
||||
logging.info("No speech detected after wake word.")
|
||||
continue
|
||||
|
||||
# 3. Parse and dispatch
|
||||
cmd = parse_command(cmd_text)
|
||||
logging.info("Parsed command: %s (from '%s')", cmd.value, cmd_text)
|
||||
|
||||
self._pub.publish(cmd, cmd_text)
|
||||
|
||||
# 4. TTS confirmation
|
||||
if not self.cfg.no_tts:
|
||||
tts_speak(TTS_CONFIRMATIONS[cmd])
|
||||
|
||||
|
||||
# ── Entry point ───────────────────────────────────────────────────────────────
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="SaltyBot phone voice command interface (Issue #553)"
|
||||
)
|
||||
parser.add_argument("--host", default="192.168.1.100",
|
||||
help="Jetson IP/hostname (default: 192.168.1.100)")
|
||||
parser.add_argument("--port", type=int, default=9090,
|
||||
help="rosbridge WebSocket port (default: 9090)")
|
||||
parser.add_argument("--model", default="base",
|
||||
choices=["tiny", "base", "small"],
|
||||
help="Whisper model size (default: base)")
|
||||
parser.add_argument("--threshold", type=float, default=0.6,
|
||||
help="Wake word match threshold 0.0-1.0 (default: 0.6)")
|
||||
parser.add_argument("--record-sec", type=float, default=CMD_RECORD_SEC,
|
||||
help=f"Seconds to record command (default: {CMD_RECORD_SEC})")
|
||||
parser.add_argument("--no-tts", action="store_true",
|
||||
help="Disable TTS confirmation")
|
||||
parser.add_argument("--debug", action="store_true",
|
||||
help="Verbose logging")
|
||||
args = parser.parse_args()
|
||||
|
||||
logging.basicConfig(
|
||||
level=logging.DEBUG if args.debug else logging.INFO,
|
||||
format="%(asctime)s [%(levelname)s] %(message)s",
|
||||
)
|
||||
|
||||
if not WHISPER_AVAILABLE:
|
||||
logging.error("openai-whisper not installed. Run: pip install openai-whisper")
|
||||
sys.exit(1)
|
||||
|
||||
cfg = VoiceCommanderConfig(
|
||||
host=args.host,
|
||||
port=args.port,
|
||||
model=args.model,
|
||||
wake_threshold=args.threshold,
|
||||
record_sec=args.record_sec,
|
||||
no_tts=args.no_tts,
|
||||
debug=args.debug,
|
||||
)
|
||||
|
||||
VoiceCommander(cfg).start()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
127
src/gimbal.c
127
src/gimbal.c
@ -1,127 +0,0 @@
|
||||
#include "gimbal.h"
|
||||
#include "servo_bus.h"
|
||||
#include "config.h"
|
||||
#include <stddef.h>
|
||||
|
||||
/*
|
||||
* gimbal.c — Pan/tilt gimbal controller for ST3215 bus servos (Issue #547)
|
||||
*
|
||||
* Tick rate: called every 1 ms from main loop; self-throttles to GIMBAL_TLM_HZ.
|
||||
* Feedback polling alternates: pan on even ticks, tilt on odd ticks.
|
||||
* This gives ~25 Hz per axis, keeping per-read latency under 2 ms total.
|
||||
*
|
||||
* Safety limits:
|
||||
* Pan: -GIMBAL_PAN_LIMIT_DEG .. +GIMBAL_PAN_LIMIT_DEG (±180 deg)
|
||||
* Tilt: -GIMBAL_TILT_LIMIT_DEG .. +GIMBAL_TILT_LIMIT_DEG (± 90 deg)
|
||||
*/
|
||||
|
||||
#define TICK_PERIOD_MS (1000u / GIMBAL_TLM_HZ) /* 20 ms at 50 Hz */
|
||||
|
||||
/* Clamp int16 to [lo, hi] */
|
||||
static int16_t _clamp16(int16_t v, int16_t lo, int16_t hi)
|
||||
{
|
||||
if (v < lo) return lo;
|
||||
if (v > hi) return hi;
|
||||
return v;
|
||||
}
|
||||
|
||||
/* ---- gimbal_init() ---- */
|
||||
|
||||
void gimbal_init(gimbal_t *g)
|
||||
{
|
||||
g->cmd_pan_x10 = 0;
|
||||
g->cmd_tilt_x10 = 0;
|
||||
g->cmd_speed = 0; /* 0 = max speed */
|
||||
g->fb_pan_x10 = 0;
|
||||
g->fb_tilt_x10 = 0;
|
||||
g->fb_pan_speed = 0;
|
||||
g->fb_tilt_speed = 0;
|
||||
g->rx_ok = 0;
|
||||
g->rx_err = 0;
|
||||
g->_last_tick_ms = 0;
|
||||
g->_poll_phase = 0;
|
||||
|
||||
/* Enable torque and center both servos */
|
||||
servo_bus_write_torque(GIMBAL_PAN_ID, true);
|
||||
servo_bus_write_torque(GIMBAL_TILT_ID, true);
|
||||
g->torque_enabled = true;
|
||||
|
||||
uint16_t center = servo_bus_deg_to_raw(0.0f);
|
||||
servo_bus_write_pos(GIMBAL_PAN_ID, center, 0);
|
||||
servo_bus_write_pos(GIMBAL_TILT_ID, center, 0);
|
||||
}
|
||||
|
||||
/* ---- gimbal_set_pos() ---- */
|
||||
|
||||
void gimbal_set_pos(gimbal_t *g, int16_t pan_x10, int16_t tilt_x10,
|
||||
uint16_t speed)
|
||||
{
|
||||
/* Clamp to hardware limits */
|
||||
pan_x10 = _clamp16(pan_x10,
|
||||
-(int16_t)(GIMBAL_PAN_LIMIT_DEG * 10),
|
||||
(int16_t)(GIMBAL_PAN_LIMIT_DEG * 10));
|
||||
tilt_x10 = _clamp16(tilt_x10,
|
||||
-(int16_t)(GIMBAL_TILT_LIMIT_DEG * 10),
|
||||
(int16_t)(GIMBAL_TILT_LIMIT_DEG * 10));
|
||||
if (speed > SB_SPEED_MAX) speed = SB_SPEED_MAX;
|
||||
|
||||
g->cmd_pan_x10 = pan_x10;
|
||||
g->cmd_tilt_x10 = tilt_x10;
|
||||
g->cmd_speed = speed;
|
||||
|
||||
float pan_deg = (float)pan_x10 / 10.0f;
|
||||
float tilt_deg = (float)tilt_x10 / 10.0f;
|
||||
|
||||
servo_bus_write_pos(GIMBAL_PAN_ID,
|
||||
servo_bus_deg_to_raw(pan_deg), speed);
|
||||
servo_bus_write_pos(GIMBAL_TILT_ID,
|
||||
servo_bus_deg_to_raw(tilt_deg), speed);
|
||||
}
|
||||
|
||||
/* ---- gimbal_torque() ---- */
|
||||
|
||||
void gimbal_torque(gimbal_t *g, bool enable)
|
||||
{
|
||||
servo_bus_write_torque(GIMBAL_PAN_ID, enable);
|
||||
servo_bus_write_torque(GIMBAL_TILT_ID, enable);
|
||||
g->torque_enabled = enable;
|
||||
}
|
||||
|
||||
/* ---- gimbal_tick() ---- */
|
||||
|
||||
void gimbal_tick(gimbal_t *g, uint32_t now_ms)
|
||||
{
|
||||
if ((now_ms - g->_last_tick_ms) < TICK_PERIOD_MS) return;
|
||||
g->_last_tick_ms = now_ms;
|
||||
|
||||
uint16_t raw = 0;
|
||||
|
||||
if (g->_poll_phase == 0u) {
|
||||
/* Poll pan position */
|
||||
if (servo_bus_read_pos(GIMBAL_PAN_ID, &raw)) {
|
||||
g->fb_pan_x10 = (int16_t)(servo_bus_raw_to_deg(raw) * 10.0f);
|
||||
g->rx_ok++;
|
||||
} else {
|
||||
g->rx_err++;
|
||||
}
|
||||
|
||||
/* Also refresh pan speed */
|
||||
uint16_t spd = 0;
|
||||
(void)servo_bus_read_speed(GIMBAL_PAN_ID, &spd);
|
||||
g->fb_pan_speed = spd;
|
||||
} else {
|
||||
/* Poll tilt position */
|
||||
if (servo_bus_read_pos(GIMBAL_TILT_ID, &raw)) {
|
||||
g->fb_tilt_x10 = (int16_t)(servo_bus_raw_to_deg(raw) * 10.0f);
|
||||
g->rx_ok++;
|
||||
} else {
|
||||
g->rx_err++;
|
||||
}
|
||||
|
||||
uint16_t spd = 0;
|
||||
(void)servo_bus_read_speed(GIMBAL_TILT_ID, &spd);
|
||||
g->fb_tilt_speed = spd;
|
||||
}
|
||||
|
||||
g->_poll_phase ^= 1u; /* toggle 0 / 1 */
|
||||
}
|
||||
146
src/jlink.c
146
src/jlink.c
@ -38,14 +38,6 @@ static void jlink_tx_locked(uint8_t *buf, uint16_t len)
|
||||
/* ---- Volatile state ---- */
|
||||
volatile JLinkState jlink_state;
|
||||
|
||||
/* ---- SCHED_SET static receive buffer (Issue #550) ---- */
|
||||
static JLinkSchedSetBuf s_sched_set_buf;
|
||||
|
||||
JLinkSchedSetBuf *jlink_get_sched_set(void)
|
||||
{
|
||||
return &s_sched_set_buf;
|
||||
}
|
||||
|
||||
/* ---- CRC16-XModem (poly 0x1021, init 0x0000) ---- */
|
||||
static uint16_t crc16_xmodem(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
@ -75,7 +67,7 @@ void jlink_init(void)
|
||||
gpio.Alternate = GPIO_AF7_USART1;
|
||||
HAL_GPIO_Init(GPIOB, &gpio);
|
||||
|
||||
/* DMA2 Stream2 Channel4 -- USART1_RX circular */
|
||||
/* DMA2 Stream2 Channel4 — USART1_RX circular */
|
||||
__HAL_RCC_DMA2_CLK_ENABLE();
|
||||
s_dma_rx.Instance = DMA2_Stream2;
|
||||
s_dma_rx.Init.Channel = DMA_CHANNEL_4;
|
||||
@ -111,11 +103,10 @@ void jlink_init(void)
|
||||
HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 7, 0);
|
||||
HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn);
|
||||
|
||||
/* Start circular DMA RX -- never stops */
|
||||
/* Start circular DMA RX — never stops */
|
||||
HAL_UART_Receive_DMA(&s_uart, s_rx_buf, JLINK_RX_BUF_LEN);
|
||||
|
||||
memset((void *)&jlink_state, 0, sizeof(jlink_state));
|
||||
memset(&s_sched_set_buf, 0, sizeof(s_sched_set_buf));
|
||||
s_rx_tail = 0;
|
||||
}
|
||||
|
||||
@ -125,7 +116,7 @@ void USART1_IRQHandler(void)
|
||||
/* Clear IDLE flag by reading SR then DR */
|
||||
if (__HAL_UART_GET_FLAG(&s_uart, UART_FLAG_IDLE)) {
|
||||
__HAL_UART_CLEAR_IDLEFLAG(&s_uart);
|
||||
/* jlink_process() drains the buffer from main loop -- no work here */
|
||||
/* jlink_process() drains the buffer from main loop — no work here */
|
||||
}
|
||||
HAL_UART_IRQHandler(&s_uart);
|
||||
}
|
||||
@ -151,7 +142,7 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
|
||||
switch (cmd) {
|
||||
|
||||
case JLINK_CMD_HEARTBEAT:
|
||||
/* Heartbeat only -- no payload action needed */
|
||||
/* Heartbeat only — no payload action needed */
|
||||
break;
|
||||
|
||||
case JLINK_CMD_DRIVE:
|
||||
@ -159,7 +150,7 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
|
||||
int16_t spd, str;
|
||||
memcpy(&spd, payload, 2);
|
||||
memcpy(&str, payload + 2, 2);
|
||||
/* Clamp to +/-1000 */
|
||||
/* Clamp to ±1000 */
|
||||
if (spd > 1000) spd = 1000;
|
||||
if (spd < -1000) spd = -1000;
|
||||
if (str > 1000) str = 1000;
|
||||
@ -183,7 +174,7 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
|
||||
memcpy(&kp, payload, 4);
|
||||
memcpy(&ki, payload + 4, 4);
|
||||
memcpy(&kd, payload + 8, 4);
|
||||
/* Sanity bounds -- same as USB CDC PID handler in main.c */
|
||||
/* Sanity bounds — same as USB CDC PID handler in main.c */
|
||||
if (kp >= 0.0f && kp <= 500.0f) jlink_state.pid_kp = kp;
|
||||
if (ki >= 0.0f && ki <= 50.0f) jlink_state.pid_ki = ki;
|
||||
if (kd >= 0.0f && kd <= 50.0f) jlink_state.pid_kd = kd;
|
||||
@ -217,62 +208,12 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
|
||||
jlink_state.pid_save_req = 1u;
|
||||
break;
|
||||
|
||||
case JLINK_CMD_GIMBAL_POS:
|
||||
/* Payload: int16 pan_x10, int16 tilt_x10, uint16 speed (6 bytes) Issue #547 */
|
||||
if (plen == 6u) {
|
||||
int16_t pan, tilt;
|
||||
uint16_t spd;
|
||||
memcpy(&pan, payload, 2);
|
||||
memcpy(&tilt, payload + 2, 2);
|
||||
memcpy(&spd, payload + 4, 2);
|
||||
jlink_state.gimbal_pan_x10 = pan;
|
||||
jlink_state.gimbal_tilt_x10 = tilt;
|
||||
jlink_state.gimbal_speed = spd;
|
||||
jlink_state.gimbal_updated = 1u;
|
||||
}
|
||||
break;
|
||||
|
||||
case JLINK_CMD_SCHED_GET:
|
||||
/* Payload-less; main loop calls jlink_send_sched_telemetry() (Issue #550) */
|
||||
jlink_state.sched_get_req = 1u;
|
||||
break;
|
||||
|
||||
case JLINK_CMD_SCHED_SET:
|
||||
/* Payload: uint8 num_bands + num_bands * sizeof(pid_sched_entry_t) bytes */
|
||||
if (plen >= 1u) {
|
||||
uint8_t nb = payload[0];
|
||||
if (nb == 0u) nb = 1u;
|
||||
if (nb > PID_SCHED_MAX_BANDS) nb = PID_SCHED_MAX_BANDS;
|
||||
uint8_t expected = 1u + nb * (uint8_t)sizeof(pid_sched_entry_t);
|
||||
if (plen >= expected) {
|
||||
s_sched_set_buf.num_bands = nb;
|
||||
memcpy(s_sched_set_buf.bands, payload + 1,
|
||||
nb * sizeof(pid_sched_entry_t));
|
||||
s_sched_set_buf.ready = 1u;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case JLINK_CMD_SCHED_SAVE:
|
||||
/* Payload: float kp, float ki, float kd (12 bytes) for single-PID record */
|
||||
if (plen == 12u) {
|
||||
float kp, ki, kd;
|
||||
memcpy(&kp, payload, 4);
|
||||
memcpy(&ki, payload + 4, 4);
|
||||
memcpy(&kd, payload + 8, 4);
|
||||
if (kp >= 0.0f && kp <= 500.0f) jlink_state.sched_save_kp = kp;
|
||||
if (ki >= 0.0f && ki <= 50.0f) jlink_state.sched_save_ki = ki;
|
||||
if (kd >= 0.0f && kd <= 50.0f) jlink_state.sched_save_kd = kd;
|
||||
jlink_state.sched_save_req = 1u;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- jlink_process() -- call from main loop every tick ---- */
|
||||
/* ---- jlink_process() — call from main loop every tick ---- */
|
||||
/*
|
||||
* Parser state machine.
|
||||
* Frame: [STX][LEN][CMD][PAYLOAD 0..LEN-1][CRC_hi][CRC_lo][ETX]
|
||||
@ -281,7 +222,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 252u /* enlarged for AUDIO chunks (126 x int16) */
|
||||
#define JLINK_MAX_PAYLOAD 252u /* enlarged for AUDIO chunks (126 × int16) */
|
||||
|
||||
typedef enum {
|
||||
PS_WAIT_STX = 0,
|
||||
@ -301,7 +242,7 @@ void jlink_process(void)
|
||||
static uint8_t s_crc_hi = 0;
|
||||
|
||||
/* Compute how many bytes the DMA has written since last drain */
|
||||
uint32_t head = JLINK_RX_BUF_LEN - __HAL_DMA_GET_COUNTER(&s_dma_rx);
|
||||
uint32_t head = JLINK_RX_BUF_LEN - __HAL_DMA_GET_COUNTER(&s_dma_rx);
|
||||
uint32_t bytes = (head - s_rx_tail) & (JLINK_RX_BUF_LEN - 1u);
|
||||
|
||||
for (uint32_t i = 0; i < bytes; i++) {
|
||||
@ -315,7 +256,7 @@ void jlink_process(void)
|
||||
|
||||
case PS_WAIT_LEN:
|
||||
if (b == 0u || b > JLINK_MAX_PAYLOAD + 1u) {
|
||||
/* Invalid length -- resync */
|
||||
/* Invalid length — resync */
|
||||
s_state = PS_WAIT_STX;
|
||||
} else {
|
||||
s_len = b;
|
||||
@ -335,12 +276,12 @@ void jlink_process(void)
|
||||
break;
|
||||
|
||||
case PS_WAIT_CRC_LO: {
|
||||
uint16_t rx_crc = ((uint16_t)s_crc_hi << 8) | b;
|
||||
uint16_t rx_crc = ((uint16_t)s_crc_hi << 8) | b;
|
||||
uint16_t calc_crc = crc16_xmodem(s_frame, s_len);
|
||||
if (rx_crc == calc_crc)
|
||||
s_state = PS_WAIT_ETX;
|
||||
else
|
||||
s_state = PS_WAIT_STX; /* CRC mismatch -- drop */
|
||||
s_state = PS_WAIT_STX; /* CRC mismatch — drop */
|
||||
break;
|
||||
}
|
||||
|
||||
@ -363,7 +304,7 @@ void jlink_send_telemetry(const jlink_tlm_status_t *status)
|
||||
* Frame: [STX][LEN][0x80][20 bytes STATUS][CRC_hi][CRC_lo][ETX]
|
||||
* LEN = 1 (CMD) + 20 (payload) = 21
|
||||
* Total frame length = 1+1+1+20+2+1 = 26 bytes
|
||||
* At 921600 baud (10 bits/byte): 26x10/921600 ~0.28ms -- safe to block.
|
||||
* At 921600 baud (10 bits/byte): 26×10/921600 ≈ 0.28ms — safe to block.
|
||||
*/
|
||||
static uint8_t frame[26];
|
||||
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_status_t); /* 20 */
|
||||
@ -388,7 +329,7 @@ void jlink_send_power_telemetry(const jlink_tlm_power_t *power)
|
||||
/*
|
||||
* Frame: [STX][LEN][0x81][11 bytes POWER][CRC_hi][CRC_lo][ETX]
|
||||
* LEN = 1 (CMD) + 11 (payload) = 12; total = 17 bytes
|
||||
* At 921600 baud: 17x10/921600 ~0.18 ms -- safe to block.
|
||||
* At 921600 baud: 17×10/921600 ≈ 0.18 ms — safe to block.
|
||||
*/
|
||||
static uint8_t frame[17];
|
||||
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_power_t); /* 11 */
|
||||
@ -432,13 +373,13 @@ void jlink_send_pid_result(const jlink_tlm_pid_result_t *result)
|
||||
jlink_tx_locked(frame_pid, sizeof(frame_pid));
|
||||
}
|
||||
|
||||
/* ---- jlink_send_battery_telemetry() -- Issue #533 ---- */
|
||||
/* ---- jlink_send_battery_telemetry() — Issue #533 ---- */
|
||||
void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt)
|
||||
{
|
||||
/*
|
||||
* Frame: [STX][LEN][0x82][10 bytes BATTERY][CRC_hi][CRC_lo][ETX]
|
||||
* LEN = 1 (CMD) + 10 (payload) = 11; total = 16 bytes
|
||||
* At 921600 baud: 16x10/921600 ~0.17 ms -- safe to block.
|
||||
* At 921600 baud: 16×10/921600 ≈ 0.17 ms — safe to block.
|
||||
*/
|
||||
static uint8_t frame[16];
|
||||
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_battery_t); /* 10 */
|
||||
@ -456,58 +397,3 @@ void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt)
|
||||
|
||||
jlink_tx_locked(frame, sizeof(frame));
|
||||
}
|
||||
|
||||
/* ---- jlink_send_gimbal_state() -- Issue #547 ---- */
|
||||
void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state)
|
||||
{
|
||||
/*
|
||||
* Frame: [STX][LEN][0x84][10 bytes GIMBAL_STATE][CRC_hi][CRC_lo][ETX]
|
||||
* LEN = 1 (CMD) + 10 (payload) = 11; total = 16 bytes
|
||||
* At 921600 baud: 16x10/921600 ~0.17 ms -- safe to block.
|
||||
*/
|
||||
static uint8_t frame[16];
|
||||
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_gimbal_state_t); /* 10 */
|
||||
const uint8_t len = 1u + plen; /* 11 */
|
||||
|
||||
frame[0] = JLINK_STX;
|
||||
frame[1] = len;
|
||||
frame[2] = JLINK_TLM_GIMBAL_STATE;
|
||||
memcpy(&frame[3], state, plen);
|
||||
|
||||
uint16_t crc = crc16_xmodem(&frame[2], len);
|
||||
frame[3 + plen] = (uint8_t)(crc >> 8);
|
||||
frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
|
||||
frame[3 + plen + 2] = JLINK_ETX;
|
||||
|
||||
jlink_tx_locked(frame, sizeof(frame));
|
||||
}
|
||||
|
||||
/* ---- jlink_send_sched_telemetry() -- Issue #550 ---- */
|
||||
void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm)
|
||||
{
|
||||
/*
|
||||
* Frame: [STX][LEN][0x85][1+N*16 bytes SCHED][CRC_hi][CRC_lo][ETX]
|
||||
* Actual payload = 1 (num_bands) + tlm->num_bands * 16 bytes.
|
||||
* Max payload = 1 + 6*16 = 97; max frame = 103 bytes.
|
||||
* At 921600 baud: 103x10/921600 ~1.1 ms -- safe to block.
|
||||
*/
|
||||
uint8_t nb = tlm->num_bands;
|
||||
if (nb > PID_SCHED_MAX_BANDS) nb = PID_SCHED_MAX_BANDS;
|
||||
uint8_t plen = 1u + nb * (uint8_t)sizeof(pid_sched_entry_t);
|
||||
uint8_t len = 1u + plen; /* CMD + payload */
|
||||
/* frame: STX + LEN + CMD + payload + CRC_hi + CRC_lo + ETX */
|
||||
uint8_t frame[103];
|
||||
|
||||
frame[0] = JLINK_STX;
|
||||
frame[1] = len;
|
||||
frame[2] = JLINK_TLM_SCHED;
|
||||
frame[3] = nb;
|
||||
memcpy(&frame[4], tlm->bands, nb * sizeof(pid_sched_entry_t));
|
||||
|
||||
uint16_t crc = crc16_xmodem(&frame[2], len);
|
||||
frame[3 + plen] = (uint8_t)(crc >> 8);
|
||||
frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
|
||||
frame[3 + plen + 2] = JLINK_ETX;
|
||||
|
||||
jlink_tx_locked(frame, (uint16_t)(3u + plen + 3u));
|
||||
}
|
||||
|
||||
34
src/main.c
34
src/main.c
@ -31,8 +31,6 @@
|
||||
#include "coulomb_counter.h"
|
||||
#include "watchdog.h"
|
||||
#include "pid_flash.h"
|
||||
#include "servo_bus.h"
|
||||
#include "gimbal.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
@ -210,13 +208,6 @@ int main(void) {
|
||||
/* Init servo pan-tilt driver for camera head (TIM4 PWM on PB6/PB7, Issue #206) */
|
||||
servo_init();
|
||||
|
||||
/* Init ST3215 serial bus servo driver (USART3 half-duplex PB10, Issue #547) */
|
||||
servo_bus_init();
|
||||
|
||||
/* Init pan/tilt gimbal (ST3215 servos, centers both axes on boot) */
|
||||
gimbal_t gimbal;
|
||||
gimbal_init(&gimbal);
|
||||
|
||||
/* Init HC-SR04 ultrasonic distance sensor (TIM1 input capture on PA1, Issue #243) */
|
||||
ultrasonic_init();
|
||||
|
||||
@ -279,9 +270,6 @@ int main(void) {
|
||||
/* Servo pan-tilt animation tick — updates smooth sweeps */
|
||||
servo_tick(now);
|
||||
|
||||
/* Gimbal ST3215 feedback polling tick (self-throttles to GIMBAL_TLM_HZ) */
|
||||
gimbal_tick(&gimbal, now);
|
||||
|
||||
/* Accumulate coulombs for battery state-of-charge estimation (Issue #325) */
|
||||
battery_accumulate_coulombs();
|
||||
|
||||
@ -359,15 +347,6 @@ int main(void) {
|
||||
jlink_state.sleep_req = 0u;
|
||||
power_mgmt_request_sleep();
|
||||
}
|
||||
/* GIMBAL_POS: forward new pan/tilt command to gimbal (Issue #547) */
|
||||
if (jlink_state.gimbal_updated) {
|
||||
jlink_state.gimbal_updated = 0u;
|
||||
gimbal_set_pos(&gimbal,
|
||||
(int16_t)jlink_state.gimbal_pan_x10,
|
||||
(int16_t)jlink_state.gimbal_tilt_x10,
|
||||
jlink_state.gimbal_speed);
|
||||
}
|
||||
|
||||
/* PID_SAVE: persist current gains to flash, reply with result (Issue #531).
|
||||
* Only allowed while disarmed -- flash sector erase stalls CPU for ~1 s. */
|
||||
if (jlink_state.pid_save_req && bal.state != BALANCE_ARMED) {
|
||||
@ -579,19 +558,6 @@ int main(void) {
|
||||
tlm.fw_minor = FW_MINOR;
|
||||
tlm.fw_patch = FW_PATCH;
|
||||
jlink_send_telemetry(&tlm);
|
||||
|
||||
/* Send gimbal state at same 50 Hz cadence (Issue #547) */
|
||||
jlink_tlm_gimbal_state_t gtlm;
|
||||
gtlm.pan_x10 = gimbal.fb_pan_x10;
|
||||
gtlm.tilt_x10 = gimbal.fb_tilt_x10;
|
||||
gtlm.pan_speed_raw = gimbal.fb_pan_speed;
|
||||
gtlm.tilt_speed_raw = gimbal.fb_tilt_speed;
|
||||
gtlm.torque_en = gimbal.torque_enabled ? 1u : 0u;
|
||||
uint32_t rx_total = gimbal.rx_ok + gimbal.rx_err;
|
||||
gtlm.rx_err_pct = (rx_total > 0u)
|
||||
? (uint8_t)(gimbal.rx_err * 100u / rx_total)
|
||||
: 0u;
|
||||
jlink_send_gimbal_state(>lm);
|
||||
}
|
||||
|
||||
/* JLINK_TLM_POWER telemetry at PM_TLM_HZ (1 Hz) */
|
||||
|
||||
@ -8,7 +8,7 @@ bool pid_flash_load(float *kp, float *ki, float *kd)
|
||||
|
||||
if (p->magic != PID_FLASH_MAGIC) return false;
|
||||
|
||||
/* Basic sanity bounds — same as JLINK_CMD_PID_SET handler */
|
||||
/* Basic sanity bounds -- same as JLINK_CMD_PID_SET handler */
|
||||
if (p->kp < 0.0f || p->kp > 500.0f) return false;
|
||||
if (p->ki < 0.0f || p->ki > 50.0f) return false;
|
||||
if (p->kd < 0.0f || p->kd > 50.0f) return false;
|
||||
@ -49,7 +49,7 @@ bool pid_flash_save(float kp, float ki, float kd)
|
||||
rec.ki = ki;
|
||||
rec.kd = kd;
|
||||
|
||||
/* Write 64 bytes as 16 × 32-bit words */
|
||||
/* Write 64 bytes as 16 x 32-bit words */
|
||||
const uint32_t *src = (const uint32_t *)&rec;
|
||||
uint32_t addr = PID_FLASH_STORE_ADDR;
|
||||
for (uint8_t i = 0; i < sizeof(rec) / 4u; i++) {
|
||||
@ -70,3 +70,92 @@ bool pid_flash_save(float kp, float ki, float kd)
|
||||
stored->ki == ki &&
|
||||
stored->kd == kd);
|
||||
}
|
||||
|
||||
/* ---- Helper: write N bytes to flash as 32-bit words ---- */
|
||||
static bool flash_write_words(uint32_t addr, const void *src_buf, uint32_t byte_len)
|
||||
{
|
||||
const uint32_t *w = (const uint32_t *)src_buf;
|
||||
for (uint32_t i = 0; i < byte_len / 4u; i++) {
|
||||
if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, addr, w[i]) != HAL_OK)
|
||||
return false;
|
||||
addr += 4u;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ---- pid_flash_load_schedule() ---- */
|
||||
bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n)
|
||||
{
|
||||
const pid_sched_flash_t *p = (const pid_sched_flash_t *)PID_SCHED_FLASH_ADDR;
|
||||
|
||||
if (p->magic != PID_SCHED_MAGIC) return false;
|
||||
if (p->num_bands == 0u || p->num_bands > PID_SCHED_MAX_BANDS) return false;
|
||||
|
||||
memcpy(out_entries, p->bands, p->num_bands * sizeof(pid_sched_entry_t));
|
||||
*out_n = p->num_bands;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ---- pid_flash_save_all() ---- */
|
||||
bool pid_flash_save_all(float kp_single, float ki_single, float kd_single,
|
||||
const pid_sched_entry_t *entries, uint8_t num_bands)
|
||||
{
|
||||
HAL_StatusTypeDef rc;
|
||||
|
||||
if (num_bands == 0u || num_bands > PID_SCHED_MAX_BANDS) return false;
|
||||
|
||||
rc = HAL_FLASH_Unlock();
|
||||
if (rc != HAL_OK) return false;
|
||||
|
||||
/* Erase sector 7 -- one erase covers both records */
|
||||
FLASH_EraseInitTypeDef erase = {
|
||||
.TypeErase = FLASH_TYPEERASE_SECTORS,
|
||||
.Sector = PID_FLASH_SECTOR,
|
||||
.NbSectors = 1,
|
||||
.VoltageRange = PID_FLASH_SECTOR_VOLTAGE,
|
||||
};
|
||||
uint32_t sector_error = 0u;
|
||||
rc = HAL_FLASHEx_Erase(&erase, §or_error);
|
||||
if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) {
|
||||
HAL_FLASH_Lock();
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Build and write schedule record at PID_SCHED_FLASH_ADDR */
|
||||
pid_sched_flash_t srec;
|
||||
memset(&srec, 0xFF, sizeof(srec));
|
||||
srec.magic = PID_SCHED_MAGIC;
|
||||
srec.num_bands = num_bands;
|
||||
srec.flags = 0u;
|
||||
srec._pad0[0] = 0u;
|
||||
srec._pad0[1] = 0u;
|
||||
memcpy(srec.bands, entries, num_bands * sizeof(pid_sched_entry_t));
|
||||
|
||||
if (!flash_write_words(PID_SCHED_FLASH_ADDR, &srec, sizeof(srec))) {
|
||||
HAL_FLASH_Lock();
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Build and write single-PID record at PID_FLASH_STORE_ADDR */
|
||||
pid_flash_t prec;
|
||||
memset(&prec, 0xFF, sizeof(prec));
|
||||
prec.magic = PID_FLASH_MAGIC;
|
||||
prec.kp = kp_single;
|
||||
prec.ki = ki_single;
|
||||
prec.kd = kd_single;
|
||||
|
||||
if (!flash_write_words(PID_FLASH_STORE_ADDR, &prec, sizeof(prec))) {
|
||||
HAL_FLASH_Lock();
|
||||
return false;
|
||||
}
|
||||
|
||||
HAL_FLASH_Lock();
|
||||
|
||||
/* Verify both records */
|
||||
const pid_sched_flash_t *sv = (const pid_sched_flash_t *)PID_SCHED_FLASH_ADDR;
|
||||
const pid_flash_t *pv = (const pid_flash_t *)PID_FLASH_STORE_ADDR;
|
||||
|
||||
return (sv->magic == PID_SCHED_MAGIC && sv->num_bands == num_bands &&
|
||||
pv->magic == PID_FLASH_MAGIC && pv->kp == kp_single &&
|
||||
pv->ki == ki_single && pv->kd == kd_single);
|
||||
}
|
||||
|
||||
189
src/servo_bus.c
189
src/servo_bus.c
@ -1,189 +0,0 @@
|
||||
#include "servo_bus.h"
|
||||
#include "config.h"
|
||||
#include "stm32f7xx_hal.h"
|
||||
#include <string.h>
|
||||
|
||||
/*
|
||||
* servo_bus.c — Feetech/ST3215 serial bus servo driver (Issue #547)
|
||||
*
|
||||
* Half-duplex on USART3 PB10 (USART3_TX, AF7) at 1 Mbps.
|
||||
* HDSEL bit (CR3.3) routes TX and RX to the TX pin.
|
||||
* TX direction: TX active, RX gated.
|
||||
* RX direction: TX idle, RX active — enabled after TC interrupt.
|
||||
*
|
||||
* All operations are blocking with short timeouts; the servo bus
|
||||
* is polled at 50 Hz from the main loop (not from an ISR).
|
||||
*/
|
||||
|
||||
static UART_HandleTypeDef s_uart;
|
||||
|
||||
/* ---- Init ---- */
|
||||
|
||||
void servo_bus_init(void)
|
||||
{
|
||||
/* GPIO: PB10 open-drain AF7 (USART3_TX) with external pull-up on bus */
|
||||
__HAL_RCC_GPIOB_CLK_ENABLE();
|
||||
GPIO_InitTypeDef gpio = {0};
|
||||
gpio.Pin = SERVO_BUS_PIN;
|
||||
gpio.Mode = GPIO_MODE_AF_OD; /* open-drain for half-duplex bus */
|
||||
gpio.Pull = GPIO_PULLUP;
|
||||
gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
|
||||
gpio.Alternate = GPIO_AF7_USART3;
|
||||
HAL_GPIO_Init(SERVO_BUS_PORT, &gpio);
|
||||
|
||||
/* USART3 at SERVO_BUS_BAUD (1 Mbps), 8N1 */
|
||||
__HAL_RCC_USART3_CLK_ENABLE();
|
||||
s_uart.Instance = USART3;
|
||||
s_uart.Init.BaudRate = SERVO_BUS_BAUD;
|
||||
s_uart.Init.WordLength = UART_WORDLENGTH_8B;
|
||||
s_uart.Init.StopBits = UART_STOPBITS_1;
|
||||
s_uart.Init.Parity = UART_PARITY_NONE;
|
||||
s_uart.Init.Mode = UART_MODE_TX_RX;
|
||||
s_uart.Init.HwFlowCtl = UART_HWCONTROL_NONE;
|
||||
s_uart.Init.OverSampling = UART_OVERSAMPLING_8; /* required for 1 Mbps @ APB1 54 MHz */
|
||||
HAL_UART_Init(&s_uart);
|
||||
|
||||
/* Enable half-duplex: HDSEL bit in CR3 routes TX/RX to TX pin */
|
||||
SET_BIT(s_uart.Instance->CR3, USART_CR3_HDSEL);
|
||||
}
|
||||
|
||||
/* ---- Checksum ---- */
|
||||
|
||||
static uint8_t _cksum(uint8_t id, uint8_t len, uint8_t instr,
|
||||
const uint8_t *params, uint8_t nparams)
|
||||
{
|
||||
uint16_t sum = (uint16_t)id + len + instr;
|
||||
for (uint8_t i = 0; i < nparams; i++) sum += params[i];
|
||||
return (uint8_t)(~sum & 0xFFu);
|
||||
}
|
||||
|
||||
/* ---- Transmit a raw packet ---- */
|
||||
|
||||
static bool _tx(uint8_t id, uint8_t instr, const uint8_t *params, uint8_t nparams)
|
||||
{
|
||||
uint8_t len = nparams + 2u; /* instr + params + cksum */
|
||||
uint8_t buf[32];
|
||||
uint8_t n = 0;
|
||||
|
||||
buf[n++] = 0xFFu;
|
||||
buf[n++] = 0xFFu;
|
||||
buf[n++] = id;
|
||||
buf[n++] = len;
|
||||
buf[n++] = instr;
|
||||
for (uint8_t i = 0; i < nparams; i++) buf[n++] = params[i];
|
||||
buf[n++] = _cksum(id, len, instr, params, nparams);
|
||||
|
||||
return HAL_UART_Transmit(&s_uart, buf, n, 10u) == HAL_OK;
|
||||
}
|
||||
|
||||
/* ---- Receive response packet ---- */
|
||||
|
||||
/*
|
||||
* Read one byte with timeout. In half-duplex the TX line is still driving
|
||||
* after the last stop bit; we must wait ~1 bit-time before the servo begins
|
||||
* its response. HAL_UART_Receive handles the timing via the timeout.
|
||||
*/
|
||||
static bool _rx(uint8_t id, uint8_t *data, uint8_t ndata)
|
||||
{
|
||||
uint8_t hdr[4]; /* 0xFF 0xFF ID LEN */
|
||||
uint8_t tail[2]; /* ERROR CKSUM (or just CKSUM if ndata==0) */
|
||||
|
||||
/* Read header */
|
||||
if (HAL_UART_Receive(&s_uart, hdr, 4u, SB_RX_TIMEOUT_MS) != HAL_OK)
|
||||
return false;
|
||||
|
||||
/* Validate header */
|
||||
if (hdr[0] != 0xFFu || hdr[1] != 0xFFu || hdr[2] != id)
|
||||
return false;
|
||||
|
||||
uint8_t pkt_len = hdr[3]; /* ERROR + DATA... + CKSUM */
|
||||
if (pkt_len < 2u || pkt_len > 20u) /* sanity */
|
||||
return false;
|
||||
|
||||
/* Read payload: ERROR + DATA bytes + CKSUM */
|
||||
uint8_t payload[20];
|
||||
uint8_t payload_len = pkt_len; /* includes error byte and cksum */
|
||||
if (HAL_UART_Receive(&s_uart, payload, payload_len, SB_RX_TIMEOUT_MS) != HAL_OK)
|
||||
return false;
|
||||
|
||||
/* Verify checksum: ~(ID + LEN + ERROR + DATA) */
|
||||
uint16_t sum = (uint16_t)id + pkt_len;
|
||||
for (uint8_t i = 0; i < (uint8_t)(pkt_len - 1u); i++) sum += payload[i];
|
||||
uint8_t expected_cksum = (uint8_t)(~sum & 0xFFu);
|
||||
if (payload[pkt_len - 1u] != expected_cksum)
|
||||
return false;
|
||||
|
||||
/* payload[0] = ERROR; payload[1..] = data */
|
||||
if (payload[0] != 0u) return false; /* servo reported error */
|
||||
|
||||
uint8_t n_data_bytes = pkt_len - 2u; /* minus ERROR and CKSUM */
|
||||
if (n_data_bytes < ndata) return false;
|
||||
|
||||
for (uint8_t i = 0; i < ndata; i++) data[i] = payload[1u + i];
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ---- Public API ---- */
|
||||
|
||||
bool servo_bus_write_pos(uint8_t id, uint16_t raw_pos, uint16_t speed)
|
||||
{
|
||||
if (raw_pos > SB_POS_MAX) raw_pos = SB_POS_MAX;
|
||||
if (speed > SB_SPEED_MAX) speed = SB_SPEED_MAX;
|
||||
|
||||
/* Write 4 bytes starting at SB_REG_GOAL_POS_L:
|
||||
* [addr][POS_L][POS_H][SPD_L][SPD_H] */
|
||||
uint8_t params[5] = {
|
||||
SB_REG_GOAL_POS_L,
|
||||
(uint8_t)(raw_pos & 0xFFu),
|
||||
(uint8_t)(raw_pos >> 8),
|
||||
(uint8_t)(speed & 0xFFu),
|
||||
(uint8_t)(speed >> 8),
|
||||
};
|
||||
return _tx(id, SB_INSTR_WRITE, params, 5u);
|
||||
/* WRITE does not generate a status packet by default */
|
||||
}
|
||||
|
||||
bool servo_bus_write_torque(uint8_t id, bool enable)
|
||||
{
|
||||
uint8_t params[2] = { SB_REG_TORQUE_EN, enable ? 1u : 0u };
|
||||
return _tx(id, SB_INSTR_WRITE, params, 2u);
|
||||
}
|
||||
|
||||
bool servo_bus_read_pos(uint8_t id, uint16_t *raw_pos)
|
||||
{
|
||||
/* READ instruction: [addr][count] */
|
||||
uint8_t params[2] = { SB_REG_PRES_POS_L, 2u };
|
||||
if (!_tx(id, SB_INSTR_READ, params, 2u)) return false;
|
||||
|
||||
uint8_t data[2];
|
||||
if (!_rx(id, data, 2u)) return false;
|
||||
|
||||
*raw_pos = (uint16_t)data[0] | ((uint16_t)data[1] << 8);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool servo_bus_read_speed(uint8_t id, uint16_t *speed)
|
||||
{
|
||||
uint8_t params[2] = { SB_REG_PRES_SPD_L, 2u };
|
||||
if (!_tx(id, SB_INSTR_READ, params, 2u)) return false;
|
||||
|
||||
uint8_t data[2];
|
||||
if (!_rx(id, data, 2u)) return false;
|
||||
|
||||
*speed = (uint16_t)data[0] | ((uint16_t)data[1] << 8);
|
||||
return true;
|
||||
}
|
||||
|
||||
uint16_t servo_bus_deg_to_raw(float deg)
|
||||
{
|
||||
/* Center = 0 deg = 2048 raw; 1 deg = 4096/360 raw */
|
||||
int32_t raw = (int32_t)(SB_POS_CENTER + (int32_t)(deg * (4096.0f / 360.0f)));
|
||||
if (raw < 0) raw = 0;
|
||||
if (raw > (int32_t)SB_POS_MAX) raw = (int32_t)SB_POS_MAX;
|
||||
return (uint16_t)raw;
|
||||
}
|
||||
|
||||
float servo_bus_raw_to_deg(uint16_t raw)
|
||||
{
|
||||
return ((float)raw - (float)SB_POS_CENTER) * (360.0f / 4096.0f);
|
||||
}
|
||||
@ -1,230 +0,0 @@
|
||||
/* diagnostics_panel.css — Saltybot System Diagnostics Dashboard (Issue #562) */
|
||||
|
||||
*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
|
||||
|
||||
:root {
|
||||
--bg0: #050510;
|
||||
--bg1: #070712;
|
||||
--bg2: #0a0a1a;
|
||||
--border: #0c2a3a;
|
||||
--border2: #1e3a5f;
|
||||
--text-dim: #374151;
|
||||
--text-mid: #6b7280;
|
||||
--text-base: #9ca3af;
|
||||
--text-hi: #d1d5db;
|
||||
--cyan: #06b6d4;
|
||||
--cyan-dim: #0e4f69;
|
||||
--green: #22c55e;
|
||||
--amber: #f59e0b;
|
||||
--red: #ef4444;
|
||||
--orange: #f97316;
|
||||
}
|
||||
|
||||
body {
|
||||
font-family: 'Courier New', Courier, monospace;
|
||||
font-size: 12px;
|
||||
background: var(--bg0);
|
||||
color: var(--text-base);
|
||||
min-height: 100dvh;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
}
|
||||
|
||||
/* ── Header ── */
|
||||
#header {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
padding: 6px 16px;
|
||||
background: var(--bg1);
|
||||
border-bottom: 1px solid var(--border);
|
||||
flex-shrink: 0;
|
||||
flex-wrap: wrap;
|
||||
gap: 8px;
|
||||
}
|
||||
.logo { color: #f97316; font-weight: bold; letter-spacing: 0.15em; font-size: 13px; }
|
||||
|
||||
#conn-bar { display: flex; align-items: center; gap: 6px; }
|
||||
#conn-dot {
|
||||
width: 8px; height: 8px; border-radius: 50%;
|
||||
background: var(--text-dim); flex-shrink: 0; transition: background 0.3s;
|
||||
}
|
||||
#conn-dot.connected { background: var(--green); }
|
||||
#conn-dot.error { background: var(--red); animation: blink 1s infinite; }
|
||||
|
||||
@keyframes blink { 0%,100% { opacity:1; } 50% { opacity:0.4; } }
|
||||
|
||||
#ws-input {
|
||||
background: var(--bg2); border: 1px solid var(--border2); border-radius: 4px;
|
||||
color: #67e8f9; padding: 2px 8px; font-family: monospace; font-size: 11px; width: 200px;
|
||||
}
|
||||
#ws-input:focus { outline: none; border-color: var(--cyan); }
|
||||
|
||||
.hdr-btn {
|
||||
padding: 3px 10px; border-radius: 4px; border: 1px solid var(--border2);
|
||||
background: var(--bg2); color: #67e8f9; font-family: monospace;
|
||||
font-size: 10px; font-weight: bold; cursor: pointer; transition: background 0.15s;
|
||||
}
|
||||
.hdr-btn:hover { background: var(--cyan-dim); }
|
||||
|
||||
#refresh-info {
|
||||
font-size: 10px; color: var(--text-mid);
|
||||
display: flex; align-items: center; gap: 4px;
|
||||
}
|
||||
#refresh-dot {
|
||||
width: 6px; height: 6px; border-radius: 50%;
|
||||
background: var(--text-dim); transition: background 0.2s;
|
||||
}
|
||||
#refresh-dot.pulse { background: var(--cyan); animation: blink 0.4s; }
|
||||
|
||||
/* ── Status bar ── */
|
||||
#status-bar {
|
||||
display: flex; gap: 8px; align-items: center;
|
||||
padding: 4px 16px; background: var(--bg1);
|
||||
border-bottom: 1px solid var(--border); font-size: 10px; flex-wrap: wrap;
|
||||
}
|
||||
.sys-badge {
|
||||
padding: 2px 8px; border-radius: 3px; font-weight: bold;
|
||||
border: 1px solid; letter-spacing: 0.05em;
|
||||
}
|
||||
.badge-ok { background: #052e16; border-color: #166534; color: #4ade80; }
|
||||
.badge-warn { background: #451a03; border-color: #92400e; color: #fcd34d; }
|
||||
.badge-error { background: #450a0a; border-color: #991b1b; color: #f87171; animation: blink 1s infinite; }
|
||||
.badge-stale { background: #111827; border-color: #374151; color: #6b7280; }
|
||||
#last-update { color: var(--text-mid); margin-left: auto; }
|
||||
|
||||
/* ── Dashboard grid ── */
|
||||
#dashboard {
|
||||
flex: 1;
|
||||
display: grid;
|
||||
grid-template-columns: repeat(3, 1fr);
|
||||
grid-template-rows: auto;
|
||||
gap: 12px;
|
||||
padding: 12px;
|
||||
align-content: start;
|
||||
overflow-y: auto;
|
||||
}
|
||||
|
||||
@media (max-width: 1024px) { #dashboard { grid-template-columns: repeat(2, 1fr); } }
|
||||
@media (max-width: 640px) { #dashboard { grid-template-columns: 1fr; } }
|
||||
|
||||
/* Spanning cards */
|
||||
.span2 { grid-column: span 2; }
|
||||
.span3 { grid-column: 1 / -1; }
|
||||
|
||||
/* ── Card ── */
|
||||
.card {
|
||||
background: var(--bg1);
|
||||
border: 1px solid var(--border);
|
||||
border-radius: 8px;
|
||||
padding: 10px 12px;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 8px;
|
||||
}
|
||||
.card.alert-warn { border-color: #92400e; }
|
||||
.card.alert-error { border-color: #991b1b; }
|
||||
|
||||
.card-title {
|
||||
font-size: 9px; font-weight: bold; letter-spacing: 0.15em;
|
||||
color: #0891b2; text-transform: uppercase;
|
||||
display: flex; align-items: center; justify-content: space-between;
|
||||
}
|
||||
.card-title .badge { font-size: 9px; padding: 1px 6px; border-radius: 3px; }
|
||||
|
||||
/* ── Gauge bar ── */
|
||||
.gauge-row {
|
||||
display: flex; flex-direction: column; gap: 3px;
|
||||
}
|
||||
.gauge-label-row {
|
||||
display: flex; justify-content: space-between; font-size: 10px;
|
||||
}
|
||||
.gauge-label { color: var(--text-mid); }
|
||||
.gauge-value { font-family: monospace; }
|
||||
.gauge-track {
|
||||
width: 100%; height: 8px;
|
||||
background: var(--bg2); border-radius: 4px;
|
||||
overflow: hidden; border: 1px solid var(--border2);
|
||||
}
|
||||
.gauge-fill {
|
||||
height: 100%; border-radius: 4px;
|
||||
transition: width 0.5s ease, background 0.5s ease;
|
||||
}
|
||||
|
||||
/* ── Big metric ── */
|
||||
.big-metric {
|
||||
display: flex; align-items: baseline; gap: 4px;
|
||||
}
|
||||
.big-num { font-size: 28px; font-weight: bold; font-family: monospace; }
|
||||
.big-unit { font-size: 11px; color: var(--text-mid); }
|
||||
|
||||
/* ── Sparkline ── */
|
||||
#batt-sparkline {
|
||||
width: 100%; border-radius: 4px;
|
||||
border: 1px solid var(--border2);
|
||||
background: var(--bg2);
|
||||
display: block;
|
||||
}
|
||||
|
||||
/* ── Node list ── */
|
||||
.node-row {
|
||||
display: flex; align-items: center; justify-content: space-between;
|
||||
padding: 3px 0; border-bottom: 1px solid var(--border);
|
||||
font-size: 10px;
|
||||
}
|
||||
.node-row:last-child { border-bottom: none; }
|
||||
.node-name { color: var(--text-base); font-family: monospace; }
|
||||
.node-status {
|
||||
padding: 1px 6px; border-radius: 3px; font-size: 9px; font-weight: bold;
|
||||
}
|
||||
.ns-ok { background: #052e16; color: #4ade80; }
|
||||
.ns-warn { background: #451a03; color: #fcd34d; }
|
||||
.ns-error { background: #450a0a; color: #f87171; }
|
||||
.ns-stale { background: #111827; color: #6b7280; }
|
||||
|
||||
/* ── Temp arc display ── */
|
||||
.temp-grid {
|
||||
display: grid; grid-template-columns: repeat(2, 1fr); gap: 6px;
|
||||
}
|
||||
.temp-box {
|
||||
background: var(--bg2); border: 1px solid var(--border2);
|
||||
border-radius: 6px; padding: 6px; text-align: center;
|
||||
}
|
||||
.temp-label { font-size: 9px; color: var(--text-mid); margin-bottom: 2px; }
|
||||
.temp-value { font-size: 22px; font-weight: bold; font-family: monospace; }
|
||||
.temp-bar-track {
|
||||
margin-top: 4px; width: 100%; height: 4px;
|
||||
background: var(--bg0); border-radius: 2px; overflow: hidden;
|
||||
}
|
||||
.temp-bar-fill { height: 100%; border-radius: 2px; transition: width 0.5s; }
|
||||
|
||||
/* ── Motor section ── */
|
||||
.motor-grid {
|
||||
display: grid; grid-template-columns: repeat(2, 1fr); gap: 8px;
|
||||
}
|
||||
.motor-box {
|
||||
background: var(--bg2); border: 1px solid var(--border2);
|
||||
border-radius: 6px; padding: 8px;
|
||||
}
|
||||
.motor-label { font-size: 9px; color: var(--text-mid); margin-bottom: 4px; }
|
||||
|
||||
/* ── WiFi bars ── */
|
||||
.rssi-bars { display: flex; align-items: flex-end; gap: 3px; }
|
||||
.rssi-bar { width: 6px; border-radius: 2px 2px 0 0; }
|
||||
|
||||
/* ── MQTT indicator ── */
|
||||
.mqtt-status { display: flex; align-items: center; gap: 6px; }
|
||||
.mqtt-dot {
|
||||
width: 10px; height: 10px; border-radius: 50%;
|
||||
background: var(--text-dim); transition: background 0.3s;
|
||||
}
|
||||
.mqtt-dot.connected { background: var(--green); animation: none; }
|
||||
.mqtt-dot.disconnected { background: var(--red); animation: blink 1s infinite; }
|
||||
|
||||
/* ── Footer ── */
|
||||
#footer {
|
||||
background: var(--bg1); border-top: 1px solid var(--border);
|
||||
padding: 4px 16px;
|
||||
display: flex; align-items: center; justify-content: space-between;
|
||||
flex-shrink: 0; font-size: 10px; color: var(--text-dim);
|
||||
}
|
||||
@ -1,267 +0,0 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="en">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
|
||||
<title>Saltybot — System Diagnostics</title>
|
||||
<link rel="stylesheet" href="diagnostics_panel.css">
|
||||
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script>
|
||||
</head>
|
||||
<body>
|
||||
|
||||
<!-- ── Header ── -->
|
||||
<div id="header">
|
||||
<div class="logo">⚡ SALTYBOT — DIAGNOSTICS</div>
|
||||
|
||||
<div id="conn-bar">
|
||||
<div id="conn-dot"></div>
|
||||
<input id="ws-input" type="text" value="ws://localhost:9090" placeholder="ws://robot-ip:9090" />
|
||||
<button id="btn-connect" class="hdr-btn">CONNECT</button>
|
||||
<span id="conn-label" style="color:#4b5563;font-size:10px">Not connected</span>
|
||||
</div>
|
||||
|
||||
<div id="refresh-info">
|
||||
<div id="refresh-dot"></div>
|
||||
<span>auto 2 Hz</span>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- ── System status bar ── -->
|
||||
<div id="status-bar">
|
||||
<span style="color:#6b7280;font-size:10px">SYSTEM</span>
|
||||
<span class="sys-badge badge-stale" id="system-badge">STALE</span>
|
||||
<span style="color:#4b5563">│</span>
|
||||
<span style="color:#6b7280;font-size:10px">BATTERY</span>
|
||||
<span class="sys-badge badge-stale" id="batt-badge">—</span>
|
||||
<span style="color:#4b5563">│</span>
|
||||
<span style="color:#6b7280;font-size:10px">THERMAL</span>
|
||||
<span class="sys-badge badge-stale" id="temp-badge">—</span>
|
||||
<span style="color:#4b5563">│</span>
|
||||
<span style="color:#6b7280;font-size:10px">NETWORK</span>
|
||||
<span class="sys-badge badge-stale" id="net-badge">—</span>
|
||||
<span id="last-update">Awaiting data…</span>
|
||||
</div>
|
||||
|
||||
<!-- ── Dashboard grid ── -->
|
||||
<div id="dashboard">
|
||||
|
||||
<!-- ╔═══════════════════ BATTERY ═══════════════════╗ -->
|
||||
<div class="card span2">
|
||||
<div class="card-title">
|
||||
BATTERY — 4S LiPo (12.0–16.8 V)
|
||||
<span class="badge badge-stale" id="batt-badge-2" style="font-size:9px">—</span>
|
||||
</div>
|
||||
|
||||
<!-- Big readout row -->
|
||||
<div style="display:flex;gap:16px;align-items:flex-end;flex-wrap:wrap">
|
||||
<div>
|
||||
<div style="font-size:9px;color:#6b7280;margin-bottom:2px">VOLTAGE</div>
|
||||
<div class="big-metric">
|
||||
<span class="big-num" id="batt-voltage" style="color:#22c55e">—</span>
|
||||
</div>
|
||||
</div>
|
||||
<div>
|
||||
<div style="font-size:9px;color:#6b7280;margin-bottom:2px">SOC</div>
|
||||
<div class="big-metric">
|
||||
<span class="big-num" id="batt-soc" style="color:#22c55e">—</span>
|
||||
</div>
|
||||
</div>
|
||||
<div>
|
||||
<div style="font-size:9px;color:#6b7280;margin-bottom:2px">CURRENT</div>
|
||||
<div class="big-metric">
|
||||
<span class="big-num" id="batt-current" style="color:#06b6d4;font-size:20px">—</span>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Gauge bars -->
|
||||
<div class="gauge-row">
|
||||
<div class="gauge-label-row">
|
||||
<span class="gauge-label">Voltage</span>
|
||||
<span class="gauge-value" id="batt-voltage-2" style="color:#6b7280">12.0–16.8 V</span>
|
||||
</div>
|
||||
<div class="gauge-track"><div class="gauge-fill" id="batt-volt-bar" style="width:0%"></div></div>
|
||||
</div>
|
||||
<div class="gauge-row">
|
||||
<div class="gauge-label-row">
|
||||
<span class="gauge-label">State of Charge</span>
|
||||
<span class="gauge-value" style="color:#6b7280">0–100%</span>
|
||||
</div>
|
||||
<div class="gauge-track"><div class="gauge-fill" id="batt-soc-bar" style="width:0%"></div></div>
|
||||
</div>
|
||||
|
||||
<!-- Sparkline history -->
|
||||
<div>
|
||||
<div style="font-size:9px;color:#6b7280;margin-bottom:4px">VOLTAGE HISTORY (last 2 min)</div>
|
||||
<canvas id="batt-sparkline" height="56"></canvas>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- ╔═══════════════════ TEMPERATURES ═══════════════════╗ -->
|
||||
<div class="card">
|
||||
<div class="card-title">TEMPERATURES</div>
|
||||
<div class="temp-grid">
|
||||
<div class="temp-box" id="cpu-temp-box">
|
||||
<div class="temp-label">CPU (Jetson)</div>
|
||||
<div class="temp-value" id="cpu-temp-val">—</div>
|
||||
<div class="temp-bar-track"><div class="temp-bar-fill" id="cpu-temp-bar" style="width:0%"></div></div>
|
||||
</div>
|
||||
<div class="temp-box" id="gpu-temp-box">
|
||||
<div class="temp-label">GPU (Jetson)</div>
|
||||
<div class="temp-value" id="gpu-temp-val">—</div>
|
||||
<div class="temp-bar-track"><div class="temp-bar-fill" id="gpu-temp-bar" style="width:0%"></div></div>
|
||||
</div>
|
||||
<div class="temp-box" id="board-temp-box">
|
||||
<div class="temp-label">Board / STM32</div>
|
||||
<div class="temp-value" id="board-temp-val">—</div>
|
||||
<div class="temp-bar-track"><div class="temp-bar-fill" id="board-temp-bar" style="width:0%"></div></div>
|
||||
</div>
|
||||
<div class="temp-box" id="motor-temp-l-box">
|
||||
<div class="temp-label">Motor L</div>
|
||||
<div class="temp-value" id="motor-temp-l-val">—</div>
|
||||
<div class="temp-bar-track"><div class="temp-bar-fill" id="motor-temp-l-bar" style="width:0%"></div></div>
|
||||
</div>
|
||||
<!-- spacer to keep 2-col grid balanced if motor-temp-r is alone -->
|
||||
<div class="temp-box" id="motor-temp-r-box">
|
||||
<div class="temp-label">Motor R</div>
|
||||
<div class="temp-value" id="motor-temp-r-val">—</div>
|
||||
<div class="temp-bar-track"><div class="temp-bar-fill" id="motor-temp-r-bar" style="width:0%"></div></div>
|
||||
</div>
|
||||
</div>
|
||||
<div style="font-size:9px;color:#374151">
|
||||
Zones: <60°C green · 60–75°C amber · >75°C red
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- ╔═══════════════════ MOTOR CURRENT ═══════════════════╗ -->
|
||||
<div class="card">
|
||||
<div class="card-title">MOTOR CURRENT & CMD</div>
|
||||
<div class="motor-grid">
|
||||
<div class="motor-box">
|
||||
<div class="motor-label">LEFT WHEEL</div>
|
||||
<div style="font-size:20px;font-weight:bold;font-family:monospace" id="motor-cur-l">—</div>
|
||||
<div class="gauge-track" style="margin-top:4px">
|
||||
<div class="gauge-fill" id="motor-bar-l" style="width:0%"></div>
|
||||
</div>
|
||||
<div style="font-size:9px;color:#4b5563;margin-top:4px">
|
||||
CMD: <span id="motor-cmd-l" style="color:#6b7280">—</span>
|
||||
</div>
|
||||
</div>
|
||||
<div class="motor-box">
|
||||
<div class="motor-label">RIGHT WHEEL</div>
|
||||
<div style="font-size:20px;font-weight:bold;font-family:monospace" id="motor-cur-r">—</div>
|
||||
<div class="gauge-track" style="margin-top:4px">
|
||||
<div class="gauge-fill" id="motor-bar-r" style="width:0%"></div>
|
||||
</div>
|
||||
<div style="font-size:9px;color:#4b5563;margin-top:4px">
|
||||
CMD: <span id="motor-cmd-r" style="color:#6b7280">—</span>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div style="font-size:10px;color:#6b7280">
|
||||
Balance state: <span id="balance-state" style="color:#9ca3af;font-family:monospace">—</span>
|
||||
</div>
|
||||
<div style="font-size:9px;color:#374151">Thresholds: warn 8A · crit 12A</div>
|
||||
</div>
|
||||
|
||||
<!-- ╔═══════════════════ MEMORY / DISK ═══════════════════╗ -->
|
||||
<div class="card">
|
||||
<div class="card-title">RESOURCES</div>
|
||||
<div class="gauge-row">
|
||||
<div class="gauge-label-row">
|
||||
<span class="gauge-label">RAM</span>
|
||||
<span class="gauge-value" id="ram-val" style="color:#9ca3af">—</span>
|
||||
</div>
|
||||
<div class="gauge-track"><div class="gauge-fill" id="ram-bar" style="width:0%"></div></div>
|
||||
</div>
|
||||
<div class="gauge-row">
|
||||
<div class="gauge-label-row">
|
||||
<span class="gauge-label">GPU Memory</span>
|
||||
<span class="gauge-value" id="gpu-val" style="color:#9ca3af">—</span>
|
||||
</div>
|
||||
<div class="gauge-track"><div class="gauge-fill" id="gpu-bar" style="width:0%"></div></div>
|
||||
</div>
|
||||
<div class="gauge-row">
|
||||
<div class="gauge-label-row">
|
||||
<span class="gauge-label">Disk</span>
|
||||
<span class="gauge-value" id="disk-val" style="color:#9ca3af">—</span>
|
||||
</div>
|
||||
<div class="gauge-track"><div class="gauge-fill" id="disk-bar" style="width:0%"></div></div>
|
||||
</div>
|
||||
<div style="font-size:9px;color:#374151">Warn ≥80% · Critical ≥95%</div>
|
||||
</div>
|
||||
|
||||
<!-- ╔═══════════════════ WIFI / LATENCY ═══════════════════╗ -->
|
||||
<div class="card">
|
||||
<div class="card-title">NETWORK</div>
|
||||
<div style="display:flex;align-items:center;gap:12px;flex-wrap:wrap">
|
||||
<div id="rssi-bars" class="rssi-bars"></div>
|
||||
<div>
|
||||
<div style="font-size:9px;color:#6b7280">RSSI</div>
|
||||
<div style="font-size:20px;font-weight:bold;font-family:monospace" id="rssi-val">—</div>
|
||||
<div style="font-size:9px" id="rssi-label" style="color:#6b7280">—</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="gauge-row">
|
||||
<div class="gauge-label-row">
|
||||
<span class="gauge-label">Rosbridge Latency</span>
|
||||
<span class="gauge-value" id="latency-val" style="color:#9ca3af">—</span>
|
||||
</div>
|
||||
</div>
|
||||
<!-- MQTT status -->
|
||||
<div style="margin-top:4px;padding-top:8px;border-top:1px solid #0c2a3a">
|
||||
<div style="font-size:9px;color:#6b7280;margin-bottom:4px">MQTT BROKER</div>
|
||||
<div class="mqtt-status">
|
||||
<div class="mqtt-dot" id="mqtt-dot"></div>
|
||||
<span id="mqtt-label" style="color:#6b7280">No data</span>
|
||||
</div>
|
||||
<div style="font-size:9px;color:#374151;margin-top:4px">
|
||||
Via /diagnostics KeyValue: mqtt_connected
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- ╔═══════════════════ ROS2 NODE HEALTH ═══════════════════╗ -->
|
||||
<div class="card span3">
|
||||
<div class="card-title">
|
||||
ROS2 NODE HEALTH — /diagnostics
|
||||
<span style="font-size:9px;color:#4b5563" id="node-count">0 nodes</span>
|
||||
</div>
|
||||
<div id="node-list">
|
||||
<div style="color:#374151;font-size:10px;text-align:center;padding:12px">
|
||||
Waiting for /diagnostics data…
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
</div>
|
||||
|
||||
<!-- ── Footer ── -->
|
||||
<div id="footer">
|
||||
<span>rosbridge: <code id="footer-ws">ws://localhost:9090</code></span>
|
||||
<span>diagnostics dashboard — issue #562 · auto 2Hz</span>
|
||||
</div>
|
||||
|
||||
<script src="diagnostics_panel.js"></script>
|
||||
<script>
|
||||
// Sync footer WS URL
|
||||
document.getElementById('ws-input').addEventListener('input', (e) => {
|
||||
document.getElementById('footer-ws').textContent = e.target.value;
|
||||
});
|
||||
|
||||
// Keep node count updated after render
|
||||
const origRenderNodes = window.renderNodes;
|
||||
const nodeCountEl = document.getElementById('node-count');
|
||||
const _origOnDiag = window.onDiagnostics;
|
||||
|
||||
// Node count updates via MutationObserver on the node-list div
|
||||
const nl = document.getElementById('node-list');
|
||||
if (nl) {
|
||||
new MutationObserver(() => {
|
||||
const rows = nl.querySelectorAll('.node-row');
|
||||
if (nodeCountEl) nodeCountEl.textContent = rows.length + ' node' + (rows.length !== 1 ? 's' : '');
|
||||
}).observe(nl, { childList: true, subtree: false });
|
||||
}
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
@ -1,592 +0,0 @@
|
||||
/**
|
||||
* diagnostics_panel.js — Saltybot System Diagnostics Dashboard (Issue #562)
|
||||
*
|
||||
* Connects to rosbridge WebSocket and subscribes to:
|
||||
* /diagnostics diagnostic_msgs/DiagnosticArray — everything
|
||||
* /saltybot/balance_state std_msgs/String (JSON) — motor cmd / state
|
||||
*
|
||||
* Diagnostic KeyValues decoded:
|
||||
* battery_voltage_v, battery_current_a, battery_soc_pct
|
||||
* cpu_temp_c, gpu_temp_c, board_temp_c
|
||||
* motor_temp_l_c, motor_temp_r_c
|
||||
* motor_current_l_a, motor_current_r_a
|
||||
* ram_used_mb, ram_total_mb
|
||||
* gpu_used_mb, gpu_total_mb
|
||||
* disk_used_gb, disk_total_gb
|
||||
* wifi_rssi_dbm, wifi_latency_ms
|
||||
* mqtt_connected
|
||||
*
|
||||
* Auto-refresh: rosbridge subscriptions push at ~2 Hz; UI polls canvas draws
|
||||
* at the same rate and updates DOM elements.
|
||||
*/
|
||||
|
||||
'use strict';
|
||||
|
||||
// ── Constants ─────────────────────────────────────────────────────────────────
|
||||
|
||||
const LIPO_MIN = 12.0; // 4S empty
|
||||
const LIPO_MAX = 16.8; // 4S full
|
||||
|
||||
const BATT_HISTORY_MAX = 120; // ~2 min at 1 Hz
|
||||
|
||||
// Alert thresholds
|
||||
const THRESHOLDS = {
|
||||
voltage_warn: 13.6,
|
||||
voltage_crit: 13.0,
|
||||
soc_warn: 25,
|
||||
soc_crit: 10,
|
||||
cpu_temp_warn: 75,
|
||||
cpu_temp_crit: 90,
|
||||
gpu_temp_warn: 75,
|
||||
gpu_temp_crit: 90,
|
||||
motor_temp_warn: 70,
|
||||
motor_temp_crit: 85,
|
||||
ram_pct_warn: 80,
|
||||
ram_pct_crit: 95,
|
||||
disk_pct_warn: 80,
|
||||
disk_pct_crit: 95,
|
||||
rssi_warn: -70,
|
||||
rssi_crit: -80,
|
||||
latency_warn: 150,
|
||||
latency_crit: 500,
|
||||
current_warn: 8.0,
|
||||
current_crit: 12.0,
|
||||
};
|
||||
|
||||
// ── State ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
let ros = null;
|
||||
let diagSub = null;
|
||||
let balanceSub = null;
|
||||
|
||||
const state = {
|
||||
// Battery
|
||||
voltage: null,
|
||||
current: null,
|
||||
soc: null,
|
||||
battHistory: [], // [{ts, v}]
|
||||
|
||||
// Temperatures
|
||||
cpuTemp: null,
|
||||
gpuTemp: null,
|
||||
boardTemp: null,
|
||||
motorTempL: null,
|
||||
motorTempR: null,
|
||||
|
||||
// Motor
|
||||
motorCurrentL: null,
|
||||
motorCurrentR: null,
|
||||
motorCmdL: null,
|
||||
motorCmdR: null,
|
||||
balanceState: 'UNKNOWN',
|
||||
|
||||
// Resources
|
||||
ramUsed: null, ramTotal: null,
|
||||
gpuUsed: null, gpuTotal: null,
|
||||
diskUsed: null, diskTotal: null,
|
||||
|
||||
// Network
|
||||
rssi: null,
|
||||
latency: null,
|
||||
|
||||
// MQTT
|
||||
mqttConnected: null,
|
||||
|
||||
// ROS nodes (DiagnosticStatus array)
|
||||
nodes: [],
|
||||
|
||||
// Overall health
|
||||
overallLevel: 3, // 3=STALE by default
|
||||
lastUpdate: null,
|
||||
};
|
||||
|
||||
// ── Utility ───────────────────────────────────────────────────────────────────
|
||||
|
||||
function numOrNull(s) {
|
||||
const n = parseFloat(s);
|
||||
return isNaN(n) ? null : n;
|
||||
}
|
||||
|
||||
function pct(used, total) {
|
||||
if (!total || total === 0) return 0;
|
||||
return Math.min(100, Math.max(0, (used / total) * 100));
|
||||
}
|
||||
|
||||
function socFromVoltage(v) {
|
||||
if (v == null || v <= 0) return null;
|
||||
return Math.max(0, Math.min(100, ((v - LIPO_MIN) / (LIPO_MAX - LIPO_MIN)) * 100));
|
||||
}
|
||||
|
||||
function threshColor(value, warnThr, critThr, invert = false) {
|
||||
// invert: lower is worse (e.g. voltage, SOC, RSSI)
|
||||
if (value == null) return '#6b7280';
|
||||
const warn = invert ? value <= warnThr : value >= warnThr;
|
||||
const crit = invert ? value <= critThr : value >= critThr;
|
||||
if (crit) return '#ef4444';
|
||||
if (warn) return '#f59e0b';
|
||||
return '#22c55e';
|
||||
}
|
||||
|
||||
function levelClass(level) {
|
||||
return ['ns-ok', 'ns-warn', 'ns-error', 'ns-stale'][level] ?? 'ns-stale';
|
||||
}
|
||||
|
||||
function levelLabel(level) {
|
||||
return ['OK', 'WARN', 'ERROR', 'STALE'][level] ?? 'STALE';
|
||||
}
|
||||
|
||||
function setBadgeClass(el, level) {
|
||||
el.className = 'sys-badge ' + ['badge-ok','badge-warn','badge-error','badge-stale'][level ?? 3];
|
||||
}
|
||||
|
||||
// ── Connection ────────────────────────────────────────────────────────────────
|
||||
|
||||
function connect() {
|
||||
const url = document.getElementById('ws-input').value.trim();
|
||||
if (!url) return;
|
||||
if (ros) ros.close();
|
||||
|
||||
ros = new ROSLIB.Ros({ url });
|
||||
|
||||
ros.on('connection', () => {
|
||||
document.getElementById('conn-dot').className = 'connected';
|
||||
document.getElementById('conn-label').textContent = url;
|
||||
setupSubscriptions();
|
||||
});
|
||||
|
||||
ros.on('error', (err) => {
|
||||
document.getElementById('conn-dot').className = 'error';
|
||||
document.getElementById('conn-label').textContent = 'ERROR: ' + (err?.message || err);
|
||||
teardown();
|
||||
});
|
||||
|
||||
ros.on('close', () => {
|
||||
document.getElementById('conn-dot').className = '';
|
||||
document.getElementById('conn-label').textContent = 'Disconnected';
|
||||
teardown();
|
||||
});
|
||||
}
|
||||
|
||||
function setupSubscriptions() {
|
||||
// /diagnostics — throttle 500ms → ~2 Hz
|
||||
diagSub = new ROSLIB.Topic({
|
||||
ros, name: '/diagnostics',
|
||||
messageType: 'diagnostic_msgs/DiagnosticArray',
|
||||
throttle_rate: 500,
|
||||
});
|
||||
diagSub.subscribe(onDiagnostics);
|
||||
|
||||
// /saltybot/balance_state
|
||||
balanceSub = new ROSLIB.Topic({
|
||||
ros, name: '/saltybot/balance_state',
|
||||
messageType: 'std_msgs/String',
|
||||
throttle_rate: 500,
|
||||
});
|
||||
balanceSub.subscribe(onBalanceState);
|
||||
}
|
||||
|
||||
function teardown() {
|
||||
if (diagSub) { diagSub.unsubscribe(); diagSub = null; }
|
||||
if (balanceSub) { balanceSub.unsubscribe(); balanceSub = null; }
|
||||
state.overallLevel = 3;
|
||||
state.lastUpdate = null;
|
||||
render();
|
||||
}
|
||||
|
||||
// ── Message handlers ──────────────────────────────────────────────────────────
|
||||
|
||||
function onDiagnostics(msg) {
|
||||
const statuses = msg.status ?? [];
|
||||
state.nodes = [];
|
||||
let overallLevel = 0;
|
||||
|
||||
for (const status of statuses) {
|
||||
const kv = {};
|
||||
for (const { key, value } of (status.values ?? [])) {
|
||||
kv[key] = value;
|
||||
}
|
||||
|
||||
// Battery
|
||||
if ('battery_voltage_v' in kv) state.voltage = numOrNull(kv.battery_voltage_v);
|
||||
if ('battery_current_a' in kv) state.current = numOrNull(kv.battery_current_a);
|
||||
if ('battery_soc_pct' in kv) state.soc = numOrNull(kv.battery_soc_pct);
|
||||
|
||||
// Temperatures
|
||||
if ('cpu_temp_c' in kv) state.cpuTemp = numOrNull(kv.cpu_temp_c);
|
||||
if ('gpu_temp_c' in kv) state.gpuTemp = numOrNull(kv.gpu_temp_c);
|
||||
if ('board_temp_c' in kv) state.boardTemp = numOrNull(kv.board_temp_c);
|
||||
if ('motor_temp_l_c' in kv) state.motorTempL = numOrNull(kv.motor_temp_l_c);
|
||||
if ('motor_temp_r_c' in kv) state.motorTempR = numOrNull(kv.motor_temp_r_c);
|
||||
|
||||
// Motor current
|
||||
if ('motor_current_l_a' in kv) state.motorCurrentL = numOrNull(kv.motor_current_l_a);
|
||||
if ('motor_current_r_a' in kv) state.motorCurrentR = numOrNull(kv.motor_current_r_a);
|
||||
|
||||
// Resources
|
||||
if ('ram_used_mb' in kv) state.ramUsed = numOrNull(kv.ram_used_mb);
|
||||
if ('ram_total_mb' in kv) state.ramTotal = numOrNull(kv.ram_total_mb);
|
||||
if ('gpu_used_mb' in kv) state.gpuUsed = numOrNull(kv.gpu_used_mb);
|
||||
if ('gpu_total_mb' in kv) state.gpuTotal = numOrNull(kv.gpu_total_mb);
|
||||
if ('disk_used_gb' in kv) state.diskUsed = numOrNull(kv.disk_used_gb);
|
||||
if ('disk_total_gb' in kv) state.diskTotal = numOrNull(kv.disk_total_gb);
|
||||
|
||||
// Network
|
||||
if ('wifi_rssi_dbm' in kv) state.rssi = numOrNull(kv.wifi_rssi_dbm);
|
||||
if ('wifi_latency_ms' in kv) state.latency = numOrNull(kv.wifi_latency_ms);
|
||||
|
||||
// MQTT
|
||||
if ('mqtt_connected' in kv) state.mqttConnected = kv.mqtt_connected === 'true';
|
||||
|
||||
// Node health
|
||||
state.nodes.push({
|
||||
name: status.name || status.hardware_id || 'unknown',
|
||||
level: status.level ?? 3,
|
||||
message: status.message || '',
|
||||
});
|
||||
|
||||
overallLevel = Math.max(overallLevel, status.level ?? 0);
|
||||
}
|
||||
|
||||
state.overallLevel = overallLevel;
|
||||
state.lastUpdate = new Date();
|
||||
|
||||
// Battery history
|
||||
if (state.voltage != null) {
|
||||
state.battHistory.push({ ts: Date.now(), v: state.voltage });
|
||||
if (state.battHistory.length > BATT_HISTORY_MAX) {
|
||||
state.battHistory.shift();
|
||||
}
|
||||
}
|
||||
|
||||
// Derive SOC from voltage if not provided
|
||||
if (state.soc == null && state.voltage != null) {
|
||||
state.soc = socFromVoltage(state.voltage);
|
||||
}
|
||||
|
||||
render();
|
||||
flashRefreshDot();
|
||||
}
|
||||
|
||||
function onBalanceState(msg) {
|
||||
try {
|
||||
const data = JSON.parse(msg.data);
|
||||
state.balanceState = data.state ?? 'UNKNOWN';
|
||||
state.motorCmdL = data.motor_cmd ?? null;
|
||||
state.motorCmdR = data.motor_cmd ?? null; // single motor_cmd field
|
||||
} catch (_) {}
|
||||
}
|
||||
|
||||
// ── Refresh indicator ─────────────────────────────────────────────────────────
|
||||
|
||||
function flashRefreshDot() {
|
||||
const dot = document.getElementById('refresh-dot');
|
||||
dot.classList.add('pulse');
|
||||
setTimeout(() => dot.classList.remove('pulse'), 400);
|
||||
}
|
||||
|
||||
// ── Sparkline canvas ──────────────────────────────────────────────────────────
|
||||
|
||||
function drawSparkline() {
|
||||
const canvas = document.getElementById('batt-sparkline');
|
||||
if (!canvas) return;
|
||||
const ctx = canvas.getContext('2d');
|
||||
const W = canvas.width = canvas.offsetWidth || 300;
|
||||
const H = canvas.height;
|
||||
|
||||
ctx.clearRect(0, 0, W, H);
|
||||
ctx.fillStyle = '#0a0a1a';
|
||||
ctx.fillRect(0, 0, W, H);
|
||||
|
||||
const data = state.battHistory;
|
||||
if (data.length < 2) {
|
||||
ctx.fillStyle = '#374151';
|
||||
ctx.font = '10px Courier New';
|
||||
ctx.textAlign = 'center';
|
||||
ctx.fillText('Awaiting battery data…', W / 2, H / 2 + 4);
|
||||
return;
|
||||
}
|
||||
|
||||
const vMin = LIPO_MIN - 0.2;
|
||||
const vMax = LIPO_MAX + 0.2;
|
||||
|
||||
// Grid lines
|
||||
ctx.strokeStyle = '#1e3a5f';
|
||||
ctx.lineWidth = 0.5;
|
||||
[0.25, 0.5, 0.75].forEach((f) => {
|
||||
const y = H - f * H;
|
||||
ctx.beginPath(); ctx.moveTo(0, y); ctx.lineTo(W, y); ctx.stroke();
|
||||
});
|
||||
|
||||
// Plot
|
||||
ctx.lineWidth = 1.5;
|
||||
ctx.beginPath();
|
||||
data.forEach((pt, i) => {
|
||||
const x = (i / (data.length - 1)) * W;
|
||||
const y = H - ((pt.v - vMin) / (vMax - vMin)) * H;
|
||||
i === 0 ? ctx.moveTo(x, y) : ctx.lineTo(x, y);
|
||||
});
|
||||
|
||||
const lastV = data[data.length - 1].v;
|
||||
ctx.strokeStyle = threshColor(lastV, THRESHOLDS.voltage_warn, THRESHOLDS.voltage_crit, true);
|
||||
ctx.stroke();
|
||||
|
||||
// Fill under
|
||||
ctx.lineTo(W, H); ctx.lineTo(0, H); ctx.closePath();
|
||||
ctx.fillStyle = 'rgba(6,182,212,0.06)';
|
||||
ctx.fill();
|
||||
|
||||
// Labels
|
||||
ctx.fillStyle = '#374151';
|
||||
ctx.font = '9px Courier New';
|
||||
ctx.textAlign = 'left';
|
||||
ctx.fillText(`${LIPO_MAX}V`, 2, 10);
|
||||
ctx.fillText(`${LIPO_MIN}V`, 2, H - 2);
|
||||
}
|
||||
|
||||
// ── Gauge helpers ─────────────────────────────────────────────────────────────
|
||||
|
||||
function setGauge(barId, pctVal, color) {
|
||||
const el = document.getElementById(barId);
|
||||
if (!el) return;
|
||||
el.style.width = `${Math.max(0, Math.min(100, pctVal))}%`;
|
||||
el.style.background = color;
|
||||
}
|
||||
|
||||
function setText(id, text) {
|
||||
const el = document.getElementById(id);
|
||||
if (el) el.textContent = text ?? '—';
|
||||
}
|
||||
|
||||
function setColor(id, color) {
|
||||
const el = document.getElementById(id);
|
||||
if (el) el.style.color = color;
|
||||
}
|
||||
|
||||
function setTempBox(id, tempC, warnT, critT) {
|
||||
const box = document.getElementById(id + '-box');
|
||||
const val = document.getElementById(id + '-val');
|
||||
const bar = document.getElementById(id + '-bar');
|
||||
const color = threshColor(tempC, warnT, critT);
|
||||
|
||||
if (val) { val.textContent = tempC != null ? tempC.toFixed(0) + '°C' : '—'; val.style.color = color; }
|
||||
if (bar) {
|
||||
const p = tempC != null ? Math.min(100, (tempC / 100) * 100) : 0;
|
||||
bar.style.width = p + '%';
|
||||
bar.style.background = color;
|
||||
}
|
||||
if (box && tempC != null) {
|
||||
box.style.borderColor = tempC >= critT ? '#991b1b' : tempC >= warnT ? '#92400e' : '#1e3a5f';
|
||||
}
|
||||
}
|
||||
|
||||
// ── RSSI bars ─────────────────────────────────────────────────────────────────
|
||||
|
||||
function rssiToLevel(dBm) {
|
||||
if (dBm == null) return { label: 'N/A', color: '#374151', bars: 0 };
|
||||
if (dBm >= -50) return { label: 'Excellent', color: '#22c55e', bars: 5 };
|
||||
if (dBm >= -60) return { label: 'Good', color: '#3b82f6', bars: 4 };
|
||||
if (dBm >= -70) return { label: 'Fair', color: '#f59e0b', bars: 3 };
|
||||
if (dBm >= -80) return { label: 'Weak', color: '#ef4444', bars: 2 };
|
||||
return { label: 'Poor', color: '#7f1d1d', bars: 1 };
|
||||
}
|
||||
|
||||
function drawRssiBars() {
|
||||
const container = document.getElementById('rssi-bars');
|
||||
if (!container) return;
|
||||
const lv = rssiToLevel(state.rssi);
|
||||
const BAR_H = [4, 8, 12, 16, 20];
|
||||
|
||||
container.innerHTML = '';
|
||||
for (let i = 0; i < 5; i++) {
|
||||
const bar = document.createElement('div');
|
||||
bar.className = 'rssi-bar';
|
||||
bar.style.height = BAR_H[i] + 'px';
|
||||
bar.style.width = '7px';
|
||||
bar.style.background = i < lv.bars ? lv.color : '#1f2937';
|
||||
container.appendChild(bar);
|
||||
}
|
||||
}
|
||||
|
||||
// ── Node list renderer ────────────────────────────────────────────────────────
|
||||
|
||||
function renderNodes() {
|
||||
const container = document.getElementById('node-list');
|
||||
if (!container) return;
|
||||
|
||||
if (state.nodes.length === 0) {
|
||||
container.innerHTML = '<div style="color:#374151;font-size:10px;text-align:center;padding:8px">No /diagnostics data yet</div>';
|
||||
return;
|
||||
}
|
||||
|
||||
container.innerHTML = state.nodes.map((n) => `
|
||||
<div class="node-row">
|
||||
<span class="node-name">${n.name}</span>
|
||||
<div style="display:flex;align-items:center;gap:6px">
|
||||
${n.message ? `<span style="color:#4b5563;font-size:9px;max-width:120px;overflow:hidden;text-overflow:ellipsis;white-space:nowrap" title="${n.message}">${n.message}</span>` : ''}
|
||||
<span class="node-status ${levelClass(n.level)}">${levelLabel(n.level)}</span>
|
||||
</div>
|
||||
</div>
|
||||
`).join('');
|
||||
}
|
||||
|
||||
// ── Overall status bar ────────────────────────────────────────────────────────
|
||||
|
||||
function updateStatusBar() {
|
||||
const lvl = state.overallLevel;
|
||||
const badge = document.getElementById('system-badge');
|
||||
const labels = ['HEALTHY', 'DEGRADED', 'FAULT', 'STALE'];
|
||||
if (badge) {
|
||||
badge.textContent = labels[lvl] ?? 'STALE';
|
||||
setBadgeClass(badge, lvl);
|
||||
}
|
||||
|
||||
// Individual badges
|
||||
updateBattBadge();
|
||||
updateTempBadge();
|
||||
updateNetBadge();
|
||||
|
||||
const upd = document.getElementById('last-update');
|
||||
if (upd && state.lastUpdate) {
|
||||
upd.textContent = 'Updated ' + state.lastUpdate.toLocaleTimeString();
|
||||
}
|
||||
}
|
||||
|
||||
function updateBattBadge() {
|
||||
const el = document.getElementById('batt-badge');
|
||||
if (!el) return;
|
||||
const v = state.voltage;
|
||||
if (v == null) { el.textContent = 'NO DATA'; setBadgeClass(el, 3); return; }
|
||||
if (v <= THRESHOLDS.voltage_crit) { el.textContent = 'CRITICAL'; setBadgeClass(el, 2); }
|
||||
else if (v <= THRESHOLDS.voltage_warn) { el.textContent = 'LOW'; setBadgeClass(el, 1); }
|
||||
else { el.textContent = 'OK'; setBadgeClass(el, 0); }
|
||||
}
|
||||
|
||||
function updateTempBadge() {
|
||||
const el = document.getElementById('temp-badge');
|
||||
if (!el) return;
|
||||
const temps = [state.cpuTemp, state.gpuTemp, state.motorTempL, state.motorTempR].filter(t => t != null);
|
||||
if (temps.length === 0) { el.textContent = 'NO DATA'; setBadgeClass(el, 3); return; }
|
||||
const max = Math.max(...temps);
|
||||
if (max >= THRESHOLDS.cpu_temp_crit) { el.textContent = 'CRITICAL'; setBadgeClass(el, 2); }
|
||||
else if (max >= THRESHOLDS.cpu_temp_warn) { el.textContent = 'HOT'; setBadgeClass(el, 1); }
|
||||
else { el.textContent = 'OK'; setBadgeClass(el, 0); }
|
||||
}
|
||||
|
||||
function updateNetBadge() {
|
||||
const el = document.getElementById('net-badge');
|
||||
if (!el) return;
|
||||
const r = state.rssi;
|
||||
if (r == null) { el.textContent = 'NO DATA'; setBadgeClass(el, 3); return; }
|
||||
if (r <= THRESHOLDS.rssi_crit) { el.textContent = 'POOR'; setBadgeClass(el, 2); }
|
||||
else if (r <= THRESHOLDS.rssi_warn) { el.textContent = 'WEAK'; setBadgeClass(el, 1); }
|
||||
else { el.textContent = 'OK'; setBadgeClass(el, 0); }
|
||||
}
|
||||
|
||||
// ── Main render ───────────────────────────────────────────────────────────────
|
||||
|
||||
function render() {
|
||||
// ── Battery ──
|
||||
const v = state.voltage;
|
||||
const soc = state.soc ?? socFromVoltage(v);
|
||||
const vColor = threshColor(v, THRESHOLDS.voltage_warn, THRESHOLDS.voltage_crit, true);
|
||||
const sColor = threshColor(soc, THRESHOLDS.soc_warn, THRESHOLDS.soc_crit, true);
|
||||
|
||||
setText('batt-voltage', v != null ? v.toFixed(2) + ' V' : '—');
|
||||
setColor('batt-voltage', vColor);
|
||||
setText('batt-soc', soc != null ? soc.toFixed(0) + ' %' : '—');
|
||||
setColor('batt-soc', sColor);
|
||||
setText('batt-current', state.current != null ? state.current.toFixed(2) + ' A' : '—');
|
||||
|
||||
setGauge('batt-soc-bar', soc ?? 0, sColor);
|
||||
setGauge('batt-volt-bar', v != null ? ((v - LIPO_MIN) / (LIPO_MAX - LIPO_MIN)) * 100 : 0, vColor);
|
||||
|
||||
drawSparkline();
|
||||
|
||||
// ── Temperatures ──
|
||||
setTempBox('cpu-temp', state.cpuTemp, THRESHOLDS.cpu_temp_warn, THRESHOLDS.cpu_temp_crit);
|
||||
setTempBox('gpu-temp', state.gpuTemp, THRESHOLDS.gpu_temp_warn, THRESHOLDS.gpu_temp_crit);
|
||||
setTempBox('board-temp', state.boardTemp, 60, 80);
|
||||
setTempBox('motor-temp-l', state.motorTempL, THRESHOLDS.motor_temp_warn, THRESHOLDS.motor_temp_crit);
|
||||
setTempBox('motor-temp-r', state.motorTempR, THRESHOLDS.motor_temp_warn, THRESHOLDS.motor_temp_crit);
|
||||
|
||||
// ── Motor current ──
|
||||
const curL = state.motorCurrentL;
|
||||
const curR = state.motorCurrentR;
|
||||
const curColorL = threshColor(curL, THRESHOLDS.current_warn, THRESHOLDS.current_crit);
|
||||
const curColorR = threshColor(curR, THRESHOLDS.current_warn, THRESHOLDS.current_crit);
|
||||
|
||||
setText('motor-cur-l', curL != null ? curL.toFixed(2) + ' A' : '—');
|
||||
setColor('motor-cur-l', curColorL);
|
||||
setText('motor-cur-r', curR != null ? curR.toFixed(2) + ' A' : '—');
|
||||
setColor('motor-cur-r', curColorR);
|
||||
setGauge('motor-bar-l', curL != null ? (curL / THRESHOLDS.current_crit) * 100 : 0, curColorL);
|
||||
setGauge('motor-bar-r', curR != null ? (curR / THRESHOLDS.current_crit) * 100 : 0, curColorR);
|
||||
setText('motor-cmd-l', state.motorCmdL != null ? state.motorCmdL.toString() : '—');
|
||||
setText('motor-cmd-r', state.motorCmdR != null ? state.motorCmdR.toString() : '—');
|
||||
setText('balance-state', state.balanceState);
|
||||
|
||||
// ── Resources ──
|
||||
const ramPct = pct(state.ramUsed, state.ramTotal);
|
||||
const gpuPct = pct(state.gpuUsed, state.gpuTotal);
|
||||
const diskPct = pct(state.diskUsed, state.diskTotal);
|
||||
|
||||
setText('ram-val',
|
||||
state.ramUsed != null ? `${state.ramUsed.toFixed(0)} / ${state.ramTotal?.toFixed(0) ?? '?'} MB` : '—');
|
||||
setGauge('ram-bar', ramPct, threshColor(ramPct, THRESHOLDS.ram_pct_warn, THRESHOLDS.ram_pct_crit));
|
||||
|
||||
setText('gpu-val',
|
||||
state.gpuUsed != null ? `${state.gpuUsed.toFixed(0)} / ${state.gpuTotal?.toFixed(0) ?? '?'} MB` : '—');
|
||||
setGauge('gpu-bar', gpuPct, threshColor(gpuPct, 70, 90));
|
||||
|
||||
setText('disk-val',
|
||||
state.diskUsed != null ? `${state.diskUsed.toFixed(1)} / ${state.diskTotal?.toFixed(1) ?? '?'} GB` : '—');
|
||||
setGauge('disk-bar', diskPct, threshColor(diskPct, THRESHOLDS.disk_pct_warn, THRESHOLDS.disk_pct_crit));
|
||||
|
||||
// ── WiFi ──
|
||||
const rLevel = rssiToLevel(state.rssi);
|
||||
setText('rssi-val', state.rssi != null ? state.rssi + ' dBm' : '—');
|
||||
setColor('rssi-val', rLevel.color);
|
||||
setText('rssi-label', rLevel.label);
|
||||
setColor('rssi-label', rLevel.color);
|
||||
setText('latency-val', state.latency != null ? state.latency.toFixed(0) + ' ms' : '—');
|
||||
setColor('latency-val', threshColor(state.latency, THRESHOLDS.latency_warn, THRESHOLDS.latency_crit));
|
||||
drawRssiBars();
|
||||
|
||||
// ── MQTT ──
|
||||
const mqttDot = document.getElementById('mqtt-dot');
|
||||
const mqttLbl = document.getElementById('mqtt-label');
|
||||
if (mqttDot) {
|
||||
mqttDot.className = 'mqtt-dot ' + (state.mqttConnected ? 'connected' : 'disconnected');
|
||||
}
|
||||
if (mqttLbl) {
|
||||
mqttLbl.textContent = state.mqttConnected === null ? 'No data'
|
||||
: state.mqttConnected ? 'Broker connected'
|
||||
: 'Broker disconnected';
|
||||
mqttLbl.style.color = state.mqttConnected ? '#4ade80' : '#f87171';
|
||||
}
|
||||
|
||||
// ── Nodes ──
|
||||
renderNodes();
|
||||
|
||||
// ── Status bar ──
|
||||
updateStatusBar();
|
||||
}
|
||||
|
||||
// ── Init ──────────────────────────────────────────────────────────────────────
|
||||
|
||||
document.getElementById('btn-connect').addEventListener('click', connect);
|
||||
document.getElementById('ws-input').addEventListener('keydown', (e) => {
|
||||
if (e.key === 'Enter') connect();
|
||||
});
|
||||
|
||||
const stored = localStorage.getItem('diag_ws_url');
|
||||
if (stored) document.getElementById('ws-input').value = stored;
|
||||
document.getElementById('ws-input').addEventListener('change', (e) => {
|
||||
localStorage.setItem('diag_ws_url', e.target.value);
|
||||
});
|
||||
|
||||
// Initial render (blank state)
|
||||
render();
|
||||
|
||||
// Periodic sparkline resize + redraw on window resize
|
||||
window.addEventListener('resize', drawSparkline);
|
||||
@ -1,210 +0,0 @@
|
||||
/* gimbal_panel.css — Saltybot Gimbal Control Panel (Issue #551) */
|
||||
|
||||
*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
|
||||
|
||||
body {
|
||||
font-family: 'Courier New', Courier, monospace;
|
||||
font-size: 12px;
|
||||
background: #050510;
|
||||
color: #d1d5db;
|
||||
height: 100dvh;
|
||||
overflow: hidden;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
}
|
||||
|
||||
/* ── Header ── */
|
||||
#header {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
padding: 6px 16px;
|
||||
background: #070712;
|
||||
border-bottom: 1px solid #083344;
|
||||
flex-shrink: 0;
|
||||
flex-wrap: wrap;
|
||||
gap: 6px;
|
||||
}
|
||||
#header .logo { color: #f97316; font-weight: bold; letter-spacing: 0.15em; font-size: 13px; }
|
||||
#conn-bar { display: flex; align-items: center; gap: 6px; }
|
||||
#conn-dot { width: 8px; height: 8px; border-radius: 50%; background: #374151; flex-shrink: 0; }
|
||||
#conn-dot.connected { background: #4ade80; }
|
||||
#conn-dot.error { background: #f87171; }
|
||||
#ws-input {
|
||||
background: #111827; border: 1px solid #1e3a5f; border-radius: 4px;
|
||||
color: #67e8f9; padding: 2px 8px; font-family: monospace; font-size: 11px; width: 200px;
|
||||
}
|
||||
#ws-input:focus { outline: none; border-color: #0891b2; }
|
||||
.btn {
|
||||
padding: 3px 10px; border-radius: 4px; border: 1px solid; cursor: pointer;
|
||||
font-family: monospace; font-size: 10px; font-weight: bold; letter-spacing: 0.05em;
|
||||
transition: background 0.15s;
|
||||
}
|
||||
.btn-cyan { background: #083344; border-color: #155e75; color: #67e8f9; }
|
||||
.btn-cyan:hover { background: #0e4f69; }
|
||||
.btn-green { background: #052e16; border-color: #166534; color: #4ade80; }
|
||||
.btn-green:hover { background: #0a4a24; }
|
||||
.btn-amber { background: #451a03; border-color: #92400e; color: #fcd34d; }
|
||||
.btn-amber:hover { background: #6b2b04; }
|
||||
.btn-red { background: #450a0a; border-color: #991b1b; color: #f87171; }
|
||||
.btn-red:hover { background: #6b1010; }
|
||||
.btn-red.active { background: #7f1d1d; border-color: #dc2626; color: #fca5a5; animation: pulse 1.5s infinite; }
|
||||
|
||||
@keyframes pulse {
|
||||
0%, 100% { opacity: 1; }
|
||||
50% { opacity: 0.65; }
|
||||
}
|
||||
|
||||
/* ── Main layout ── */
|
||||
#main {
|
||||
flex: 1;
|
||||
display: grid;
|
||||
grid-template-columns: 1fr 280px;
|
||||
gap: 12px;
|
||||
padding: 12px;
|
||||
min-height: 0;
|
||||
}
|
||||
|
||||
@media (max-width: 720px) {
|
||||
#main { grid-template-columns: 1fr; grid-template-rows: 1fr auto; }
|
||||
body { font-size: 11px; overflow-y: auto; height: auto; }
|
||||
}
|
||||
|
||||
/* ── Camera feed ── */
|
||||
#camera-section {
|
||||
display: flex; flex-direction: column; gap: 8px; min-height: 0;
|
||||
}
|
||||
#cam-toolbar {
|
||||
display: flex; gap: 6px; align-items: center; flex-shrink: 0; flex-wrap: wrap;
|
||||
}
|
||||
#cam-toolbar span { color: #6b7280; font-size: 10px; margin-left: auto; }
|
||||
#camera-frame {
|
||||
flex: 1;
|
||||
background: #000;
|
||||
border-radius: 8px;
|
||||
border: 1px solid #083344;
|
||||
display: flex; align-items: center; justify-content: center;
|
||||
position: relative;
|
||||
overflow: hidden;
|
||||
min-height: 160px;
|
||||
}
|
||||
#camera-img {
|
||||
max-width: 100%; max-height: 100%;
|
||||
object-fit: contain;
|
||||
display: none;
|
||||
}
|
||||
#camera-img.visible { display: block; }
|
||||
#no-signal {
|
||||
display: flex; flex-direction: column; align-items: center; gap: 6px;
|
||||
color: #374151; user-select: none;
|
||||
}
|
||||
#no-signal .icon { font-size: 36px; }
|
||||
#fps-badge {
|
||||
position: absolute; top: 8px; right: 8px;
|
||||
background: rgba(0,0,0,0.7); color: #4ade80;
|
||||
padding: 2px 6px; border-radius: 4px; font-size: 10px;
|
||||
display: none;
|
||||
}
|
||||
#fps-badge.visible { display: block; }
|
||||
|
||||
/* Angle overlay on camera */
|
||||
#angle-overlay {
|
||||
position: absolute; bottom: 8px; left: 8px;
|
||||
background: rgba(0,0,0,0.7);
|
||||
padding: 4px 8px; border-radius: 4px;
|
||||
color: #67e8f9; font-size: 10px; font-family: monospace;
|
||||
display: none;
|
||||
}
|
||||
#angle-overlay.visible { display: block; }
|
||||
|
||||
/* ── Controls sidebar ── */
|
||||
#controls {
|
||||
display: flex; flex-direction: column; gap: 10px; overflow-y: auto; min-height: 0;
|
||||
}
|
||||
|
||||
.card {
|
||||
background: #070712; border: 1px solid #083344;
|
||||
border-radius: 8px; padding: 10px;
|
||||
}
|
||||
.card-title {
|
||||
font-size: 9px; font-weight: bold; letter-spacing: 0.15em;
|
||||
color: #0891b2; margin-bottom: 8px; text-transform: uppercase;
|
||||
}
|
||||
|
||||
/* ── Pan/Tilt Joystick ── */
|
||||
#pad-wrap {
|
||||
display: flex; flex-direction: column; align-items: center; gap: 6px;
|
||||
}
|
||||
#gimbal-pad {
|
||||
cursor: crosshair;
|
||||
border-radius: 6px;
|
||||
border: 1px solid #1e3a5f;
|
||||
touch-action: none;
|
||||
user-select: none;
|
||||
}
|
||||
.pad-labels {
|
||||
display: grid; grid-template-columns: 1fr auto 1fr;
|
||||
width: 200px; font-size: 9px; color: #4b5563; text-align: center; align-items: center;
|
||||
}
|
||||
|
||||
/* ── Angle display ── */
|
||||
#angle-display {
|
||||
display: grid; grid-template-columns: 1fr 1fr; gap: 6px;
|
||||
}
|
||||
.angle-box {
|
||||
background: #0a0a1a; border: 1px solid #1e3a5f; border-radius: 6px;
|
||||
padding: 6px; display: flex; flex-direction: column; gap: 2px;
|
||||
}
|
||||
.angle-label { font-size: 9px; color: #6b7280; }
|
||||
.angle-value { font-size: 18px; color: #67e8f9; font-family: monospace; }
|
||||
.angle-unit { font-size: 9px; color: #374151; }
|
||||
|
||||
/* ── Presets ── */
|
||||
#presets {
|
||||
display: grid; grid-template-columns: 1fr 1fr 1fr; gap: 6px;
|
||||
}
|
||||
#presets button { padding: 6px 4px; text-align: center; line-height: 1.3; }
|
||||
#presets button .preset-name { display: block; font-size: 9px; font-weight: bold; letter-spacing: 0.05em; }
|
||||
#presets button .preset-val { display: block; font-size: 9px; color: #6b7280; }
|
||||
|
||||
/* Home button */
|
||||
#btn-home {
|
||||
width: 100%; padding: 8px;
|
||||
background: #1c1c2e; border: 1px solid #374151; border-radius: 6px;
|
||||
color: #9ca3af; font-family: monospace; font-size: 10px; font-weight: bold;
|
||||
letter-spacing: 0.1em; cursor: pointer; transition: background 0.15s;
|
||||
}
|
||||
#btn-home:hover { background: #2d2d44; color: #d1d5db; }
|
||||
|
||||
/* Tracking toggle */
|
||||
#tracking-row {
|
||||
display: flex; align-items: center; justify-content: space-between;
|
||||
}
|
||||
.toggle-label { font-size: 10px; color: #9ca3af; }
|
||||
.toggle-switch {
|
||||
position: relative; width: 40px; height: 20px;
|
||||
}
|
||||
.toggle-switch input { opacity: 0; width: 0; height: 0; }
|
||||
.toggle-slider {
|
||||
position: absolute; inset: 0;
|
||||
background: #1f2937; border-radius: 10px; cursor: pointer;
|
||||
transition: background 0.2s;
|
||||
}
|
||||
.toggle-slider::before {
|
||||
content: ''; position: absolute;
|
||||
width: 14px; height: 14px; border-radius: 50%;
|
||||
background: #6b7280; top: 3px; left: 3px;
|
||||
transition: transform 0.2s, background 0.2s;
|
||||
}
|
||||
.toggle-switch input:checked + .toggle-slider { background: #0c4a6e; }
|
||||
.toggle-switch input:checked + .toggle-slider::before {
|
||||
background: #38bdf8; transform: translateX(20px);
|
||||
}
|
||||
|
||||
/* ── Footer ── */
|
||||
#footer {
|
||||
background: #070712; border-top: 1px solid #083344;
|
||||
padding: 4px 16px;
|
||||
display: flex; align-items: center; justify-content: space-between;
|
||||
flex-shrink: 0; font-size: 10px; color: #374151;
|
||||
}
|
||||
@ -1,183 +0,0 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="en">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
|
||||
<title>Saltybot — Gimbal Control</title>
|
||||
<link rel="stylesheet" href="gimbal_panel.css">
|
||||
<!-- roslib from CDN -->
|
||||
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script>
|
||||
<style>
|
||||
/* Cam button active state (can't use CSS-only with JS-toggled class without Tailwind) */
|
||||
.cam-btn { padding: 3px 10px; border-radius: 4px; border: 1px solid #1e3a5f;
|
||||
background: #070712; color: #4b5563; font-family: monospace;
|
||||
font-size: 10px; font-weight: bold; cursor: pointer; transition: all 0.15s; }
|
||||
.cam-btn:hover { color: #d1d5db; }
|
||||
.cam-btn.active { background: #083344; border-color: #155e75; color: #67e8f9; }
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
|
||||
<!-- ── Header ── -->
|
||||
<div id="header">
|
||||
<div class="logo">⚡ SALTYBOT — GIMBAL</div>
|
||||
|
||||
<div id="conn-bar">
|
||||
<div id="conn-dot"></div>
|
||||
<input id="ws-input" type="text" value="ws://localhost:9090" placeholder="ws://robot-ip:9090" />
|
||||
<button id="btn-connect" class="btn btn-cyan">CONNECT</button>
|
||||
<span id="conn-status" style="color:#4b5563;font-size:10px;">Not connected</span>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- ── Main ── -->
|
||||
<div id="main">
|
||||
|
||||
<!-- ── Left: Camera feed ── -->
|
||||
<section id="camera-section">
|
||||
|
||||
<!-- Camera selector toolbar -->
|
||||
<div id="cam-toolbar">
|
||||
<button class="cam-btn active" data-cam="front">FRONT</button>
|
||||
<button class="cam-btn" data-cam="rear" >REAR</button>
|
||||
<button class="cam-btn" data-cam="left" >LEFT</button>
|
||||
<button class="cam-btn" data-cam="right">RIGHT</button>
|
||||
<span id="cam-topic">/camera/front/image_raw/compressed</span>
|
||||
</div>
|
||||
|
||||
<!-- Video frame -->
|
||||
<div id="camera-frame">
|
||||
<img id="camera-img" alt="gimbal camera feed" />
|
||||
<div id="no-signal">
|
||||
<div class="icon">📷</div>
|
||||
<div>NO SIGNAL</div>
|
||||
</div>
|
||||
<div id="fps-badge">— fps</div>
|
||||
<div id="angle-overlay">PAN 0.0° TILT 0.0°</div>
|
||||
</div>
|
||||
|
||||
</section>
|
||||
|
||||
<!-- ── Right: Controls ── -->
|
||||
<aside id="controls">
|
||||
|
||||
<!-- Pan/Tilt pad -->
|
||||
<div class="card">
|
||||
<div class="card-title">PAN / TILT CONTROL</div>
|
||||
<div id="pad-wrap">
|
||||
<div class="pad-labels">
|
||||
<span>← PAN</span>
|
||||
<span style="color:#155e75">DRAG</span>
|
||||
<span>PAN →</span>
|
||||
</div>
|
||||
<canvas id="gimbal-pad" width="200" height="150"></canvas>
|
||||
<div style="font-size:9px;color:#374151;text-align:center">
|
||||
↑ TILT UP | ↓ TILT DOWN
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Current angles -->
|
||||
<div class="card">
|
||||
<div class="card-title">CURRENT ANGLES (feedback)</div>
|
||||
<div id="angle-display">
|
||||
<div class="angle-box">
|
||||
<div class="angle-label">PAN</div>
|
||||
<div class="angle-value" id="pan-val">0.0</div>
|
||||
<div class="angle-unit">degrees</div>
|
||||
</div>
|
||||
<div class="angle-box">
|
||||
<div class="angle-label">TILT</div>
|
||||
<div class="angle-value" id="tilt-val">0.0</div>
|
||||
<div class="angle-unit">degrees</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Preset positions -->
|
||||
<div class="card">
|
||||
<div class="card-title">PRESET POSITIONS</div>
|
||||
<div id="presets">
|
||||
<button class="btn btn-cyan" data-preset="0">
|
||||
<span class="preset-name">CENTER</span>
|
||||
<span class="preset-val">0° / 0°</span>
|
||||
</button>
|
||||
<button class="btn btn-cyan" data-preset="1">
|
||||
<span class="preset-name">LEFT</span>
|
||||
<span class="preset-val">-45° / 0°</span>
|
||||
</button>
|
||||
<button class="btn btn-cyan" data-preset="2">
|
||||
<span class="preset-name">RIGHT</span>
|
||||
<span class="preset-val">+45° / 0°</span>
|
||||
</button>
|
||||
<button class="btn btn-cyan" data-preset="3">
|
||||
<span class="preset-name">UP</span>
|
||||
<span class="preset-val">0° / +45°</span>
|
||||
</button>
|
||||
<button class="btn btn-cyan" data-preset="4">
|
||||
<span class="preset-name">DOWN</span>
|
||||
<span class="preset-val">0° / -30°</span>
|
||||
</button>
|
||||
</div>
|
||||
<button id="btn-home" style="margin-top:8px">⌂ HOME (0° / 0°)</button>
|
||||
</div>
|
||||
|
||||
<!-- Person tracking -->
|
||||
<div class="card">
|
||||
<div class="card-title">PERSON TRACKING</div>
|
||||
<div id="tracking-row">
|
||||
<span class="toggle-label" id="tracking-label">OFF — manual control</span>
|
||||
<label class="toggle-switch">
|
||||
<input type="checkbox" id="tracking-toggle">
|
||||
<span class="toggle-slider"></span>
|
||||
</label>
|
||||
</div>
|
||||
<div style="font-size:9px;color:#4b5563;margin-top:6px">
|
||||
Publishes to <code>/gimbal/tracking_enabled</code>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Topic info -->
|
||||
<div class="card" style="font-size:9px;color:#4b5563;line-height:1.8">
|
||||
<div class="card-title">ROS TOPICS</div>
|
||||
<div>PUB <code style="color:#374151">/gimbal/cmd</code> → geometry_msgs/Vector3</div>
|
||||
<div>SUB <code style="color:#374151">/gimbal/state</code> → geometry_msgs/Vector3</div>
|
||||
<div>PUB <code style="color:#374151">/gimbal/tracking_enabled</code> → std_msgs/Bool</div>
|
||||
<div>SUB <code style="color:#374151">/camera/*/image_raw/compressed</code></div>
|
||||
</div>
|
||||
|
||||
</aside>
|
||||
</div>
|
||||
|
||||
<!-- ── Footer ── -->
|
||||
<div id="footer">
|
||||
<span>rosbridge: <code id="footer-ws">ws://localhost:9090</code></span>
|
||||
<span>gimbal control panel — issue #551</span>
|
||||
</div>
|
||||
|
||||
<script src="gimbal_panel.js"></script>
|
||||
<script>
|
||||
// Sync footer ws URL
|
||||
document.getElementById('ws-input').addEventListener('input', (e) => {
|
||||
document.getElementById('footer-ws').textContent = e.target.value;
|
||||
});
|
||||
|
||||
// Tracking label update
|
||||
document.getElementById('tracking-toggle').addEventListener('change', (e) => {
|
||||
document.getElementById('tracking-label').textContent =
|
||||
e.target.checked ? 'ON — following person' : 'OFF — manual control';
|
||||
});
|
||||
|
||||
// Camera topic label update
|
||||
document.querySelectorAll('.cam-btn').forEach((btn) => {
|
||||
btn.addEventListener('click', () => {
|
||||
const cam = btn.dataset.cam;
|
||||
document.getElementById('cam-topic').textContent =
|
||||
`/camera/${cam}/image_raw/compressed`;
|
||||
document.getElementById('footer-ws').textContent =
|
||||
document.getElementById('ws-input').value;
|
||||
});
|
||||
});
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
@ -1,392 +0,0 @@
|
||||
/**
|
||||
* gimbal_panel.js — Saltybot Gimbal Control Panel (Issue #551)
|
||||
*
|
||||
* Connects to rosbridge via WebSocket (roslib.js).
|
||||
* Publishes pan/tilt commands, subscribes to gimbal state + camera.
|
||||
*
|
||||
* ROS topics:
|
||||
* /gimbal/cmd geometry_msgs/Vector3 publish (x=pan°, y=tilt°)
|
||||
* /gimbal/state geometry_msgs/Vector3 subscribe (x=pan°, y=tilt°)
|
||||
* /gimbal/tracking_enabled std_msgs/Bool publish
|
||||
* /camera/front/image_raw/compressed sensor_msgs/CompressedImage subscribe
|
||||
*/
|
||||
|
||||
'use strict';
|
||||
|
||||
// ── Config ────────────────────────────────────────────────────────────────────
|
||||
|
||||
const PAN_MIN = -180; // degrees
|
||||
const PAN_MAX = 180;
|
||||
const TILT_MIN = -45; // degrees
|
||||
const TILT_MAX = 90;
|
||||
const CMD_TOPIC = '/gimbal/cmd';
|
||||
const STATE_TOPIC = '/gimbal/state';
|
||||
const TRACKING_TOPIC = '/gimbal/tracking_enabled';
|
||||
const CAM_TOPIC = '/camera/front/image_raw/compressed';
|
||||
const CAM_TOPICS = {
|
||||
front: '/camera/front/image_raw/compressed',
|
||||
rear: '/camera/rear/image_raw/compressed',
|
||||
left: '/camera/left/image_raw/compressed',
|
||||
right: '/camera/right/image_raw/compressed',
|
||||
};
|
||||
|
||||
const PRESETS = [
|
||||
{ name: 'CENTER', pan: 0, tilt: 0 },
|
||||
{ name: 'LEFT', pan: -45, tilt: 0 },
|
||||
{ name: 'RIGHT', pan: 45, tilt: 0 },
|
||||
{ name: 'UP', pan: 0, tilt: 45 },
|
||||
{ name: 'DOWN', pan: 0, tilt:-30 },
|
||||
{ name: 'HOME', pan: 0, tilt: 0 },
|
||||
];
|
||||
|
||||
// ── State ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
let ros = null;
|
||||
let cmdTopic = null;
|
||||
let trackingTopic = null;
|
||||
let stateSub = null;
|
||||
let camSub = null;
|
||||
|
||||
let currentPan = 0;
|
||||
let currentTilt = 0;
|
||||
let targetPan = 0;
|
||||
let targetTilt = 0;
|
||||
let trackingOn = false;
|
||||
let selectedCam = 'front';
|
||||
|
||||
// Camera FPS tracking
|
||||
let fpsCount = 0;
|
||||
let fpsTime = Date.now();
|
||||
let fps = 0;
|
||||
|
||||
// Pad dragging state
|
||||
let padDragging = false;
|
||||
|
||||
// ── DOM refs ──────────────────────────────────────────────────────────────────
|
||||
|
||||
const connDot = document.getElementById('conn-dot');
|
||||
const connStatus = document.getElementById('conn-status');
|
||||
const wsInput = document.getElementById('ws-input');
|
||||
const padCanvas = document.getElementById('gimbal-pad');
|
||||
const panVal = document.getElementById('pan-val');
|
||||
const tiltVal = document.getElementById('tilt-val');
|
||||
const camImg = document.getElementById('camera-img');
|
||||
const noSignal = document.getElementById('no-signal');
|
||||
const fpsBadge = document.getElementById('fps-badge');
|
||||
const angleOvl = document.getElementById('angle-overlay');
|
||||
|
||||
// ── Connection ────────────────────────────────────────────────────────────────
|
||||
|
||||
function connect() {
|
||||
const url = wsInput.value.trim();
|
||||
if (!url) return;
|
||||
if (ros) { ros.close(); }
|
||||
|
||||
ros = new ROSLIB.Ros({ url });
|
||||
|
||||
ros.on('connection', () => {
|
||||
connDot.className = 'connected';
|
||||
connStatus.textContent = url;
|
||||
document.getElementById('btn-connect').textContent = 'RECONNECT';
|
||||
setupTopics();
|
||||
});
|
||||
|
||||
ros.on('error', (err) => {
|
||||
connDot.className = 'error';
|
||||
connStatus.textContent = 'ERROR: ' + (err?.message || err);
|
||||
});
|
||||
|
||||
ros.on('close', () => {
|
||||
connDot.className = '';
|
||||
connStatus.textContent = 'DISCONNECTED';
|
||||
teardownTopics();
|
||||
});
|
||||
}
|
||||
|
||||
function setupTopics() {
|
||||
// Publish: /gimbal/cmd
|
||||
cmdTopic = new ROSLIB.Topic({
|
||||
ros, name: CMD_TOPIC,
|
||||
messageType: 'geometry_msgs/Vector3',
|
||||
});
|
||||
|
||||
// Publish: /gimbal/tracking_enabled
|
||||
trackingTopic = new ROSLIB.Topic({
|
||||
ros, name: TRACKING_TOPIC,
|
||||
messageType: 'std_msgs/Bool',
|
||||
});
|
||||
|
||||
// Subscribe: /gimbal/state
|
||||
stateSub = new ROSLIB.Topic({
|
||||
ros, name: STATE_TOPIC,
|
||||
messageType: 'geometry_msgs/Vector3',
|
||||
throttle_rate: 100,
|
||||
});
|
||||
stateSub.subscribe((msg) => {
|
||||
currentPan = msg.x ?? 0;
|
||||
currentTilt = msg.y ?? 0;
|
||||
updateAngleDisplay();
|
||||
drawPad();
|
||||
});
|
||||
|
||||
// Subscribe: camera
|
||||
subscribeCamera(selectedCam);
|
||||
}
|
||||
|
||||
function teardownTopics() {
|
||||
if (stateSub) { stateSub.unsubscribe(); stateSub = null; }
|
||||
if (camSub) { camSub.unsubscribe(); camSub = null; }
|
||||
cmdTopic = trackingTopic = null;
|
||||
}
|
||||
|
||||
function subscribeCamera(camId) {
|
||||
if (camSub) { camSub.unsubscribe(); camSub = null; }
|
||||
const topic = CAM_TOPICS[camId] ?? CAM_TOPICS.front;
|
||||
if (!ros) return;
|
||||
camSub = new ROSLIB.Topic({
|
||||
ros, name: topic,
|
||||
messageType: 'sensor_msgs/CompressedImage',
|
||||
throttle_rate: 50,
|
||||
});
|
||||
camSub.subscribe((msg) => {
|
||||
const fmt = msg.format?.includes('png') ? 'png' : 'jpeg';
|
||||
camImg.src = `data:image/${fmt};base64,${msg.data}`;
|
||||
camImg.classList.add('visible');
|
||||
noSignal.style.display = 'none';
|
||||
fpsBadge.classList.add('visible');
|
||||
angleOvl.classList.add('visible');
|
||||
|
||||
// FPS
|
||||
fpsCount++;
|
||||
const now = Date.now();
|
||||
if (now - fpsTime >= 1000) {
|
||||
fps = Math.round(fpsCount * 1000 / (now - fpsTime));
|
||||
fpsCount = 0; fpsTime = now;
|
||||
fpsBadge.textContent = fps + ' fps';
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
// ── Publish helpers ───────────────────────────────────────────────────────────
|
||||
|
||||
function publishCmd(pan, tilt) {
|
||||
if (!cmdTopic) return;
|
||||
cmdTopic.publish(new ROSLIB.Message({ x: pan, y: tilt, z: 0.0 }));
|
||||
}
|
||||
|
||||
function publishTracking(enabled) {
|
||||
if (!trackingTopic) return;
|
||||
trackingTopic.publish(new ROSLIB.Message({ data: enabled }));
|
||||
}
|
||||
|
||||
// ── Gimbal pad drawing ────────────────────────────────────────────────────────
|
||||
|
||||
function drawPad() {
|
||||
const canvas = padCanvas;
|
||||
const ctx = canvas.getContext('2d');
|
||||
const W = canvas.width;
|
||||
const H = canvas.height;
|
||||
|
||||
ctx.clearRect(0, 0, W, H);
|
||||
|
||||
// Background
|
||||
ctx.fillStyle = '#0a0a1a';
|
||||
ctx.fillRect(0, 0, W, H);
|
||||
|
||||
// Grid lines
|
||||
ctx.strokeStyle = '#1e3a5f';
|
||||
ctx.lineWidth = 1;
|
||||
// Vertical lines
|
||||
for (let i = 1; i < 4; i++) {
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(W * i / 4, 0); ctx.lineTo(W * i / 4, H);
|
||||
ctx.stroke();
|
||||
}
|
||||
// Horizontal lines
|
||||
for (let i = 1; i < 4; i++) {
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(0, H * i / 4); ctx.lineTo(W, H * i / 4);
|
||||
ctx.stroke();
|
||||
}
|
||||
|
||||
// Center cross
|
||||
ctx.strokeStyle = '#374151';
|
||||
ctx.lineWidth = 1;
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(W / 2, 0); ctx.lineTo(W / 2, H);
|
||||
ctx.moveTo(0, H / 2); ctx.lineTo(W, H / 2);
|
||||
ctx.stroke();
|
||||
|
||||
// Helpers to map degrees ↔ canvas coords
|
||||
const panToX = (p) => ((p - PAN_MIN) / (PAN_MAX - PAN_MIN)) * W;
|
||||
const tiltToY = (t) => (1 - (t - TILT_MIN) / (TILT_MAX - TILT_MIN)) * H;
|
||||
|
||||
// Target position (what we're commanding)
|
||||
const tx = panToX(targetPan);
|
||||
const ty = tiltToY(targetTilt);
|
||||
|
||||
// Line from center to target
|
||||
ctx.strokeStyle = 'rgba(6,182,212,0.3)';
|
||||
ctx.lineWidth = 1;
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(W / 2, H / 2);
|
||||
ctx.lineTo(tx, ty);
|
||||
ctx.stroke();
|
||||
|
||||
// Target ring
|
||||
ctx.strokeStyle = '#0891b2';
|
||||
ctx.lineWidth = 1.5;
|
||||
ctx.beginPath();
|
||||
ctx.arc(tx, ty, 10, 0, Math.PI * 2);
|
||||
ctx.stroke();
|
||||
|
||||
// Current position (feedback)
|
||||
const cx_ = panToX(currentPan);
|
||||
const cy_ = tiltToY(currentTilt);
|
||||
ctx.fillStyle = '#22d3ee';
|
||||
ctx.beginPath();
|
||||
ctx.arc(cx_, cy_, 6, 0, Math.PI * 2);
|
||||
ctx.fill();
|
||||
|
||||
// Labels: axis limits
|
||||
ctx.fillStyle = '#374151';
|
||||
ctx.font = '9px Courier New';
|
||||
ctx.fillText(`${PAN_MIN}°`, 2, H / 2 - 3);
|
||||
ctx.fillText(`${PAN_MAX}°`, W - 24, H / 2 - 3);
|
||||
ctx.fillText(`${TILT_MAX}°`, W / 2 + 3, 11);
|
||||
ctx.fillText(`${TILT_MIN}°`, W / 2 + 3, H - 3);
|
||||
|
||||
// Current angle text
|
||||
ctx.fillStyle = '#0891b2';
|
||||
ctx.font = 'bold 9px Courier New';
|
||||
ctx.fillText(`PAN ${targetPan.toFixed(0)}° TILT ${targetTilt.toFixed(0)}°`, 4, H - 3);
|
||||
}
|
||||
|
||||
// ── Pad interaction ───────────────────────────────────────────────────────────
|
||||
|
||||
function padCoordsToAngles(clientX, clientY) {
|
||||
const rect = padCanvas.getBoundingClientRect();
|
||||
const rx = (clientX - rect.left) / rect.width;
|
||||
const ry = (clientY - rect.top) / rect.height;
|
||||
const pan = PAN_MIN + rx * (PAN_MAX - PAN_MIN);
|
||||
const tilt = TILT_MAX - ry * (TILT_MAX - TILT_MIN);
|
||||
return {
|
||||
pan: Math.max(PAN_MIN, Math.min(PAN_MAX, pan)),
|
||||
tilt: Math.max(TILT_MIN, Math.min(TILT_MAX, tilt)),
|
||||
};
|
||||
}
|
||||
|
||||
function padPointerDown(e) {
|
||||
padDragging = true;
|
||||
padCanvas.setPointerCapture(e.pointerId);
|
||||
const { pan, tilt } = padCoordsToAngles(e.clientX, e.clientY);
|
||||
setTarget(pan, tilt);
|
||||
}
|
||||
|
||||
function padPointerMove(e) {
|
||||
if (!padDragging) return;
|
||||
const { pan, tilt } = padCoordsToAngles(e.clientX, e.clientY);
|
||||
setTarget(pan, tilt);
|
||||
}
|
||||
|
||||
function padPointerUp() {
|
||||
padDragging = false;
|
||||
}
|
||||
|
||||
padCanvas.addEventListener('pointerdown', padPointerDown);
|
||||
padCanvas.addEventListener('pointermove', padPointerMove);
|
||||
padCanvas.addEventListener('pointerup', padPointerUp);
|
||||
padCanvas.addEventListener('pointercancel', padPointerUp);
|
||||
|
||||
// Touch passthrough
|
||||
padCanvas.addEventListener('touchstart', (e) => e.preventDefault(), { passive: false });
|
||||
padCanvas.addEventListener('touchmove', (e) => e.preventDefault(), { passive: false });
|
||||
|
||||
// ── Target update ─────────────────────────────────────────────────────────────
|
||||
|
||||
function setTarget(pan, tilt) {
|
||||
targetPan = Math.round(pan * 10) / 10;
|
||||
targetTilt = Math.round(tilt * 10) / 10;
|
||||
updateAngleDisplay();
|
||||
drawPad();
|
||||
publishCmd(targetPan, targetTilt);
|
||||
}
|
||||
|
||||
function updateAngleDisplay() {
|
||||
panVal.textContent = currentPan.toFixed(1);
|
||||
tiltVal.textContent = currentTilt.toFixed(1);
|
||||
angleOvl.textContent = `PAN ${currentPan.toFixed(1)}° TILT ${currentTilt.toFixed(1)}°`;
|
||||
}
|
||||
|
||||
// ── Presets ───────────────────────────────────────────────────────────────────
|
||||
|
||||
function applyPreset(pan, tilt) {
|
||||
setTarget(pan, tilt);
|
||||
}
|
||||
|
||||
function goHome() {
|
||||
setTarget(0, 0);
|
||||
}
|
||||
|
||||
// ── Tracking toggle ───────────────────────────────────────────────────────────
|
||||
|
||||
document.getElementById('tracking-toggle').addEventListener('change', (e) => {
|
||||
trackingOn = e.target.checked;
|
||||
publishTracking(trackingOn);
|
||||
});
|
||||
|
||||
// ── Camera selector ───────────────────────────────────────────────────────────
|
||||
|
||||
document.querySelectorAll('.cam-btn').forEach((btn) => {
|
||||
btn.addEventListener('click', () => {
|
||||
selectedCam = btn.dataset.cam;
|
||||
document.querySelectorAll('.cam-btn').forEach((b) => b.classList.remove('active'));
|
||||
btn.classList.add('active');
|
||||
subscribeCamera(selectedCam);
|
||||
// Reset fps display
|
||||
camImg.classList.remove('visible');
|
||||
noSignal.style.display = '';
|
||||
fpsBadge.classList.remove('visible');
|
||||
angleOvl.classList.remove('visible');
|
||||
fpsCount = 0; fpsTime = Date.now();
|
||||
});
|
||||
});
|
||||
|
||||
// ── Connect button ────────────────────────────────────────────────────────────
|
||||
|
||||
document.getElementById('btn-connect').addEventListener('click', connect);
|
||||
wsInput.addEventListener('keydown', (e) => { if (e.key === 'Enter') connect(); });
|
||||
|
||||
// ── Preset buttons ────────────────────────────────────────────────────────────
|
||||
|
||||
document.querySelectorAll('[data-preset]').forEach((btn) => {
|
||||
const idx = parseInt(btn.dataset.preset, 10);
|
||||
const p = PRESETS[idx];
|
||||
btn.addEventListener('click', () => applyPreset(p.pan, p.tilt));
|
||||
});
|
||||
|
||||
document.getElementById('btn-home').addEventListener('click', goHome);
|
||||
|
||||
// ── Init ──────────────────────────────────────────────────────────────────────
|
||||
|
||||
// Size canvas to fill its container initially
|
||||
function resizePad() {
|
||||
const w = padCanvas.offsetWidth || 200;
|
||||
padCanvas.width = w;
|
||||
padCanvas.height = Math.round(w * 0.75); // 4:3 aspect
|
||||
drawPad();
|
||||
}
|
||||
|
||||
window.addEventListener('resize', resizePad);
|
||||
resizePad();
|
||||
|
||||
// Auto-connect on load using stored URL or default
|
||||
const storedUrl = localStorage.getItem('gimbal_ws_url');
|
||||
if (storedUrl) wsInput.value = storedUrl;
|
||||
wsInput.addEventListener('change', () => {
|
||||
localStorage.setItem('gimbal_ws_url', wsInput.value);
|
||||
});
|
||||
|
||||
// Initial draw
|
||||
drawPad();
|
||||
updateAngleDisplay();
|
||||
@ -94,16 +94,12 @@ import { ParameterServer } from './components/ParameterServer.jsx';
|
||||
// Teleop web interface (issue #534)
|
||||
import { TeleopWebUI } from './components/TeleopWebUI.jsx';
|
||||
|
||||
// Gimbal control panel (issue #551)
|
||||
import { GimbalPanel } from './components/GimbalPanel.jsx';
|
||||
|
||||
const TAB_GROUPS = [
|
||||
{
|
||||
label: 'TELEOP',
|
||||
color: 'text-orange-600',
|
||||
tabs: [
|
||||
{ id: 'teleop-webui', label: 'Drive' },
|
||||
{ id: 'gimbal', label: 'Gimbal' },
|
||||
],
|
||||
},
|
||||
{
|
||||
@ -300,10 +296,9 @@ export default function App() {
|
||||
{/* ── Content ── */}
|
||||
<main className={`flex-1 ${
|
||||
activeTab === 'salty-face' ? '' :
|
||||
['eventlog', 'control', 'imu', 'teleop-webui', 'gimbal'].includes(activeTab) ? 'flex flex-col' : 'overflow-y-auto'
|
||||
['eventlog', 'control', 'imu', 'teleop-webui'].includes(activeTab) ? 'flex flex-col' : 'overflow-y-auto'
|
||||
} ${activeTab === 'salty-face' ? '' : 'p-4'}`}>
|
||||
{activeTab === 'teleop-webui' && <TeleopWebUI subscribe={subscribe} publish={publishFn} />}
|
||||
{activeTab === 'gimbal' && <GimbalPanel subscribe={subscribe} publish={publishFn} />}
|
||||
|
||||
{activeTab === 'salty-face' && <SaltyFace subscribe={subscribe} />}
|
||||
|
||||
|
||||
@ -1,382 +0,0 @@
|
||||
/**
|
||||
* GimbalPanel.jsx — Gimbal control panel with live camera preview (Issue #551).
|
||||
*
|
||||
* Features:
|
||||
* - Interactive 2-D pan/tilt joystick pad (click-drag + touch)
|
||||
* - Live camera stream via rosbridge (sensor_msgs/CompressedImage)
|
||||
* - Camera selector: front / rear / left / right
|
||||
* - Preset positions: CENTER / LEFT / RIGHT / UP / DOWN
|
||||
* - Home button (0°, 0°)
|
||||
* - Person-tracking toggle (publishes std_msgs/Bool)
|
||||
* - Current angle display from /gimbal/state feedback
|
||||
* - Mobile-responsive two-column → single-column layout
|
||||
*
|
||||
* ROS topics:
|
||||
* /gimbal/cmd geometry_msgs/Vector3 publish (x=pan°, y=tilt°)
|
||||
* /gimbal/state geometry_msgs/Vector3 subscribe
|
||||
* /gimbal/tracking_enabled std_msgs/Bool publish
|
||||
* /camera/<name>/image_raw/compressed sensor_msgs/CompressedImage subscribe
|
||||
*/
|
||||
|
||||
import { useEffect, useRef, useState, useCallback } from 'react';
|
||||
|
||||
// ── Constants ─────────────────────────────────────────────────────────────────
|
||||
|
||||
const PAN_MIN = -180;
|
||||
const PAN_MAX = 180;
|
||||
const TILT_MIN = -45;
|
||||
const TILT_MAX = 90;
|
||||
|
||||
const CAMERAS = [
|
||||
{ id: 'front', label: 'Front', topic: '/camera/front/image_raw/compressed' },
|
||||
{ id: 'rear', label: 'Rear', topic: '/camera/rear/image_raw/compressed' },
|
||||
{ id: 'left', label: 'Left', topic: '/camera/left/image_raw/compressed' },
|
||||
{ id: 'right', label: 'Right', topic: '/camera/right/image_raw/compressed' },
|
||||
];
|
||||
|
||||
const PRESETS = [
|
||||
{ label: 'CENTER', pan: 0, tilt: 0 },
|
||||
{ label: 'LEFT', pan: -45, tilt: 0 },
|
||||
{ label: 'RIGHT', pan: 45, tilt: 0 },
|
||||
{ label: 'UP', pan: 0, tilt: 45 },
|
||||
{ label: 'DOWN', pan: 0, tilt:-30 },
|
||||
];
|
||||
|
||||
// ── Pan/Tilt pad ──────────────────────────────────────────────────────────────
|
||||
|
||||
function GimbalPad({ targetPan, targetTilt, currentPan, currentTilt, onMove }) {
|
||||
const canvasRef = useRef(null);
|
||||
const dragging = useRef(false);
|
||||
|
||||
const draw = useCallback(() => {
|
||||
const canvas = canvasRef.current;
|
||||
if (!canvas) return;
|
||||
const ctx = canvas.getContext('2d');
|
||||
const W = canvas.width;
|
||||
const H = canvas.height;
|
||||
|
||||
ctx.clearRect(0, 0, W, H);
|
||||
ctx.fillStyle = '#0a0a1a';
|
||||
ctx.fillRect(0, 0, W, H);
|
||||
|
||||
// Grid
|
||||
ctx.strokeStyle = '#1e3a5f';
|
||||
ctx.lineWidth = 1;
|
||||
for (let i = 1; i < 4; i++) {
|
||||
ctx.beginPath(); ctx.moveTo(W * i / 4, 0); ctx.lineTo(W * i / 4, H); ctx.stroke();
|
||||
ctx.beginPath(); ctx.moveTo(0, H * i / 4); ctx.lineTo(W, H * i / 4); ctx.stroke();
|
||||
}
|
||||
|
||||
// Center cross
|
||||
ctx.strokeStyle = '#374151';
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(W / 2, 0); ctx.lineTo(W / 2, H);
|
||||
ctx.moveTo(0, H / 2); ctx.lineTo(W, H / 2);
|
||||
ctx.stroke();
|
||||
|
||||
const panToX = (p) => ((p - PAN_MIN) / (PAN_MAX - PAN_MIN)) * W;
|
||||
const tiltToY = (t) => (1 - (t - TILT_MIN) / (TILT_MAX - TILT_MIN)) * H;
|
||||
|
||||
// Targeting line
|
||||
const tx = panToX(targetPan);
|
||||
const ty = tiltToY(targetTilt);
|
||||
ctx.strokeStyle = 'rgba(6,182,212,0.25)';
|
||||
ctx.lineWidth = 1;
|
||||
ctx.beginPath(); ctx.moveTo(W / 2, H / 2); ctx.lineTo(tx, ty); ctx.stroke();
|
||||
|
||||
// Target ring
|
||||
ctx.strokeStyle = '#0891b2';
|
||||
ctx.lineWidth = 2;
|
||||
ctx.beginPath(); ctx.arc(tx, ty, 10, 0, Math.PI * 2); ctx.stroke();
|
||||
// Cross-hair inside ring
|
||||
ctx.strokeStyle = '#0e7490';
|
||||
ctx.lineWidth = 1;
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(tx - 6, ty); ctx.lineTo(tx + 6, ty);
|
||||
ctx.moveTo(tx, ty - 6); ctx.lineTo(tx, ty + 6);
|
||||
ctx.stroke();
|
||||
|
||||
// Current position dot (feedback)
|
||||
const cx_ = panToX(currentPan);
|
||||
const cy_ = tiltToY(currentTilt);
|
||||
ctx.fillStyle = '#22d3ee';
|
||||
ctx.beginPath(); ctx.arc(cx_, cy_, 5, 0, Math.PI * 2); ctx.fill();
|
||||
|
||||
// Axis labels
|
||||
ctx.fillStyle = '#374151';
|
||||
ctx.font = '9px Courier New';
|
||||
ctx.fillText(`${PAN_MIN}°`, 3, H / 2 - 3);
|
||||
ctx.fillText(`${PAN_MAX}°`, W - 28, H / 2 - 3);
|
||||
ctx.fillText(`+${TILT_MAX}°`, W / 2 + 3, 11);
|
||||
ctx.fillText(`${TILT_MIN}°`, W / 2 + 3, H - 3);
|
||||
|
||||
// Live readout
|
||||
ctx.fillStyle = '#0891b2';
|
||||
ctx.font = 'bold 9px Courier New';
|
||||
ctx.fillText(`▶ ${targetPan.toFixed(0)}° / ${targetTilt.toFixed(0)}°`, 4, H - 3);
|
||||
}, [targetPan, targetTilt, currentPan, currentTilt]);
|
||||
|
||||
useEffect(() => { draw(); }, [draw]);
|
||||
|
||||
const toAngles = (clientX, clientY) => {
|
||||
const rect = canvasRef.current.getBoundingClientRect();
|
||||
const rx = (clientX - rect.left) / rect.width;
|
||||
const ry = (clientY - rect.top) / rect.height;
|
||||
const pan = PAN_MIN + rx * (PAN_MAX - PAN_MIN);
|
||||
const tilt = TILT_MAX - ry * (TILT_MAX - TILT_MIN);
|
||||
return {
|
||||
pan: Math.max(PAN_MIN, Math.min(PAN_MAX, pan)),
|
||||
tilt: Math.max(TILT_MIN, Math.min(TILT_MAX, tilt)),
|
||||
};
|
||||
};
|
||||
|
||||
const onPD = useCallback((e) => {
|
||||
dragging.current = true;
|
||||
canvasRef.current.setPointerCapture(e.pointerId);
|
||||
const { pan, tilt } = toAngles(e.clientX, e.clientY);
|
||||
onMove(pan, tilt);
|
||||
}, [onMove]);
|
||||
|
||||
const onPM = useCallback((e) => {
|
||||
if (!dragging.current) return;
|
||||
const { pan, tilt } = toAngles(e.clientX, e.clientY);
|
||||
onMove(pan, tilt);
|
||||
}, [onMove]);
|
||||
|
||||
const onPU = useCallback(() => { dragging.current = false; }, []);
|
||||
|
||||
return (
|
||||
<canvas
|
||||
ref={canvasRef}
|
||||
width={240}
|
||||
height={160}
|
||||
className="rounded border border-cyan-950 cursor-crosshair w-full"
|
||||
style={{ touchAction: 'none' }}
|
||||
onPointerDown={onPD}
|
||||
onPointerMove={onPM}
|
||||
onPointerUp={onPU}
|
||||
onPointerCancel={onPU}
|
||||
/>
|
||||
);
|
||||
}
|
||||
|
||||
// ── Camera feed ───────────────────────────────────────────────────────────────
|
||||
|
||||
function CameraFeed({ subscribe, topic }) {
|
||||
const [src, setSrc] = useState(null);
|
||||
const [fps, setFps] = useState(0);
|
||||
const cntRef = useRef(0);
|
||||
const timeRef = useRef(Date.now());
|
||||
|
||||
useEffect(() => {
|
||||
setSrc(null);
|
||||
cntRef.current = 0;
|
||||
timeRef.current = Date.now();
|
||||
|
||||
const unsub = subscribe(topic, 'sensor_msgs/CompressedImage', (msg) => {
|
||||
const fmt = msg.format?.includes('png') ? 'png' : 'jpeg';
|
||||
setSrc(`data:image/${fmt};base64,${msg.data}`);
|
||||
cntRef.current++;
|
||||
const now = Date.now();
|
||||
const dt = now - timeRef.current;
|
||||
if (dt >= 1000) {
|
||||
setFps(Math.round(cntRef.current * 1000 / dt));
|
||||
cntRef.current = 0;
|
||||
timeRef.current = now;
|
||||
}
|
||||
});
|
||||
return unsub;
|
||||
}, [subscribe, topic]);
|
||||
|
||||
return (
|
||||
<div className="relative flex-1 min-h-0 bg-black rounded-lg border border-cyan-950 flex items-center justify-center overflow-hidden">
|
||||
{src ? (
|
||||
<img src={src} alt="gimbal camera" className="max-w-full max-h-full object-contain" draggable={false} />
|
||||
) : (
|
||||
<div className="flex flex-col items-center gap-2 text-gray-700">
|
||||
<div className="text-4xl">📷</div>
|
||||
<div className="text-xs tracking-widest">NO SIGNAL</div>
|
||||
</div>
|
||||
)}
|
||||
{src && (
|
||||
<div className="absolute top-2 right-2 bg-black bg-opacity-60 text-green-400 text-xs font-mono px-1.5 py-0.5 rounded">
|
||||
{fps} fps
|
||||
</div>
|
||||
)}
|
||||
</div>
|
||||
);
|
||||
}
|
||||
|
||||
// ── Main component ────────────────────────────────────────────────────────────
|
||||
|
||||
export function GimbalPanel({ subscribe, publish }) {
|
||||
const [selectedCam, setSelectedCam] = useState('front');
|
||||
const [targetPan, setTargetPan] = useState(0);
|
||||
const [targetTilt, setTargetTilt] = useState(0);
|
||||
const [currentPan, setCurrentPan] = useState(0);
|
||||
const [currentTilt, setCurrentTilt] = useState(0);
|
||||
const [tracking, setTracking] = useState(false);
|
||||
|
||||
const currentCam = CAMERAS.find(c => c.id === selectedCam);
|
||||
|
||||
// Subscribe to gimbal state feedback
|
||||
useEffect(() => {
|
||||
const unsub = subscribe('/gimbal/state', 'geometry_msgs/Vector3', (msg) => {
|
||||
setCurrentPan(msg.x ?? 0);
|
||||
setCurrentTilt(msg.y ?? 0);
|
||||
});
|
||||
return unsub;
|
||||
}, [subscribe]);
|
||||
|
||||
// Publish gimbal command
|
||||
const sendCmd = useCallback((pan, tilt) => {
|
||||
const p = Math.round(pan * 10) / 10;
|
||||
const t = Math.round(tilt * 10) / 10;
|
||||
setTargetPan(p);
|
||||
setTargetTilt(t);
|
||||
if (publish) {
|
||||
publish('/gimbal/cmd', 'geometry_msgs/Vector3', { x: p, y: t, z: 0.0 });
|
||||
}
|
||||
}, [publish]);
|
||||
|
||||
// Preset handler
|
||||
const applyPreset = useCallback((pan, tilt) => {
|
||||
sendCmd(pan, tilt);
|
||||
}, [sendCmd]);
|
||||
|
||||
// Tracking toggle
|
||||
const toggleTracking = () => {
|
||||
const next = !tracking;
|
||||
setTracking(next);
|
||||
if (publish) {
|
||||
publish('/gimbal/tracking_enabled', 'std_msgs/Bool', { data: next });
|
||||
}
|
||||
};
|
||||
|
||||
return (
|
||||
<div className="flex flex-col lg:flex-row h-full gap-3">
|
||||
|
||||
{/* ── Left: Camera + pad ── */}
|
||||
<div className="flex flex-col gap-2 flex-1 min-h-0">
|
||||
|
||||
{/* Camera selector */}
|
||||
<div className="flex gap-1 shrink-0 flex-wrap">
|
||||
{CAMERAS.map((cam) => (
|
||||
<button
|
||||
key={cam.id}
|
||||
onClick={() => setSelectedCam(cam.id)}
|
||||
className={`px-3 py-1 text-xs font-bold rounded border tracking-widest transition-colors ${
|
||||
selectedCam === cam.id
|
||||
? 'bg-cyan-950 border-cyan-700 text-cyan-300'
|
||||
: 'bg-gray-950 border-gray-800 text-gray-600 hover:text-gray-300'
|
||||
}`}
|
||||
>
|
||||
{cam.label.toUpperCase()}
|
||||
</button>
|
||||
))}
|
||||
<div className="flex-1" />
|
||||
<div className="text-xs text-gray-700 self-center font-mono truncate max-w-xs">
|
||||
{currentCam?.topic}
|
||||
</div>
|
||||
</div>
|
||||
|
||||
{/* Camera feed */}
|
||||
<CameraFeed subscribe={subscribe} topic={currentCam?.topic} />
|
||||
</div>
|
||||
|
||||
{/* ── Right: Controls ── */}
|
||||
<div className="flex flex-col gap-3 w-full lg:w-72 shrink-0">
|
||||
|
||||
{/* Pan/Tilt pad */}
|
||||
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 space-y-2">
|
||||
<div className="text-xs font-bold tracking-widest text-cyan-700">PAN / TILT</div>
|
||||
<GimbalPad
|
||||
targetPan={targetPan}
|
||||
targetTilt={targetTilt}
|
||||
currentPan={currentPan}
|
||||
currentTilt={currentTilt}
|
||||
onMove={sendCmd}
|
||||
/>
|
||||
<div className="text-xs text-gray-700 text-center">click or drag to aim</div>
|
||||
</div>
|
||||
|
||||
{/* Angle readout */}
|
||||
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 grid grid-cols-2 gap-2">
|
||||
<div>
|
||||
<div className="text-gray-600 text-xs mb-1">TARGET PAN</div>
|
||||
<div className="text-xl font-mono text-cyan-300">{targetPan.toFixed(1)}°</div>
|
||||
</div>
|
||||
<div>
|
||||
<div className="text-gray-600 text-xs mb-1">TARGET TILT</div>
|
||||
<div className="text-xl font-mono text-amber-300">{targetTilt.toFixed(1)}°</div>
|
||||
</div>
|
||||
<div>
|
||||
<div className="text-gray-600 text-xs mb-1">ACTUAL PAN</div>
|
||||
<div className="text-sm font-mono text-cyan-600">{currentPan.toFixed(1)}°</div>
|
||||
</div>
|
||||
<div>
|
||||
<div className="text-gray-600 text-xs mb-1">ACTUAL TILT</div>
|
||||
<div className="text-sm font-mono text-amber-600">{currentTilt.toFixed(1)}°</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
{/* Presets */}
|
||||
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 space-y-2">
|
||||
<div className="text-xs font-bold tracking-widest text-cyan-700">PRESETS</div>
|
||||
<div className="grid grid-cols-3 gap-1.5">
|
||||
{PRESETS.map((p) => (
|
||||
<button
|
||||
key={p.label}
|
||||
onClick={() => applyPreset(p.pan, p.tilt)}
|
||||
className="py-1.5 text-xs font-bold rounded border border-cyan-950 bg-gray-900 text-gray-500 hover:bg-cyan-950 hover:text-cyan-300 hover:border-cyan-800 transition-colors"
|
||||
>
|
||||
<div>{p.label}</div>
|
||||
<div className="text-gray-700 font-normal text-xs">{p.pan}°/{p.tilt}°</div>
|
||||
</button>
|
||||
))}
|
||||
<button
|
||||
onClick={() => applyPreset(0, 0)}
|
||||
className="py-1.5 col-span-3 text-xs font-bold rounded border border-gray-800 bg-gray-900 text-gray-600 hover:bg-gray-800 hover:text-gray-300 transition-colors"
|
||||
>
|
||||
⌂ HOME (0° / 0°)
|
||||
</button>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
{/* Person tracking */}
|
||||
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 space-y-2">
|
||||
<div className="text-xs font-bold tracking-widest text-cyan-700">PERSON TRACKING</div>
|
||||
<div className="flex items-center justify-between">
|
||||
<span className={`text-xs ${tracking ? 'text-cyan-400' : 'text-gray-600'}`}>
|
||||
{tracking ? 'ON — following person' : 'OFF — manual control'}
|
||||
</span>
|
||||
<button
|
||||
onClick={toggleTracking}
|
||||
className={`relative w-10 h-5 rounded-full border transition-colors ${
|
||||
tracking
|
||||
? 'bg-cyan-950 border-cyan-700'
|
||||
: 'bg-gray-900 border-gray-700'
|
||||
}`}
|
||||
>
|
||||
<span className={`absolute top-0.5 w-4 h-4 rounded-full transition-transform ${
|
||||
tracking
|
||||
? 'translate-x-5 bg-cyan-400'
|
||||
: 'translate-x-0.5 bg-gray-600'
|
||||
}`} />
|
||||
</button>
|
||||
</div>
|
||||
<div className="text-xs text-gray-700 font-mono">/gimbal/tracking_enabled</div>
|
||||
</div>
|
||||
|
||||
{/* Topic reference */}
|
||||
<div className="bg-gray-950 rounded border border-gray-800 p-2 text-xs text-gray-700 space-y-1">
|
||||
<div className="font-bold text-gray-600 mb-1">ROS TOPICS</div>
|
||||
<div><span className="text-gray-600">PUB</span> /gimbal/cmd (Vector3)</div>
|
||||
<div><span className="text-gray-600">SUB</span> /gimbal/state (Vector3)</div>
|
||||
<div><span className="text-gray-600">PUB</span> /gimbal/tracking_enabled (Bool)</div>
|
||||
</div>
|
||||
|
||||
</div>
|
||||
</div>
|
||||
);
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user