update bwe

This commit is contained in:
harshabose
2025-05-23 07:30:18 +05:30
parent 9f1131b321
commit 31bc2d08c4
5 changed files with 706 additions and 278 deletions
+352
View File
@@ -0,0 +1,352 @@
package transcode
//
// import (
// "context"
// "errors"
// "fmt"
// "math"
// "sync"
// "time"
//
// "github.com/asticode/go-astiav"
//
// "github.com/harshabose/simple_webrtc_comm/transcode/internal"
// "github.com/harshabose/tools/buffer/pkg"
// )
//
// type VP8Encoder struct {
// buffer buffer.BufferWithGenerator[astiav.Packet]
// filter *Filter
// codec *astiav.Codec
// codecFlags *astiav.Dictionary
// copyCodecFlags *astiav.Dictionary
// codecSettings codecSettings
// bandwidthChan chan int64
// options []EncoderOption
//
// encoderContext *astiav.CodecContext
// fallbackEncoderContext *astiav.CodecContext
//
// ctx context.Context
// mux sync.Mutex
// }
//
// func NewVP8Encoder(ctx context.Context, filter *Filter, options ...EncoderOption) (*VP8Encoder, error) {
// encoder := &VP8Encoder{
// filter: filter,
// codecFlags: astiav.NewDictionary(),
// ctx: ctx,
// }
//
// if encoder.codec = astiav.FindEncoder(astiav.CodecIDVp8); encoder.codec == nil {
// return nil, errors.New("VP8 encoder not found")
// }
//
// encoderContext, err := createNewVP8Encoder(encoder.codec, filter)
// if err != nil {
// return nil, err
// }
// encoder.encoderContext = encoderContext
//
// for _, option := range options {
// if err := option(encoder); err != nil {
// return nil, err
// }
// }
//
// if encoder.codecSettings == nil {
// fmt.Println("warn: no VP8 encoder settings were provided")
// }
//
// copyDict, err := copyDictionary(encoder.codecFlags)
// if err != nil {
// return nil, err
// }
// encoder.copyCodecFlags = copyDict
//
// if err := openVP8Encoder(encoder.encoderContext, encoder.codec, encoder.codecFlags); err != nil {
// return nil, err
// }
//
// if encoder.buffer == nil {
// encoder.buffer = buffer.CreateChannelBuffer(ctx, 256, internal.CreatePacketPool())
// }
//
// return encoder, nil
// }
//
// func (e *VP8Encoder) Start() {
// go e.loop()
// }
//
// func (e *VP8Encoder) GetPacket() (*astiav.Packet, error) {
// ctx, cancel := context.WithTimeout(e.ctx, time.Second)
// defer cancel()
// return e.buffer.Pop(ctx)
// }
//
// func (e *VP8Encoder) WaitForPacket() chan *astiav.Packet {
// return e.buffer.GetChannel()
// }
//
// func (e *VP8Encoder) PutBack(packet *astiav.Packet) {
// e.buffer.PutBack(packet)
// }
//
// func (e *VP8Encoder) GetTimeBase() astiav.Rational {
// e.mux.Lock()
// defer e.mux.Unlock()
//
// if e.encoderContext != nil {
// return e.encoderContext.TimeBase()
// }
// if e.fallbackEncoderContext != nil {
// return e.fallbackEncoderContext.TimeBase()
// }
// return astiav.Rational{}
// }
//
// func (e *VP8Encoder) GetDuration() time.Duration {
// e.mux.Lock()
// defer e.mux.Unlock()
//
// if e.encoderContext != nil {
// return time.Duration(float64(time.Second) / e.encoderContext.Framerate().Float64())
// }
// if e.fallbackEncoderContext != nil {
// return time.Duration(float64(time.Second) / e.fallbackEncoderContext.Framerate().Float64())
// }
// return time.Second / 30
// }
//
// func (e *VP8Encoder) SetBitrateChannel(channel chan int64) {
// e.mux.Lock()
// defer e.mux.Unlock()
// e.bandwidthChan = channel
// }
//
// // Get current VP8 bitrate from encoder context
// func (e *VP8Encoder) getCurrentBitrate() (int64, error) {
// e.mux.Lock()
// defer e.mux.Unlock()
//
// if e.encoderContext != nil {
// return e.encoderContext.BitRate() / 1000, nil // Convert to kbps
// }
// return 0, errors.New("no encoder context available")
// }
//
// // Update VP8 bitrate (simpler than x264)
// func (e *VP8Encoder) updateBitrate(bitrate int64) error {
// start := time.Now()
//
// e.mux.Lock()
// current, err := e.getCurrentBitrate()
// if err != nil {
// e.mux.Unlock()
// fmt.Printf("error getting current bitrate; err: %s\n", err.Error())
// return err
// }
//
// // Same change logic as your x264 version
// change := math.Abs(float64(current)-float64(bitrate)) / math.Abs(float64(current))
//
// if change < 0.1 || change > 2.0 {
// e.mux.Unlock()
// fmt.Printf("change not appropriate; current: %d; new: %d; change:%f\n", current, bitrate, change)
// return nil
// }
//
// fmt.Printf("VP8 bitrate change approved; change: %f\n", change)
//
// // Set VP8 bitrate parameters
// if err := e.updateVP8Options(bitrate); err != nil {
// e.mux.Unlock()
// fmt.Printf("error while updating VP8 options; err: %s\n", err.Error())
// return err
// }
//
// e.mux.Unlock()
// if err := e.createNewEncoderContext(); err != nil {
// return err
// }
//
// duration := time.Since(start)
// fmt.Printf("🔄 VP8 Bitrate updated: %d → %d (%.1f%%) in %v\n",
// current, bitrate, change*100, duration)
//
// return nil
// }
//
// // Update VP8-specific options
// func (e *VP8Encoder) updateVP8Options(bitrate int64) error {
// // VP8 uses simpler parameter names
// paramsToUpdate := map[string]string{
// "deadline": "1", // Real-time encoding
// "b:v": fmt.Sprintf("%dk", bitrate), // Target bitrate
// "minrate": fmt.Sprintf("%dk", bitrate*80/100), // Min bitrate (80% of target)
// "maxrate": fmt.Sprintf("%dk", bitrate*120/100), // Max bitrate (120% of target)
// "bufsize": fmt.Sprintf("%dk", bitrate/5), // Buffer size
// "crf": "10", // Good quality balance
// "cpu-used": "8", // Fastest preset for real-time
// }
//
// for param, value := range paramsToUpdate {
// if err := e.copyCodecFlags.Set(param, value, 0); err != nil {
// return err
// }
// }
//
// return nil
// }
//
// // Rest of your encoder methods...
// func (e *VP8Encoder) createNewEncoderContext() error {
// e.mux.Lock()
// e.fallbackEncoderContext = e.encoderContext
// e.encoderContext = nil
//
// copyDict, err := copyDictionary(e.copyCodecFlags)
// if err != nil {
// e.mux.Unlock()
// return err
// }
//
// e.codecFlags.Free()
// e.codecFlags = copyDict
// e.mux.Unlock()
//
// encoderContext, err := createNewOpenVP8Encoder(e.codec, e.filter, e.codecFlags)
// if err != nil {
// e.mux.Lock()
// e.encoderContext = e.fallbackEncoderContext
// e.fallbackEncoderContext = nil
// e.mux.Unlock()
// fmt.Printf("New VP8 encoder creation failed, reverted: %v\n", err)
// return err
// }
//
// e.mux.Lock()
// oldFallback := e.fallbackEncoderContext
// e.encoderContext = encoderContext
// e.fallbackEncoderContext = nil
// e.mux.Unlock()
//
// if oldFallback != nil {
// oldFallback.Free()
// fmt.Printf("🧹 Cleaned up fallback VP8 encoder context\n")
// }
//
// return nil
// }
//
// func (e *VP8Encoder) pickContextAndProcess(frame *astiav.Frame) error {
// e.mux.Lock()
// defer e.mux.Unlock()
//
// if e.encoderContext != nil {
// return e.sendFrameAndPutPackets(e.encoderContext, frame)
// }
// if e.fallbackEncoderContext != nil {
// return e.sendFrameAndPutPackets(e.fallbackEncoderContext, frame)
// }
// return errors.New("invalid VP8 encoder context state")
// }
//
// func (e *VP8Encoder) sendFrameAndPutPackets(encoderContext *astiav.CodecContext, frame *astiav.Frame) error {
// defer e.filter.PutBack(frame)
//
// if err := encoderContext.SendFrame(frame); err != nil {
// return err
// }
//
// for {
// packet := e.buffer.Generate()
// if err := encoderContext.ReceivePacket(packet); err != nil {
// e.buffer.PutBack(packet)
// break
// }
// if err := e.pushPacket(packet); err != nil {
// e.buffer.PutBack(packet)
// continue
// }
// }
// return nil
// }
//
// func (e *VP8Encoder) pushPacket(packet *astiav.Packet) error {
// ctx, cancel := context.WithTimeout(e.ctx, time.Second)
// defer cancel()
// return e.buffer.Push(ctx, packet)
// }
//
// func (e *VP8Encoder) loop() {
// e.encoderContext.SetBitRate(2_000_000)
// fmt.Println("VP8 loop started")
// defer e.Close()
//
// for {
// select {
// case <-e.ctx.Done():
// return
// case bitrate := <-e.bandwidthChan:
// fmt.Println("bitrate recommended:", bitrate)
// // if err := e.updateBitrate(bitrate); err != nil {
// // fmt.Printf("error while updating VP8 bitrate; err: %s\n", err.Error())
// // }
// case frame := <-e.filter.WaitForFrame():
// if err := e.pickContextAndProcess(frame); err != nil {
// if !errors.Is(err, astiav.ErrEagain) {
// continue
// }
// }
// }
// }
// }
//
// func (e *VP8Encoder) Close() {
// e.mux.Lock()
// defer e.mux.Unlock()
//
// if e.encoderContext != nil {
// e.encoderContext.Free()
// e.encoderContext = nil
// }
// if e.fallbackEncoderContext != nil {
// e.fallbackEncoderContext.Free()
// e.fallbackEncoderContext = nil
// }
// }
//
// // Helper functions for VP8 encoder creation
// func createNewVP8Encoder(codec *astiav.Codec, filter *Filter) (*astiav.CodecContext, error) {
// encoderContext := astiav.AllocCodecContext(codec)
// if encoderContext == nil {
// return nil, ErrorAllocateCodecContext
// }
//
// // Set VP8-specific context parameters
// withVideoSetEncoderContextParameter(filter, encoderContext)
//
// return encoderContext, nil
// }
//
// func createNewOpenVP8Encoder(codec *astiav.Codec, filter *Filter, settings *astiav.Dictionary) (*astiav.CodecContext, error) {
// encoderContext, err := createNewVP8Encoder(codec, filter)
// if err != nil {
// return nil, err
// }
//
// if err := openVP8Encoder(encoderContext, codec, settings); err != nil {
// encoderContext.Free()
// return nil, err
// }
//
// return encoderContext, nil
// }
//
// func openVP8Encoder(encoderContext *astiav.CodecContext, codec *astiav.Codec, settings *astiav.Dictionary) error {
// encoderContext.SetFlags(astiav.NewCodecContextFlags(astiav.CodecContextFlagGlobalHeader))
// return encoderContext.Open(codec, settings)
// }
+76 -26
View File
@@ -13,7 +13,7 @@ import (
)
type (
EncoderOption = func(*Encoder) error
EncoderOption = func(encoder *Encoder) error
)
type codecSettings interface {
@@ -247,16 +247,16 @@ var HighQualityX264Settings = X264OpenSettings{
var WebRTCOptimisedX264Settings = X264OpenSettings{
X264Opts: X264Opts{
// RateControl: "cbr",
Bitrate: "2500", // Keep your current target
VBVMaxBitrate: "2500", // Same as target!
VBVBuffer: "100", // 2500/30fps ≈ 83 kbits (single frame)
RateTol: "1.0", // More tolerance
SyncLookAhead: "0", // Already correct
AnnexB: "1", // Already correct
Bitrate: "800", // Keep your current target
VBVMaxBitrate: "900", // Same as target!
VBVBuffer: "300", // 2500/30fps ≈ 83 kbits (single frame)
RateTol: "0.1", // More tolerance
SyncLookAhead: "0", // Already correct
AnnexB: "1", // Already correct
},
LookAhead: "0", // Critical fix!
Qmin: "16", // Wider range
Qmax: "45", // Much wider range
Qmin: "26", // Wider range
Qmax: "42", // Much wider range
Level: "3.1", // Better compatibility
Preset: "ultrafast",
Tune: "zerolatency",
@@ -264,14 +264,14 @@ var WebRTCOptimisedX264Settings = X264OpenSettings{
Profile: "baseline",
BFrames: "0",
BAdapt: "0",
NGOP: "30",
NGOPMin: "15",
NGOP: "50",
NGOPMin: "25",
Scenecut: "0",
InfraRefresh: "1",
SlicedThreads: "1",
ForceIDR: "1",
AQMode: "0",
AQStrength: "0",
AQMode: "1",
AQStrength: "0.5",
MBTree: "0",
Threads: "0",
@@ -319,19 +319,32 @@ func WithX264LowBandwidthOptions(encoder *Encoder) error {
})
}
func withVideoSetEncoderParameters(filter *Filter) EncoderOption {
return func(encoder *Encoder) error {
withVideoSetEncoderContextParameter(filter, encoder.encoderContext)
return nil
}
}
func withAudioSetEncoderParameters(filter *Filter) EncoderOption {
return func(encoder *Encoder) error {
withAudioSetEncoderContextParameters(filter, encoder.encoderContext)
return nil
}
}
//
//
// func WithDefaultVP8Options(encoder *VP8Encoder) error {
// encoder.codecSettings = DefaultVP8Settings
//
// return encoder.codecSettings.ForEach(func(key, value string) error {
// if value == "" {
// return nil
// }
// return encoder.codecFlags.Set(key, value, 0)
// })
// }
//
// func withVideoSetEncoderParameters(filter *Filter) EncoderOption {
// return func(encoder *VP8Encoder) error {
// withVideoSetEncoderContextParameter(filter, encoder.encoderContext)
// return nil
// }
// }
//
// func withAudioSetEncoderParameters(filter *Filter) EncoderOption {
// return func(encoder *VP8Encoder) error {
// withAudioSetEncoderContextParameters(filter, encoder.encoderContext)
// return nil
// }
// }
func withAudioSetEncoderContextParameters(filter *Filter, eCtx *astiav.CodecContext) {
eCtx.SetTimeBase(filter.sinkContext.TimeBase())
@@ -355,3 +368,40 @@ func WithEncoderBufferSize(size int) EncoderOption {
return nil
}
}
type VP8Settings struct {
Deadline string `vp8:"deadline"` // Real-time encoding
Bitrate string `vp8:"b"` // Target bitrate
MinRate string `vp8:"minrate"` // Minimum bitrate
MaxRate string `vp8:"maxrate"` // Maximum bitrate
BufSize string `vp8:"bufsize"` // Buffer size
CRF string `vp8:"crf"` // Quality setting
CPUUsed string `vp8:"cpu-used"` // Speed preset
}
var DefaultVP8Settings = VP8Settings{
Deadline: "1", // Real-time
Bitrate: "2500k", // 2.5 Mbps
MinRate: "2000k", // Min 2 Mbps
MaxRate: "3000k", // Max 3 Mbps
BufSize: "500k", // 500kb buffer
CRF: "10", // Good quality
CPUUsed: "8", // Fastest
}
func (s VP8Settings) ForEach(fn func(key, value string) error) error {
t := reflect.TypeOf(s)
v := reflect.ValueOf(s)
for i := 0; i < t.NumField(); i++ {
field := t.Field(i)
tag := field.Tag.Get("vp8")
if tag != "" {
if err := fn(tag, v.Field(i).String()); err != nil {
return err
}
}
}
return nil
}
-4
View File
@@ -18,7 +18,6 @@ type Filter struct {
decoder *Decoder
buffer buffer.BufferWithGenerator[astiav.Frame]
graph *astiav.FilterGraph
updators []Updator
input *astiav.FilterInOut
output *astiav.FilterInOut
srcContext *astiav.BuffersrcFilterContext
@@ -127,9 +126,6 @@ func CreateFilter(ctx context.Context, decoder *Decoder, filterConfig *FilterCon
func (filter *Filter) Start() {
go filter.loop()
for _, updator := range filter.updators {
updator.Start(filter)
}
}
func (filter *Filter) loop() {
+35 -6
View File
@@ -2,6 +2,7 @@ package transcode
import (
"context"
"encoding/base64"
"errors"
"fmt"
"math"
@@ -232,11 +233,29 @@ func (e *Encoder) updateX264OptsWithNewBitrate(newBitrate int64) error {
x264opts := entry.Value()
parts := strings.Split(x264opts, ":")
// Calculate VBV parameters based on bitrate
vbvMaxRate := newBitrate // Same as target for CBR-like behavior
vbvBuffer := max(newBitrate/10, 100) // ~40ms buffer at target bitrate
// Enforce level 3.1 limits (14,000 kbps max)
maxAllowedBitrate := int64(12000) // Stay below 14,000 limit
// Clamp the new bitrate
if newBitrate > maxAllowedBitrate {
fmt.Printf("⚠️ Bitrate %d clamped to %d (level 3.1 limit)\n", newBitrate, maxAllowedBitrate)
newBitrate = maxAllowedBitrate
}
// Conservative VBV calculations within level limits
vbvMaxRate := min(newBitrate+200, maxAllowedBitrate) // Small headroom
vbvBuffer := min(newBitrate/2, 5000) // Cap buffer at 5000kb
// Ensure minimum values
if vbvBuffer < 200 {
vbvBuffer = 200
}
// Ensure maxrate > bitrate (but within limits)
if vbvMaxRate <= newBitrate {
vbvMaxRate = min(newBitrate+100, maxAllowedBitrate)
}
// Update all VBV-related parameters
paramsToUpdate := map[string]string{
"bitrate": fmt.Sprintf("%d", newBitrate),
"vbv-maxrate": fmt.Sprintf("%d", vbvMaxRate),
@@ -254,13 +273,15 @@ func (e *Encoder) updateX264OptsWithNewBitrate(newBitrate int64) error {
}
}
// If parameter not found, add it
if !found {
parts = append(parts, fmt.Sprintf("%s=%s", paramName, paramValue))
}
}
newX264opts := strings.Join(parts, ":")
fmt.Printf("🔧 Safe update: bitrate=%d, vbv-maxrate=%d, vbv-bufsize=%d\n",
newBitrate, vbvMaxRate, vbvBuffer)
return e.copyCodecFlags.Set("x264opts", newX264opts, 0)
}
@@ -286,7 +307,7 @@ func (e *Encoder) updateBitrate(bitrate int64) error {
change := math.Abs(float64(current)-float64(bitrate)) / math.Abs(float64(current))
if change < 0.5 || change > 2 {
if change < 0.1 || change > 2 {
e.mux.Unlock()
fmt.Printf("change not appropriate; current: %d; new: %d; change:%f\n", current, bitrate, change)
return nil
@@ -375,6 +396,7 @@ func (e *Encoder) loop() {
case <-e.ctx.Done():
return
case bitrate := <-e.bandwidthChan:
fmt.Println("updated bitrate:", bitrate)
if err := e.updateBitrate(bitrate); err != nil {
fmt.Printf("error while encoding; err: %s\n", err.Error())
}
@@ -433,6 +455,13 @@ func (e *Encoder) findParameterSets(extraData []byte) {
}
fmt.Println("SPS for current encoder: ", e.sps)
fmt.Println("PPS for current encoder: ", e.pps)
// Convert to base64
spsBase64 := base64.StdEncoding.EncodeToString(e.sps)
ppsBase64 := base64.StdEncoding.EncodeToString(e.pps)
fmt.Printf("DefaultSPSBase64 = \"%s\"\n", spsBase64)
fmt.Printf("DefaultPPSBase64 = \"%s\"\n", ppsBase64)
}
}
+243 -242
View File
@@ -1,244 +1,245 @@
package transcode
import (
"context"
"errors"
"fmt"
"sync"
"time"
"github.com/aler9/gomavlib"
"github.com/aler9/gomavlib/pkg/dialects/ardupilotmega"
"github.com/aler9/gomavlib/pkg/dialects/common"
"github.com/asticode/go-astiav"
)
type Propeller string
func (prop Propeller) String() string {
return string(prop)
}
const (
PropellerOne Propeller = "propeller0"
PropellerTwo Propeller = "propeller1"
PropellerThree Propeller = "propeller2"
PropellerFour Propeller = "propeller3"
PropellerFive Propeller = "propeller4"
PropellerSix Propeller = "propeller5"
PropellerSeven Propeller = "propeller6"
PropellerEight Propeller = "propeller7"
)
type Updator interface {
Start(*Filter)
}
func WithUpdateFilter(updator Updator) FilterOption {
return func(filter *Filter) error {
filter.updators = append(filter.updators, updator)
return nil
}
}
type notch struct {
prop Propeller
harmonics uint8
frequencies []float32
nBlades uint8
}
func createNotch(prop Propeller, fundamental float32, harmonics, nBlades uint8) *notch {
n := &notch{
prop: prop,
harmonics: harmonics,
frequencies: make([]float32, harmonics),
nBlades: nBlades,
}
for i := uint8(0); i < n.harmonics; i++ {
n.frequencies[i] = fundamental * float32(i+1)
}
return n
}
func (notch *notch) update(rpm float32) {
fundamental := rpm * float32(notch.nBlades) / 60.0
for i := uint8(0); i < notch.harmonics; i++ {
notch.frequencies[i] = (notch.frequencies[i] + fundamental*float32(i+1)) / 2.0
}
}
type PropNoiseFilterUpdator struct {
notches []*notch
node *gomavlib.Node
interval time.Duration
mux sync.RWMutex
flags astiav.FilterCommandFlags
ctx context.Context
}
func CreatePropNoiseFilterUpdator(ctx context.Context, mavlinkSerial string, baudrate int, interval time.Duration) (*PropNoiseFilterUpdator, error) {
updater := &PropNoiseFilterUpdator{
notches: make([]*notch, 0),
flags: astiav.NewFilterCommandFlags(astiav.FilterCommandFlagFast, astiav.FilterCommandFlagOne),
interval: interval,
ctx: ctx,
}
config := gomavlib.NodeConf{
Endpoints: []gomavlib.EndpointConf{
gomavlib.EndpointSerial{
Device: mavlinkSerial,
Baud: baudrate,
},
},
Dialect: ardupilotmega.Dialect,
OutVersion: gomavlib.V2,
OutSystemID: 10,
}
node, err := gomavlib.NewNode(config)
if err != nil {
return nil, err
}
updater.node = node
return updater, nil
}
func (update *PropNoiseFilterUpdator) AddNotchFilter(id Propeller, frequency float32, harmonics uint8, nBlades uint8) {
update.mux.Lock()
// rpm nBlades will have RPM to rpm conversion with number of blades (Nb / 60)
update.notches = append(update.notches, createNotch(id, frequency, harmonics, nBlades))
update.mux.Unlock()
}
func (update *PropNoiseFilterUpdator) loop1() {
ticker := time.NewTicker(update.interval)
defer ticker.Stop()
for {
select {
case <-update.ctx.Done():
return
case <-ticker.C:
update.node.WriteMessageAll(&ardupilotmega.MessageCommandLong{
TargetSystem: 1,
TargetComponent: 0,
Command: common.MAV_CMD_REQUEST_MESSAGE,
Confirmation: 0,
Param1: float32((&ardupilotmega.MessageEscTelemetry_1To_4{}).GetID()),
Param2: 0,
Param3: 0,
Param4: 0,
Param5: 0,
Param6: 0,
Param7: 0,
})
update.node.WriteMessageAll(&ardupilotmega.MessageCommandLong{
TargetSystem: 1,
TargetComponent: 0,
Command: common.MAV_CMD_REQUEST_MESSAGE,
Confirmation: 0,
Param1: float32((&ardupilotmega.MessageEscTelemetry_5To_8{}).GetID()),
Param2: 0,
Param3: 0,
Param4: 0,
Param5: 0,
Param6: 0,
Param7: 0,
})
}
}
}
func (update *PropNoiseFilterUpdator) loop2() {
eventChan := update.node.Events()
loop:
for {
select {
case <-update.ctx.Done():
return
case event, ok := <-eventChan:
if !ok {
return
}
if frm, ok := event.(*gomavlib.EventFrame); ok {
switch msg := frm.Message().(type) {
case *ardupilotmega.MessageEscTelemetry_1To_4:
update.mux.Lock()
length := min(len(update.notches), 4)
if length <= 0 {
continue loop
}
for i := 0; i < length; i++ {
update.notches[i].update(float32(msg.Rpm[i]))
}
update.mux.Unlock()
case *ardupilotmega.MessageEscTelemetry_5To_8:
update.mux.Lock()
length := min(len(update.notches)-4, 4)
if length <= 0 {
continue loop
}
for i := 0; i < length; i++ {
update.notches[i+4].update(float32(msg.Rpm[i]))
}
update.mux.Unlock()
}
}
}
}
}
func (update *PropNoiseFilterUpdator) loop3(filter *Filter) {
ticker := time.NewTicker(update.interval)
defer ticker.Stop()
for {
select {
case <-update.ctx.Done():
return
case <-ticker.C:
if err := update.update(filter); err != nil {
fmt.Printf("Error updating notch filter: %v\n", err)
}
}
}
}
func (update *PropNoiseFilterUpdator) Start(filter *Filter) {
go update.loop1()
go update.loop2()
go update.loop3(filter)
}
func (update *PropNoiseFilterUpdator) update(filter *Filter) error {
if filter == nil {
return errors.New("filter is nil")
}
filter.mux.Lock()
defer filter.mux.Unlock()
for index, notch := range update.notches {
target := fmt.Sprintf("%s%d", notch.prop.String(), index)
for _, frequency := range notch.frequencies {
if _, err := filter.graph.SendCommand(target, "frequency", fmt.Sprintf("%.2f", frequency), update.flags); err != nil {
return err
}
}
}
return nil
}
//
// import (
// "context"
// "errors"
// "fmt"
// "sync"
// "time"
//
// "github.com/aler9/gomavlib"
// "github.com/aler9/gomavlib/pkg/dialects/ardupilotmega"
// "github.com/aler9/gomavlib/pkg/dialects/common"
// "github.com/asticode/go-astiav"
// )
//
// type Propeller string
//
// func (prop Propeller) String() string {
// return string(prop)
// }
//
// const (
// PropellerOne Propeller = "propeller0"
// PropellerTwo Propeller = "propeller1"
// PropellerThree Propeller = "propeller2"
// PropellerFour Propeller = "propeller3"
// PropellerFive Propeller = "propeller4"
// PropellerSix Propeller = "propeller5"
// PropellerSeven Propeller = "propeller6"
// PropellerEight Propeller = "propeller7"
// )
//
// type Updator interface {
// Start(*Filter)
// }
//
// func WithUpdateFilter(updator Updator) FilterOption {
// return func(filter *Filter) error {
// filter.updators = append(filter.updators, updator)
// return nil
// }
// }
//
// type notch struct {
// prop Propeller
// harmonics uint8
// frequencies []float32
// nBlades uint8
// }
//
// func createNotch(prop Propeller, fundamental float32, harmonics, nBlades uint8) *notch {
// n := &notch{
// prop: prop,
// harmonics: harmonics,
// frequencies: make([]float32, harmonics),
// nBlades: nBlades,
// }
//
// for i := uint8(0); i < n.harmonics; i++ {
// n.frequencies[i] = fundamental * float32(i+1)
// }
//
// return n
// }
//
// func (notch *notch) update(rpm float32) {
// fundamental := rpm * float32(notch.nBlades) / 60.0
// for i := uint8(0); i < notch.harmonics; i++ {
// notch.frequencies[i] = (notch.frequencies[i] + fundamental*float32(i+1)) / 2.0
// }
// }
//
// type PropNoiseFilterUpdator struct {
// notches []*notch
// node *gomavlib.Node
// interval time.Duration
// mux sync.RWMutex
// flags astiav.FilterCommandFlags
// ctx context.Context
// }
//
// func CreatePropNoiseFilterUpdator(ctx context.Context, mavlinkSerial string, baudrate int, interval time.Duration) (*PropNoiseFilterUpdator, error) {
// updater := &PropNoiseFilterUpdator{
// notches: make([]*notch, 0),
// flags: astiav.NewFilterCommandFlags(astiav.FilterCommandFlagFast, astiav.FilterCommandFlagOne),
// interval: interval,
// ctx: ctx,
// }
//
// config := gomavlib.NodeConf{
// Endpoints: []gomavlib.EndpointConf{
// gomavlib.EndpointSerial{
// Device: mavlinkSerial,
// Baud: baudrate,
// },
// },
// Dialect: ardupilotmega.Dialect,
// OutVersion: gomavlib.V2,
// OutSystemID: 10,
// }
//
// node, err := gomavlib.NewNode(config)
// if err != nil {
// return nil, err
// }
// updater.node = node
//
// return updater, nil
// }
//
// func (update *PropNoiseFilterUpdator) AddNotchFilter(id Propeller, frequency float32, harmonics uint8, nBlades uint8) {
// update.mux.Lock()
//
// // rpm nBlades will have RPM to rpm conversion with number of blades (Nb / 60)
// update.notches = append(update.notches, createNotch(id, frequency, harmonics, nBlades))
//
// update.mux.Unlock()
// }
//
// func (update *PropNoiseFilterUpdator) loop1() {
// ticker := time.NewTicker(update.interval)
// defer ticker.Stop()
//
// for {
// select {
// case <-update.ctx.Done():
// return
// case <-ticker.C:
// update.node.WriteMessageAll(&ardupilotmega.MessageCommandLong{
// TargetSystem: 1,
// TargetComponent: 0,
// Command: common.MAV_CMD_REQUEST_MESSAGE,
// Confirmation: 0,
// Param1: float32((&ardupilotmega.MessageEscTelemetry_1To_4{}).GetID()),
// Param2: 0,
// Param3: 0,
// Param4: 0,
// Param5: 0,
// Param6: 0,
// Param7: 0,
// })
//
// update.node.WriteMessageAll(&ardupilotmega.MessageCommandLong{
// TargetSystem: 1,
// TargetComponent: 0,
// Command: common.MAV_CMD_REQUEST_MESSAGE,
// Confirmation: 0,
// Param1: float32((&ardupilotmega.MessageEscTelemetry_5To_8{}).GetID()),
// Param2: 0,
// Param3: 0,
// Param4: 0,
// Param5: 0,
// Param6: 0,
// Param7: 0,
// })
// }
// }
// }
//
// func (update *PropNoiseFilterUpdator) loop2() {
// eventChan := update.node.Events()
//
// loop:
// for {
// select {
// case <-update.ctx.Done():
// return
// case event, ok := <-eventChan:
// if !ok {
// return
// }
//
// if frm, ok := event.(*gomavlib.EventFrame); ok {
// switch msg := frm.Message().(type) {
// case *ardupilotmega.MessageEscTelemetry_1To_4:
// update.mux.Lock()
//
// length := min(len(update.notches), 4)
// if length <= 0 {
// continue loop
// }
// for i := 0; i < length; i++ {
// update.notches[i].update(float32(msg.Rpm[i]))
// }
//
// update.mux.Unlock()
// case *ardupilotmega.MessageEscTelemetry_5To_8:
// update.mux.Lock()
//
// length := min(len(update.notches)-4, 4)
// if length <= 0 {
// continue loop
// }
// for i := 0; i < length; i++ {
// update.notches[i+4].update(float32(msg.Rpm[i]))
// }
//
// update.mux.Unlock()
// }
// }
// }
// }
// }
//
// func (update *PropNoiseFilterUpdator) loop3(filter *Filter) {
// ticker := time.NewTicker(update.interval)
// defer ticker.Stop()
//
// for {
// select {
// case <-update.ctx.Done():
// return
// case <-ticker.C:
// if err := update.update(filter); err != nil {
// fmt.Printf("Error updating notch filter: %v\n", err)
// }
// }
// }
// }
//
// func (update *PropNoiseFilterUpdator) Start(filter *Filter) {
// go update.loop1()
// go update.loop2()
// go update.loop3(filter)
// }
//
// func (update *PropNoiseFilterUpdator) update(filter *Filter) error {
// if filter == nil {
// return errors.New("filter is nil")
// }
// filter.mux.Lock()
// defer filter.mux.Unlock()
//
// for index, notch := range update.notches {
// target := fmt.Sprintf("%s%d", notch.prop.String(), index)
// for _, frequency := range notch.frequencies {
// if _, err := filter.graph.SendCommand(target, "frequency", fmt.Sprintf("%.2f", frequency), update.flags); err != nil {
// return err
// }
// }
// }
//
// return nil
// }