Improving godocs and adding doc.go to mavlink package
This commit is contained in:
parent
6d2e25b0d8
commit
a9a2a4f46f
|
@ -0,0 +1,72 @@
|
|||
/*
|
||||
This package contains the Gobot adaptor and driver for the MAVlink Communication Protocol (http://qgroundcontrol.org/mavlink/start).
|
||||
|
||||
Installing:
|
||||
|
||||
go get github.com/hybridgroup/gobot && go install github.com/hybridgroup/gobot/platforms/mavlink
|
||||
|
||||
Example:
|
||||
|
||||
package main
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
|
||||
"github.com/hybridgroup/gobot"
|
||||
"github.com/hybridgroup/gobot/platforms/mavlink"
|
||||
common "github.com/hybridgroup/gobot/platforms/mavlink/common"
|
||||
)
|
||||
|
||||
func main() {
|
||||
gbot := gobot.NewGobot()
|
||||
|
||||
adaptor := mavlink.NewMavlinkAdaptor("iris", "/dev/ttyACM0")
|
||||
iris := mavlink.NewMavlinkDriver(adaptor, "iris")
|
||||
|
||||
work := func() {
|
||||
gobot.Once(iris.Event("packet"), func(data interface{}) {
|
||||
packet := data.(*common.MAVLinkPacket)
|
||||
|
||||
dataStream := common.NewRequestDataStream(100,
|
||||
packet.SystemID,
|
||||
packet.ComponentID,
|
||||
4,
|
||||
1,
|
||||
)
|
||||
iris.SendPacket(common.CraftMAVLinkPacket(packet.SystemID,
|
||||
packet.ComponentID,
|
||||
dataStream,
|
||||
))
|
||||
})
|
||||
|
||||
gobot.On(iris.Event("message"), func(data interface{}) {
|
||||
if data.(common.MAVLinkMessage).Id() == 30 {
|
||||
message := data.(*common.Attitude)
|
||||
fmt.Println("Attitude")
|
||||
fmt.Println("TIME_BOOT_MS", message.TIME_BOOT_MS)
|
||||
fmt.Println("ROLL", message.ROLL)
|
||||
fmt.Println("PITCH", message.PITCH)
|
||||
fmt.Println("YAW", message.YAW)
|
||||
fmt.Println("ROLLSPEED", message.ROLLSPEED)
|
||||
fmt.Println("PITCHSPEED", message.PITCHSPEED)
|
||||
fmt.Println("YAWSPEED", message.YAWSPEED)
|
||||
fmt.Println("")
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
robot := gobot.NewRobot("mavBot",
|
||||
[]gobot.Connection{adaptor},
|
||||
[]gobot.Device{iris},
|
||||
work,
|
||||
)
|
||||
|
||||
gbot.AddRobot(robot)
|
||||
|
||||
gbot.Start()
|
||||
}
|
||||
|
||||
For further information refer to mavlink README:
|
||||
https://github.com/hybridgroup/gobot/blob/master/platforms/mavlink/README.md
|
||||
*/
|
||||
package mavlink
|
|
@ -13,6 +13,7 @@ type MavlinkAdaptor struct {
|
|||
connect func(*MavlinkAdaptor)
|
||||
}
|
||||
|
||||
// NewMavLinkAdaptor creates a new mavlink adaptor with specified name and port
|
||||
func NewMavlinkAdaptor(name string, port string) *MavlinkAdaptor {
|
||||
return &MavlinkAdaptor{
|
||||
Adaptor: *gobot.NewAdaptor(
|
||||
|
@ -30,11 +31,13 @@ func NewMavlinkAdaptor(name string, port string) *MavlinkAdaptor {
|
|||
}
|
||||
}
|
||||
|
||||
// Connect returns true if connection to device is successful
|
||||
func (m *MavlinkAdaptor) Connect() bool {
|
||||
m.connect(m)
|
||||
return true
|
||||
}
|
||||
|
||||
// Finalize returns true if connection to devices is closed successfully
|
||||
func (m *MavlinkAdaptor) Finalize() bool {
|
||||
m.sp.Close()
|
||||
return true
|
||||
|
|
|
@ -15,6 +15,11 @@ type MavlinkDriver struct {
|
|||
type MavlinkInterface interface {
|
||||
}
|
||||
|
||||
// NewMavlinkDriver creates a new mavlink driver with specified name.
|
||||
//
|
||||
// It add the following events:
|
||||
// "packet" - triggered when a new packet is read
|
||||
// "message" - triggered when a new valid message is processed
|
||||
func NewMavlinkDriver(a *MavlinkAdaptor, name string) *MavlinkDriver {
|
||||
m := &MavlinkDriver{
|
||||
Driver: *gobot.NewDriver(
|
||||
|
@ -30,10 +35,13 @@ func NewMavlinkDriver(a *MavlinkAdaptor, name string) *MavlinkDriver {
|
|||
return m
|
||||
}
|
||||
|
||||
// adaptor returns driver associated adaptor
|
||||
func (m *MavlinkDriver) adaptor() *MavlinkAdaptor {
|
||||
return m.Driver.Adaptor().(*MavlinkAdaptor)
|
||||
}
|
||||
|
||||
// Start begins process to read mavlink packets every m.Interval
|
||||
// and process them
|
||||
func (m *MavlinkDriver) Start() bool {
|
||||
go func() {
|
||||
for {
|
||||
|
@ -55,8 +63,10 @@ func (m *MavlinkDriver) Start() bool {
|
|||
return true
|
||||
}
|
||||
|
||||
// SendPacket sends a packet to mavlink device
|
||||
func (m *MavlinkDriver) SendPacket(packet *common.MAVLinkPacket) {
|
||||
m.adaptor().sp.Write(packet.Pack())
|
||||
}
|
||||
|
||||
// Halt returns true if device is halted successfully
|
||||
func (m *MavlinkDriver) Halt() bool { return true }
|
||||
|
|
Loading…
Reference in New Issue