c0ae81a6b0
Some checks are pending
CI Benchmark - Fusion Loop Timing / Fusion Loop Timing Benchmark (push) Waiting to run
Ran gofmt across the entire mothership codebase to ensure consistent code formatting per Go standards. All tests pass after formatting.
531 lines
15 KiB
Go
531 lines
15 KiB
Go
package main
|
||
|
||
import (
|
||
"encoding/binary"
|
||
"math"
|
||
"math/rand"
|
||
)
|
||
|
||
const (
|
||
// WiFi physical constants
|
||
wavelength = 0.123 // meters (2.4 GHz)
|
||
halfWavelength = wavelength / 2.0
|
||
subcarrierSpacing = 312.5e3 // Hz
|
||
c = 3e8 // speed of light m/s
|
||
|
||
// CSI frame constants — must match ingestion/frame.go format
|
||
nSub = 64 // number of subcarriers for HT20
|
||
|
||
// Path loss model constants (log-distance model)
|
||
// PL(d) = PL_0 + 10*n*log10(d/d_0)
|
||
pl0 = 40.0 // dBm reference power at d0
|
||
d0 = 1.0 // meters reference distance
|
||
n = 2.0 // path loss exponent (free space)
|
||
|
||
// Reflection coefficient (power, dimensionless)
|
||
reflectionCoeff = 0.3
|
||
)
|
||
|
||
// generateCSIFrame generates a synthetic CSI binary frame.
|
||
// The frame format matches the ingestion layer (ingestion/frame.go):
|
||
//
|
||
// Header (24 bytes fixed):
|
||
// [0:6] node_mac — TX node MAC address
|
||
// [6:12] peer_mac — RX node MAC address
|
||
// [12:20] timestamp_us — uint64 LE, microseconds since node boot
|
||
// [20] rssi — int8, dBm
|
||
// [21] noise_floor — int8, dBm
|
||
// [22] channel — uint8, WiFi channel
|
||
// [23] n_sub — uint8, subcarrier count
|
||
// Payload (n_sub × 2 bytes):
|
||
// Per subcarrier: int8 I, int8 Q
|
||
func generateCSIFrame(tx, rx *VirtualNode, walkers []*Walker, walls []Wall, frameNum int, rng *rand.Rand) []byte {
|
||
// Calculate combined CSI from all walkers
|
||
amplitude, phaseBase := computeCSIForWalkers(tx, rx, walkers, walls)
|
||
|
||
// Compute RSSI from amplitude
|
||
rssi := amplitudeToRSSI(amplitude)
|
||
|
||
// Create frame buffer (headerSize=24 defined in main.go)
|
||
frame := make([]byte, headerSize+nSub*2)
|
||
|
||
// Write header (matches ingestion/frame.go ParseFrame layout)
|
||
copy(frame[0:6], tx.MAC[:]) // node_mac
|
||
copy(frame[6:12], rx.MAC[:]) // peer_mac
|
||
binary.LittleEndian.PutUint64(frame[12:20], uint64(frameNum*50000)) // timestamp_us
|
||
frame[20] = byte(rssi) // rssi
|
||
var noiseFloor int8 = -95
|
||
frame[21] = byte(noiseFloor) // noise_floor: -95 dBm
|
||
frame[22] = byte(*flagChannel) // channel (from --channel flag)
|
||
frame[23] = nSub // n_sub
|
||
|
||
// Generate I/Q pairs for each subcarrier
|
||
for k := 0; k < nSub; k++ {
|
||
// Phase for this subcarrier
|
||
phase := phaseBase + float64(k)*0.1
|
||
|
||
// Add temporal variation
|
||
phase += 0.1 * math.Sin(2*math.Pi*float64(frameNum)/100.0)
|
||
|
||
// Normalize phase to [-π, π]
|
||
for phase > math.Pi {
|
||
phase -= 2 * math.Pi
|
||
}
|
||
for phase < -math.Pi {
|
||
phase += 2 * math.Pi
|
||
}
|
||
|
||
// Add frequency-selective fading
|
||
freqFading := 0.8 + 0.4*math.Sin(2*math.Pi*float64(k)/16.0)
|
||
subAmplitude := amplitude * freqFading
|
||
|
||
// Generate I/Q with noise
|
||
i, q := generateIQPair(subAmplitude, phase, rng)
|
||
|
||
// Write to payload (interleaved I,Q)
|
||
offset := headerSize + k*2
|
||
frame[offset] = byte(i)
|
||
frame[offset+1] = byte(q)
|
||
}
|
||
|
||
return frame
|
||
}
|
||
|
||
// computeCSIForWalkers computes the combined CSI amplitude and phase from all walkers
|
||
// using a two-ray propagation model (direct path + first-order reflection).
|
||
func computeCSIForWalkers(tx, rx *VirtualNode, walkers []*Walker, walls []Wall) (float64, float64) {
|
||
if len(walkers) == 0 {
|
||
// No walkers, return baseline noise
|
||
return 0.001, 0.0
|
||
}
|
||
|
||
var totalAmplitude float64
|
||
var totalPhase float64
|
||
var weight float64
|
||
|
||
for _, walker := range walkers {
|
||
// Direct path contribution
|
||
directAmp, directPhase := computeDirectPath(tx.Position, rx.Position, walker.Position, walls)
|
||
|
||
// First-order reflection contribution (strongest reflected ray)
|
||
reflAmp, reflPhase := computeFirstOrderReflection(tx.Position, rx.Position, walker.Position, walls)
|
||
|
||
// Combine direct and reflected paths (coherent sum for two-ray model)
|
||
// The reflected ray interferes with the direct ray, creating constructive/destructive interference
|
||
combinedAmp := directAmp + reflAmp*math.Cos(directPhase-reflPhase)
|
||
combinedPhase := math.Atan2(
|
||
directAmp*math.Sin(directPhase)+reflAmp*math.Sin(reflPhase),
|
||
directAmp*math.Cos(directPhase)+reflAmp*math.Cos(reflPhase),
|
||
)
|
||
|
||
// Scale to reasonable values
|
||
combinedAmp *= 1000.0
|
||
|
||
// Accumulate
|
||
totalAmplitude += combinedAmp
|
||
totalPhase += combinedPhase
|
||
weight += 1.0
|
||
}
|
||
|
||
// Normalize phase
|
||
if weight > 0 {
|
||
totalPhase /= weight
|
||
}
|
||
|
||
return totalAmplitude, totalPhase
|
||
}
|
||
|
||
// computeDirectPath computes the CSI contribution from the direct path
|
||
// through the walker to the receiver, using the log-distance path loss model.
|
||
func computeDirectPath(tx, rx, walker Point, walls []Wall) (float64, float64) {
|
||
// Distance from TX to walker
|
||
d1 := distance(tx, walker)
|
||
// Distance from walker to RX
|
||
d2 := distance(walker, rx)
|
||
// Total path length
|
||
dTotal := d1 + d2
|
||
|
||
// Direct TX-RX distance (for Fresnel zone calculation)
|
||
dDirect := distance(tx, rx)
|
||
|
||
// Path length excess for Fresnel zone calculation
|
||
excess := dTotal - dDirect
|
||
if excess < 0 {
|
||
excess = 0
|
||
}
|
||
|
||
// Fresnel zone number
|
||
zoneNumber := int(math.Ceil(excess / halfWavelength))
|
||
if zoneNumber < 1 {
|
||
zoneNumber = 1
|
||
}
|
||
|
||
// Zone decay (inverse square)
|
||
decay := 1.0 / math.Pow(float64(zoneNumber), 2.0)
|
||
|
||
// Log-distance path loss model: PL(d) = PL_0 + 10*n*log10(d/d_0)
|
||
var pathLossDB float64
|
||
if dTotal >= d0 {
|
||
pathLossDB = pl0 + 10.0*n*math.Log10(dTotal/d0)
|
||
} else {
|
||
// For distances < d0, use free space approximation
|
||
pathLossDB = pl0
|
||
}
|
||
|
||
// Wall attenuation on the direct path
|
||
wallLoss := computeWallLoss(tx, walker, walls)
|
||
wallLoss += computeWallLoss(walker, rx, walls)
|
||
|
||
// Total loss in dB
|
||
totalLossDB := pathLossDB + wallLoss
|
||
|
||
// Convert to linear amplitude
|
||
amplitude := math.Pow(10.0, -totalLossDB/20.0)
|
||
|
||
// Apply Fresnel zone decay
|
||
amplitude *= decay
|
||
|
||
// Phase at this position (based on total path length)
|
||
phase := 2 * math.Pi * dTotal / wavelength
|
||
|
||
return amplitude, phase
|
||
}
|
||
|
||
// computeFirstOrderReflection computes the CSI contribution from the strongest
|
||
// first-order reflection off a wall segment. Uses image method to find reflection point.
|
||
func computeFirstOrderReflection(tx, rx, walker Point, walls []Wall) (float64, float64) {
|
||
var bestReflAmp float64
|
||
var bestReflPhase float64
|
||
var foundReflection bool
|
||
|
||
for _, wall := range walls {
|
||
// Compute reflection point using image method
|
||
reflPoint, ok := findReflectionPoint(tx, rx, wall)
|
||
if !ok {
|
||
continue
|
||
}
|
||
|
||
// Reflected path: TX -> reflection point -> RX
|
||
dTxRefl := distance(tx, reflPoint)
|
||
dReflRx := distance(reflPoint, rx)
|
||
dReflTotal := dTxRefl + dReflRx
|
||
|
||
// Direct path length (for comparison)
|
||
dDirect := distance(tx, rx)
|
||
|
||
// Log-distance path loss for reflected path
|
||
var pathLossDB float64
|
||
if dReflTotal >= d0 {
|
||
pathLossDB = pl0 + 10.0*n*math.Log10(dReflTotal/d0)
|
||
} else {
|
||
pathLossDB = pl0
|
||
}
|
||
|
||
// Wall attenuation (transmission loss through wall if any)
|
||
// Plus reflection loss (weakest material first = lowest attenuation)
|
||
wallLoss := wall.Attenuation
|
||
|
||
// Total loss in dB
|
||
totalLossDB := pathLossDB + wallLoss
|
||
|
||
// Convert to linear amplitude
|
||
amplitude := math.Pow(10.0, -totalLossDB/20.0)
|
||
|
||
// Apply reflection coefficient (power coefficient)
|
||
amplitude *= math.Sqrt(reflectionCoeff)
|
||
|
||
// Normalize by direct path loss (so reflected ray is relative to direct)
|
||
directPathLoss := pl0 + 10.0*n*math.Log10(dDirect/d0)
|
||
amplitude *= math.Pow(10.0, (directPathLoss-pathLossDB)/20.0)
|
||
|
||
// Phase based on reflected path length
|
||
phase := 2 * math.Pi * dReflTotal / wavelength
|
||
|
||
// Keep the strongest reflection (lowest attenuation)
|
||
if !foundReflection || amplitude > bestReflAmp {
|
||
bestReflAmp = amplitude
|
||
bestReflPhase = phase
|
||
foundReflection = true
|
||
}
|
||
}
|
||
|
||
if !foundReflection {
|
||
return 0, 0
|
||
}
|
||
|
||
return bestReflAmp, bestReflPhase
|
||
}
|
||
|
||
// findReflectionPoint finds the specular reflection point on a wall segment
|
||
// for a ray from TX to RX using the image method.
|
||
// Returns the reflection point and true if a valid reflection exists, false otherwise.
|
||
func findReflectionPoint(tx, rx Point, wall Wall) (Point, bool) {
|
||
// For a vertical wall segment (in 2D floor plane), the reflection point
|
||
// is found by reflecting the TX across the wall line and finding the
|
||
// intersection with the wall segment.
|
||
|
||
// Wall line equation: ax + by + c = 0
|
||
// For vertical wall from (x1, y1) to (x2, y2):
|
||
// If x1 == x2 (vertical wall), reflection is straightforward
|
||
if math.Abs(wall.X1-wall.X2) < 1e-6 {
|
||
// Vertical wall at x = wall.X1
|
||
// Reflect TX across the wall
|
||
reflTxX := 2*wall.X1 - tx.X
|
||
reflTxY := tx.Y
|
||
|
||
// Find intersection of line from reflTx to RX with the wall
|
||
// Parametric line: reflTx + t*(rx - reflTx)
|
||
dx := rx.X - reflTxX
|
||
dy := rx.Y - reflTxY
|
||
|
||
if math.Abs(dx) < 1e-6 {
|
||
// Line is vertical, no intersection with vertical wall (or parallel)
|
||
return Point{}, false
|
||
}
|
||
|
||
t := (wall.X1 - reflTxX) / dx
|
||
|
||
// Compute intersection point
|
||
intersectY := reflTxY + t*dy
|
||
|
||
// Check if intersection is within wall segment bounds
|
||
minY := math.Min(wall.Y1, wall.Y2)
|
||
maxY := math.Max(wall.Y1, wall.Y2)
|
||
|
||
if intersectY < minY || intersectY > maxY {
|
||
return Point{}, false
|
||
}
|
||
|
||
// Z coordinate is average of TX and RX Z (reflection in vertical plane)
|
||
intersectZ := (tx.Z + rx.Z) / 2.0
|
||
|
||
return Point{X: wall.X1, Y: intersectY, Z: intersectZ}, true
|
||
}
|
||
|
||
// For horizontal wall segment (y1 == y2)
|
||
if math.Abs(wall.Y1-wall.Y2) < 1e-6 {
|
||
// Horizontal wall at y = wall.Y1
|
||
// Reflect TX across the wall
|
||
reflTxX := tx.X
|
||
reflTxY := 2*wall.Y1 - tx.Y
|
||
|
||
// Find intersection of line from reflTx to RX with the wall
|
||
dx := rx.X - reflTxX
|
||
dy := rx.Y - reflTxY
|
||
|
||
if math.Abs(dy) < 1e-6 {
|
||
// Line is horizontal, no intersection
|
||
return Point{}, false
|
||
}
|
||
|
||
t := (wall.Y1 - reflTxY) / dy
|
||
|
||
// Compute intersection point
|
||
intersectX := reflTxX + t*dx
|
||
|
||
// Check if intersection is within wall segment bounds
|
||
minX := math.Min(wall.X1, wall.X2)
|
||
maxX := math.Max(wall.X1, wall.X2)
|
||
|
||
if intersectX < minX || intersectX > maxX {
|
||
return Point{}, false
|
||
}
|
||
|
||
// Z coordinate is average of TX and RX Z
|
||
intersectZ := (tx.Z + rx.Z) / 2.0
|
||
|
||
return Point{X: intersectX, Y: wall.Y1, Z: intersectZ}, true
|
||
}
|
||
|
||
// General case: angled wall segment
|
||
// Compute line intersection
|
||
// Wall line: (x1, y1) to (x2, y2)
|
||
// Ray from reflected TX to RX
|
||
|
||
// Reflect TX across the wall line
|
||
reflTx := reflectPointAcrossLine(tx, Point{X: wall.X1, Y: wall.Y1, Z: 0}, Point{X: wall.X2, Y: wall.Y2, Z: 0})
|
||
|
||
// Find intersection of line from reflTx to RX with the wall line
|
||
intersect, ok := lineIntersection(
|
||
reflTx, rx,
|
||
Point{X: wall.X1, Y: wall.Y1, Z: 0}, Point{X: wall.X2, Y: wall.Y2, Z: 0},
|
||
)
|
||
if !ok {
|
||
return Point{}, false
|
||
}
|
||
|
||
// Check if intersection is within wall segment bounds
|
||
minX := math.Min(wall.X1, wall.X2)
|
||
maxX := math.Max(wall.X1, wall.X2)
|
||
minY := math.Min(wall.Y1, wall.Y2)
|
||
maxY := math.Max(wall.Y1, wall.Y2)
|
||
|
||
if intersect.X < minX || intersect.X > maxX || intersect.Y < minY || intersect.Y > maxY {
|
||
return Point{}, false
|
||
}
|
||
|
||
// Z coordinate is average of TX and RX Z
|
||
intersect.Z = (tx.Z + rx.Z) / 2.0
|
||
|
||
return intersect, true
|
||
}
|
||
|
||
// reflectPointAcrossLine reflects a point across a line defined by two points.
|
||
// Uses the formula for reflection of a point across a line in 2D.
|
||
func reflectPointAcrossLine(p, lineStart, lineEnd Point) Point {
|
||
// Line direction vector
|
||
lx := lineEnd.X - lineStart.X
|
||
ly := lineEnd.Y - lineStart.Y
|
||
|
||
// Vector from line start to point
|
||
px := p.X - lineStart.X
|
||
py := p.Y - lineStart.Y
|
||
|
||
// Project p onto the line (dot product)
|
||
dot := px*lx + py*ly
|
||
lenSq := lx*lx + ly*ly
|
||
|
||
if lenSq < 1e-10 {
|
||
// Line segment is too short
|
||
return p
|
||
}
|
||
|
||
// Projection parameter
|
||
t := dot / lenSq
|
||
|
||
// Closest point on line
|
||
closestX := lineStart.X + t*lx
|
||
closestY := lineStart.Y + t*ly
|
||
|
||
// Reflected point: p' = closest + (closest - p) = 2*closest - p
|
||
return Point{
|
||
X: 2*closestX - p.X,
|
||
Y: 2*closestY - p.Y,
|
||
Z: p.Z,
|
||
}
|
||
}
|
||
|
||
// lineIntersection finds the intersection point of two line segments in 2D.
|
||
// Returns the intersection point and true if the lines intersect, false otherwise.
|
||
func lineIntersection(p1, p2, p3, p4 Point) (Point, bool) {
|
||
// Line 1: p1 to p2
|
||
// Line 2: p3 to p4
|
||
|
||
x1, y1 := p1.X, p1.Y
|
||
x2, y2 := p2.X, p2.Y
|
||
x3, y3 := p3.X, p3.Y
|
||
x4, y4 := p4.X, p4.Y
|
||
|
||
// Compute denominator
|
||
denom := (x1-x2)*(y3-y4) - (y1-y2)*(x3-x4)
|
||
if math.Abs(denom) < 1e-10 {
|
||
// Lines are parallel
|
||
return Point{}, false
|
||
}
|
||
|
||
// Compute intersection point using parametric form
|
||
t := ((x1-x3)*(y3-y4) - (y1-y3)*(x3-x4)) / denom
|
||
u := -((x1-x2)*(y1-y3) - (y1-y2)*(x1-x3)) / denom
|
||
|
||
// Check if intersection is within both line segments
|
||
if t < 0 || t > 1 || u < 0 || u > 1 {
|
||
return Point{}, false
|
||
}
|
||
|
||
// Compute intersection point
|
||
intersectX := x1 + t*(x2-x1)
|
||
intersectY := y1 + t*(y2-y1)
|
||
|
||
return Point{X: intersectX, Y: intersectY}, true
|
||
}
|
||
|
||
// distance computes Euclidean distance between two points
|
||
func distance(a, b Point) float64 {
|
||
dx := a.X - b.X
|
||
dy := a.Y - b.Y
|
||
dz := a.Z - b.Z
|
||
return math.Sqrt(dx*dx + dy*dy + dz*dz)
|
||
}
|
||
|
||
// computeWallLoss computes wall attenuation for a path through all walls.
|
||
// For each wall the path intersects, add the wall's attenuation value.
|
||
func computeWallLoss(from, to Point, walls []Wall) float64 {
|
||
totalLoss := 0.0
|
||
|
||
for _, wall := range walls {
|
||
if pathIntersectsWall(from.X, from.Y, to.X, to.Y, wall.X1, wall.Y1, wall.X2, wall.Y2) {
|
||
totalLoss += wall.Attenuation
|
||
}
|
||
}
|
||
|
||
return totalLoss
|
||
}
|
||
|
||
// pathIntersectsWall checks if a path intersects a wall segment (2D)
|
||
func pathIntersectsWall(x1, y1, x2, y2, wx1, wy1, wx2, wy2 float64) bool {
|
||
// Compute orientations
|
||
ccw := func(ax, ay, bx, by, cx, cy float64) float64 {
|
||
return (bx-ax)*(cy-ay) - (by-ay)*(cx-ax)
|
||
}
|
||
|
||
o1 := ccw(x1, y1, x2, y2, wx1, wy1)
|
||
o2 := ccw(x1, y1, x2, y2, wx2, wy2)
|
||
o3 := ccw(wx1, wy1, wx2, wy2, x1, y1)
|
||
o4 := ccw(wx1, wy1, wx2, wy2, x2, y2)
|
||
|
||
// Check for intersection
|
||
return o1*o2 < 0 && o3*o4 < 0
|
||
}
|
||
|
||
// amplitudeToRSSI converts amplitude to RSSI in dBm
|
||
func amplitudeToRSSI(amplitude float64) int8 {
|
||
// Convert amplitude to dBm (reference: amplitude 1.0 = -30 dBm)
|
||
amplitudeDBm := -30.0 + 20.0*math.Log10(amplitude)
|
||
|
||
// Clamp to realistic range
|
||
if amplitudeDBm < -90 {
|
||
amplitudeDBm = -90
|
||
}
|
||
if amplitudeDBm > -30 {
|
||
amplitudeDBm = -30
|
||
}
|
||
|
||
return int8(amplitudeDBm)
|
||
}
|
||
|
||
// generateIQPair generates a synthetic I/Q pair with Gaussian noise
|
||
func generateIQPair(amplitude, phase float64, rng *rand.Rand) (int8, int8) {
|
||
// Box-Muller transform for Gaussian noise
|
||
u1 := rng.Float64()
|
||
u2 := rng.Float64()
|
||
z0 := math.Sqrt(-2.0*math.Log(u1)) * math.Cos(2.0*math.Pi*u2)
|
||
z1 := math.Sqrt(-2.0*math.Log(u1)) * math.Sin(2.0*math.Pi*u2)
|
||
|
||
noiseI := z0 * *flagNoiseSigma
|
||
noiseQ := z1 * *flagNoiseSigma
|
||
|
||
// Convert to I/Q
|
||
i := amplitude*math.Cos(phase) + noiseI
|
||
q := amplitude*math.Sin(phase) + noiseQ
|
||
|
||
// Scale to int8 range
|
||
scale := 127.0 / 10.0 // Scale factor
|
||
i *= scale
|
||
q *= scale
|
||
|
||
// Clamp to int8 range [-127, 127]
|
||
if i > 127 {
|
||
i = 127
|
||
}
|
||
if i < -127 {
|
||
i = -127
|
||
}
|
||
if q > 127 {
|
||
q = 127
|
||
}
|
||
if q < -127 {
|
||
q = -127
|
||
}
|
||
|
||
return int8(i), int8(q)
|
||
}
|