gobot.io/x/gobot@v1.16.0/examples/mavlink.go (about)

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