github.com/bluenviron/gomavlib/v2@v2.2.1-0.20240308101627-2c07e3da629c/node_heartbeat.go (about)

     1  package gomavlib
     2  
     3  import (
     4  	"reflect"
     5  	"time"
     6  
     7  	"github.com/bluenviron/gomavlib/v2/pkg/message"
     8  )
     9  
    10  type nodeHeartbeat struct {
    11  	n            *Node
    12  	msgHeartbeat message.Message
    13  
    14  	// in
    15  	terminate chan struct{}
    16  
    17  	// out
    18  	done chan struct{}
    19  }
    20  
    21  func newNodeHeartbeat(n *Node) *nodeHeartbeat {
    22  	// module is disabled
    23  	if n.conf.HeartbeatDisable {
    24  		return nil
    25  	}
    26  
    27  	// dialect must be enabled
    28  	if n.conf.Dialect == nil {
    29  		return nil
    30  	}
    31  
    32  	// heartbeat message must exist in dialect and correspond to standard
    33  	msgHeartbeat := func() message.Message {
    34  		for _, m := range n.conf.Dialect.Messages {
    35  			if m.GetID() == 0 {
    36  				return m
    37  			}
    38  		}
    39  		return nil
    40  	}()
    41  	if msgHeartbeat == nil {
    42  		return nil
    43  	}
    44  	mde, err := message.NewReadWriter(msgHeartbeat)
    45  	if err != nil || mde.CRCExtra() != 50 {
    46  		return nil
    47  	}
    48  
    49  	h := &nodeHeartbeat{
    50  		n:            n,
    51  		msgHeartbeat: msgHeartbeat,
    52  		terminate:    make(chan struct{}),
    53  		done:         make(chan struct{}),
    54  	}
    55  
    56  	return h
    57  }
    58  
    59  func (h *nodeHeartbeat) close() {
    60  	close(h.terminate)
    61  	<-h.done
    62  }
    63  
    64  func (h *nodeHeartbeat) run() {
    65  	defer close(h.done)
    66  
    67  	ticker := time.NewTicker(h.n.conf.HeartbeatPeriod)
    68  	defer ticker.Stop()
    69  
    70  	for {
    71  		select {
    72  		case <-ticker.C:
    73  			m := reflect.New(reflect.TypeOf(h.msgHeartbeat).Elem())
    74  			m.Elem().FieldByName("Type").SetUint(uint64(h.n.conf.HeartbeatSystemType))
    75  			m.Elem().FieldByName("Autopilot").SetUint(uint64(h.n.conf.HeartbeatAutopilotType))
    76  			m.Elem().FieldByName("BaseMode").SetUint(0)
    77  			m.Elem().FieldByName("CustomMode").SetUint(0)
    78  			m.Elem().FieldByName("SystemStatus").SetUint(4) // MAV_STATE_ACTIVE
    79  			m.Elem().FieldByName("MavlinkVersion").SetUint(uint64(h.n.conf.Dialect.Version))
    80  			h.n.WriteMessageAll(m.Interface().(message.Message)) //nolint:errcheck
    81  
    82  		case <-h.terminate:
    83  			return
    84  		}
    85  	}
    86  }