diff --git a/dist/hornverifier b/dist/hornverifier new file mode 100755 index 00000000..ff0b173c Binary files /dev/null and b/dist/hornverifier differ diff --git a/dist/roverd b/dist/roverd index 4287554c..891b1c45 100755 Binary files a/dist/roverd and b/dist/roverd differ diff --git a/dist/servoverifier b/dist/servoverifier index ff78f449..35426b97 100755 Binary files a/dist/servoverifier and b/dist/servoverifier differ diff --git a/pi/roverd/Makefile b/pi/roverd/Makefile index c57c54b9..4df4d7fb 100644 --- a/pi/roverd/Makefile +++ b/pi/roverd/Makefile @@ -11,9 +11,10 @@ build: pi-build: GOOS=$(GOOS) GOARCH=$(GOARCH) GOARM=$(GOARM) go build -trimpath -ldflags="-s -w" -o $(BIN_DIR)/roverd ./cmd/roverd GOOS=$(GOOS) GOARCH=$(GOARCH) GOARM=$(GOARM) go build -trimpath -ldflags="-s -w" -o $(BIN_DIR)/servoverifier ./cmd/servoverifier + GOOS=$(GOOS) GOARCH=$(GOARCH) GOARM=$(GOARM) go build -trimpath -ldflags="-s -w" -o $(BIN_DIR)/hornverifier ./cmd/hornverifier dummy: GOOS=linux GOARCH=amd64 go build -tags dummy -o $(BIN_DIR)/roverd-dummy ./cmd/roverd clean: - rm -f $(BIN_DIR)/roverd $(BIN_DIR)/servoverifier + rm -f $(BIN_DIR)/roverd $(BIN_DIR)/servoverifier $(BIN_DIR)/hornverifier diff --git a/pi/roverd/cmd/hornverifier/main.go b/pi/roverd/cmd/hornverifier/main.go new file mode 100644 index 00000000..32e0bd06 --- /dev/null +++ b/pi/roverd/cmd/hornverifier/main.go @@ -0,0 +1,183 @@ +package main + +import ( + "bufio" + "encoding/binary" + "flag" + "fmt" + "log" + "math" + "os/exec" + "strconv" + "strings" + "time" +) + +const twoPi = 2 * math.Pi + +func main() { + var ( + device = flag.String("device", "", "ALSA device (empty = default)") + rate = flag.Int("rate", 48000, "Sample rate in Hz") + channels = flag.Int("channels", 1, "Number of audio channels") + duration = flag.Duration("duration", 2*time.Second, "Total horn duration") + freqsRaw = flag.String("freqs", "440,550,660", "Comma-separated frequencies in Hz") + volume = flag.Float64("volume", 0.25, "Output volume 0.0-1.0") + attack = flag.Duration("attack", 20*time.Millisecond, "Attack time") + release = flag.Duration("release", 60*time.Millisecond, "Release time") + ) + flag.Parse() + + if *rate <= 0 { + log.Fatalf("rate must be > 0 (got %d)", *rate) + } + if *channels <= 0 { + log.Fatalf("channels must be > 0 (got %d)", *channels) + } + if *duration <= 0 { + log.Fatalf("duration must be > 0 (got %s)", *duration) + } + if *volume <= 0 || *volume > 1.0 { + log.Fatalf("volume must be within (0,1] (got %.3f)", *volume) + } + if *attack < 0 || *release < 0 { + log.Fatalf("attack/release must be >= 0") + } + + freqs, err := parseFreqs(*freqsRaw) + if err != nil { + log.Fatalf("parse freqs: %v", err) + } + if len(freqs) == 0 { + log.Fatal("no frequencies provided") + } + + if *attack+*release > *duration { + log.Fatalf("attack+release must be <= duration (%s + %s > %s)", *attack, *release, *duration) + } + + args := []string{"-q", "-f", "S16_LE", "-c", fmt.Sprintf("%d", *channels), "-r", fmt.Sprintf("%d", *rate), "-t", "raw"} + if *device != "" { + args = append(args, "-D", *device) + } + cmd := exec.Command("aplay", args...) + stdin, err := cmd.StdinPipe() + if err != nil { + log.Fatalf("aplay stdin: %v", err) + } + if err := cmd.Start(); err != nil { + log.Fatalf("start aplay: %v", err) + } + + writer := bufio.NewWriterSize(stdin, 32*1024) + if err := synthChord(writer, freqs, *rate, *channels, *duration, *volume, *attack, *release); err != nil { + _ = stdin.Close() + _ = cmd.Wait() + log.Fatalf("synth: %v", err) + } + if err := writer.Flush(); err != nil { + _ = stdin.Close() + _ = cmd.Wait() + log.Fatalf("flush: %v", err) + } + if err := stdin.Close(); err != nil { + _ = cmd.Wait() + log.Fatalf("close stdin: %v", err) + } + if err := cmd.Wait(); err != nil { + log.Fatalf("aplay failed: %v", err) + } + log.Print("Horn verification complete") +} + +func parseFreqs(raw string) ([]float64, error) { + trimmed := strings.TrimSpace(raw) + if trimmed == "" { + return nil, nil + } + parts := strings.Split(trimmed, ",") + freqs := make([]float64, 0, len(parts)) + for _, part := range parts { + part = strings.TrimSpace(part) + if part == "" { + continue + } + value, err := strconv.ParseFloat(part, 64) + if err != nil { + return nil, fmt.Errorf("invalid freq %q", part) + } + if value <= 0 { + return nil, fmt.Errorf("freq must be > 0 (got %.3f)", value) + } + freqs = append(freqs, value) + } + return freqs, nil +} + +func synthChord(writer *bufio.Writer, freqs []float64, rate, channels int, duration time.Duration, volume float64, attack, release time.Duration) error { + totalFrames := int(float64(rate) * duration.Seconds()) + if totalFrames <= 0 { + return fmt.Errorf("duration too short") + } + + phase := make([]float64, len(freqs)) + increment := make([]float64, len(freqs)) + for i, f := range freqs { + increment[i] = twoPi * f / float64(rate) + } + + attackFrames := int(float64(rate) * attack.Seconds()) + releaseFrames := int(float64(rate) * release.Seconds()) + steadyFrames := totalFrames - attackFrames - releaseFrames + + framesPerChunk := 512 + buf := make([]byte, framesPerChunk*channels*2) + sampleIndex := 0 + scale := volume / float64(len(freqs)) + + for framesLeft := totalFrames; framesLeft > 0; { + framesNow := framesPerChunk + if framesLeft < framesNow { + framesNow = framesLeft + } + for i := 0; i < framesNow; i++ { + env := envelope(sampleIndex, attackFrames, steadyFrames, releaseFrames) + sample := 0.0 + for j := range freqs { + sample += math.Sin(phase[j]) + phase[j] += increment[j] + if phase[j] > twoPi { + phase[j] -= twoPi + } + } + sample *= scale * env + if sample > 1.0 { + sample = 1.0 + } else if sample < -1.0 { + sample = -1.0 + } + intSample := int16(sample * math.MaxInt16) + offset := i * channels * 2 + for ch := 0; ch < channels; ch++ { + binary.LittleEndian.PutUint16(buf[offset+ch*2:], uint16(intSample)) + } + sampleIndex++ + } + if _, err := writer.Write(buf[:framesNow*channels*2]); err != nil { + return err + } + framesLeft -= framesNow + } + return nil +} + +func envelope(sampleIndex, attackFrames, steadyFrames, releaseFrames int) float64 { + if attackFrames > 0 && sampleIndex < attackFrames { + return float64(sampleIndex) / float64(attackFrames) + } + if releaseFrames > 0 && sampleIndex >= attackFrames+steadyFrames { + relIndex := sampleIndex - (attackFrames + steadyFrames) + return float64(releaseFrames-relIndex) / float64(releaseFrames) + } + return 1.0 +}