gobot.io/x/gobot/v2@v2.1.0/examples/mavlink.go (about)

     1  //go:build example
     2  // +build example
     3  
     4  //
     5  // Do not build by default.
     6  
     7  package main
     8  
     9  import (
    10  	"fmt"
    11  
    12  	"gobot.io/x/gobot/v2"
    13  	"gobot.io/x/gobot/v2/platforms/mavlink"
    14  	common "gobot.io/x/gobot/v2/platforms/mavlink/common"
    15  )
    16  
    17  func main() {
    18  	adaptor := mavlink.NewAdaptor("/dev/ttyACM0")
    19  	iris := mavlink.NewDriver(adaptor)
    20  
    21  	work := func() {
    22  		iris.Once(mavlink.PacketEvent, func(data interface{}) {
    23  			packet := data.(*common.MAVLinkPacket)
    24  
    25  			dataStream := common.NewRequestDataStream(100,
    26  				packet.SystemID,
    27  				packet.ComponentID,
    28  				4,
    29  				1,
    30  			)
    31  			iris.SendPacket(common.CraftMAVLinkPacket(packet.SystemID,
    32  				packet.ComponentID,
    33  				dataStream,
    34  			))
    35  		})
    36  
    37  		iris.On(mavlink.MessageEvent, func(data interface{}) {
    38  			if data.(common.MAVLinkMessage).Id() == 30 {
    39  				message := data.(*common.Attitude)
    40  				fmt.Println("Attitude")
    41  				fmt.Println("TIME_BOOT_MS", message.TIME_BOOT_MS)
    42  				fmt.Println("ROLL", message.ROLL)
    43  				fmt.Println("PITCH", message.PITCH)
    44  				fmt.Println("YAW", message.YAW)
    45  				fmt.Println("ROLLSPEED", message.ROLLSPEED)
    46  				fmt.Println("PITCHSPEED", message.PITCHSPEED)
    47  				fmt.Println("YAWSPEED", message.YAWSPEED)
    48  				fmt.Println("")
    49  			}
    50  		})
    51  	}
    52  
    53  	robot := gobot.NewRobot("mavBot",
    54  		[]gobot.Connection{adaptor},
    55  		[]gobot.Device{iris},
    56  		work,
    57  	)
    58  
    59  	robot.Start()
    60  }