├── .gitignore ├── .travis.yml ├── x25 ├── x25_test.go └── x25.go ├── License.md ├── mavlink ├── doc.go ├── dialect_test.go ├── dialect.go ├── definitions │ ├── test.xml │ ├── python_array_test.xml │ ├── ualberta.xml │ ├── autoquad.xml │ ├── minimal.xml │ ├── ASLUAV.xml │ ├── pixhawk.xml │ ├── slugs.xml │ └── matrixpilot.xml ├── message_test.go ├── message.go ├── ualberta.go ├── asluav.go └── pixhawk.go ├── examples └── udprx │ └── main.go ├── mavgen ├── main.go ├── mavgen_test.go └── mavgen.go └── ReadMe.md /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | 3 | # don't commit mavgen binary 4 | mavgen/mavgen 5 | 6 | # os x cruft 7 | .DS_Store 8 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | 2 | language: go 3 | 4 | go: 5 | - 1.4 6 | - 1.5 7 | - tip 8 | 9 | # run mavgen on the mavlink package, and run tests 10 | 11 | script: 12 | - go generate github.com/liamstask/go-mavlink/mavlink 13 | - go test -v ./... 14 | -------------------------------------------------------------------------------- /x25/x25_test.go: -------------------------------------------------------------------------------- 1 | package x25 2 | 3 | import ( 4 | // "hash" 5 | "testing" 6 | ) 7 | 8 | func TestParsePacket(t *testing.T) { 9 | x := New() 10 | x.Sum16() 11 | // if h, ok := x.(*hash.Hash); !ok { 12 | // t.Errorf("X25 does not implement hash.Hash") 13 | // } 14 | } 15 | -------------------------------------------------------------------------------- /License.md: -------------------------------------------------------------------------------- 1 | Copyright 2015 Liam Staskawicz 2 | 3 | Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 4 | 5 | http://www.apache.org/licenses/LICENSE-2.0 6 | 7 | Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. 8 | -------------------------------------------------------------------------------- /mavlink/doc.go: -------------------------------------------------------------------------------- 1 | /* 2 | Package mavlink provides support for encoding/decoding mavlink (http://qgroundcontrol.org/mavlink/start) messages. 3 | 4 | Encoding is done via the Encoder type, which wraps an io.Writer, and decoding is done via the Decoder type, which wraps an io.Reader. 5 | 6 | The Packet type represents the wire type used for message transmission/reception, and a variety of generated classes each implement the Message type. 7 | A Message can be converted to a Packet by calling Message.Pack(p) and a Packet can be converted to a Message by calling Message.Unpack(p). 8 | 9 | See the ReadMe for examples of typical usage. 10 | 11 | */ 12 | package mavlink 13 | -------------------------------------------------------------------------------- /x25/x25.go: -------------------------------------------------------------------------------- 1 | package x25 2 | 3 | // implements hash.Hash 4 | type X25 struct { 5 | crc uint16 6 | } 7 | 8 | func New() *X25 { 9 | x := &X25{} 10 | x.Reset() 11 | return x 12 | } 13 | 14 | func (x *X25) WriteByte(b byte) error { 15 | tmp := b ^ byte(x.crc&0xff) 16 | tmp ^= (tmp << 4) 17 | x.crc = (x.crc >> 8) ^ (uint16(tmp) << 8) ^ (uint16(tmp) << 3) ^ (uint16(tmp) >> 4) 18 | return nil 19 | } 20 | 21 | func (x *X25) Write(p []byte) (n int, err error) { 22 | for _, b := range p { 23 | x.WriteByte(b) 24 | } 25 | return len(p), nil 26 | } 27 | 28 | func (x *X25) Sum16() uint16 { return x.crc } 29 | 30 | func (x *X25) Sum(in []byte) []byte { 31 | s := x.Sum16() 32 | return append(in, byte(s>>8), byte(s)) 33 | } 34 | 35 | func (x *X25) Size() int { return 2 } 36 | 37 | func (x *X25) BlockSize() int { return 1 } 38 | 39 | func (x *X25) Reset() { x.crc = 0xffff } 40 | -------------------------------------------------------------------------------- /mavlink/dialect_test.go: -------------------------------------------------------------------------------- 1 | package mavlink 2 | 3 | import ( 4 | "testing" 5 | ) 6 | 7 | func TestAddRemove(t *testing.T) { 8 | 9 | ds := DialectSlice{DialectCommon} 10 | 11 | // verify initial state 12 | if len(ds) != 1 { 13 | t.Error("bad len after remove") 14 | } 15 | if ds.IndexOf(DialectCommon) != 0 { 16 | t.Error("couldn't find dialect") 17 | } 18 | 19 | // verify addition 20 | ds.Add(DialectArdupilotmega) 21 | if len(ds) != 2 { 22 | t.Error("bad len after add") 23 | } 24 | if ds.IndexOf(DialectArdupilotmega) != 1 { 25 | t.Error("couldn't find dialect") 26 | } 27 | 28 | // verify removal 29 | ds.Remove(DialectCommon) 30 | if len(ds) != 1 { 31 | t.Error("bad len after remove") 32 | } 33 | if ds.IndexOf(DialectCommon) >= 0 { 34 | t.Error("wrong dialect") 35 | } 36 | if ds.IndexOf(DialectArdupilotmega) != 0 { 37 | t.Error("wrong dialect") 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /examples/udprx/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | import ( 4 | "flag" 5 | "log" 6 | "net" 7 | 8 | "github.com/liamstask/go-mavlink/mavlink" 9 | ) 10 | 11 | ////////////////////////////////////// 12 | // 13 | // mavlink udp rx example 14 | // 15 | // listen to ardupilot SITL and prints received msgs, more info: 16 | // http://dev.ardupilot.com/wiki/simulation-2/sitl-simulator-software-in-the-loop/setting-up-sitl-on-linux/ 17 | // 18 | // run via `go run main.go` 19 | // 20 | ////////////////////////////////////// 21 | 22 | var rxaddr = flag.String("addr", ":14550", "address to listen on") 23 | 24 | func main() { 25 | 26 | flag.Parse() 27 | 28 | listenAndServe(*rxaddr) 29 | } 30 | 31 | func listenAndServe(addr string) { 32 | 33 | udpAddr, err := net.ResolveUDPAddr("udp", addr) 34 | if err != nil { 35 | log.Fatal(err) 36 | } 37 | 38 | conn, listenerr := net.ListenUDP("udp", udpAddr) 39 | if listenerr != nil { 40 | log.Fatal(listenerr) 41 | } 42 | 43 | log.Println("listening on", udpAddr) 44 | 45 | dec := mavlink.NewDecoder(conn) 46 | dec.Dialects.Add(mavlink.DialectArdupilotmega) 47 | 48 | for { 49 | pkt, err := dec.Decode() 50 | if err != nil { 51 | log.Println("Decode fail:", err) 52 | if pkt != nil { 53 | log.Println(*pkt) 54 | } 55 | continue 56 | } 57 | 58 | log.Println("msg rx:", pkt.MsgID, pkt.Payload) 59 | } 60 | } 61 | -------------------------------------------------------------------------------- /mavgen/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | import ( 4 | "flag" 5 | "log" 6 | "os" 7 | "path/filepath" 8 | "strings" 9 | ) 10 | 11 | var ( 12 | infile = flag.String("f", "", "mavlink definition file input") 13 | outfile = flag.String("o", "", "output file name; default input.go") 14 | ) 15 | 16 | func main() { 17 | 18 | log.SetFlags(0) 19 | log.SetPrefix("mavgen: ") 20 | flag.Parse() 21 | 22 | fin, err := os.Open(*infile) 23 | if err != nil { 24 | log.Fatal("couldn't open input: ", err) 25 | } 26 | defer fin.Close() 27 | 28 | d, err := ParseDialect(fin, baseName(*infile)) 29 | if err != nil { 30 | log.Fatal("parse fail: ", err) 31 | } 32 | 33 | fout, err := os.Create(findOutFile()) 34 | if err != nil { 35 | log.Fatal("couldn't open output: ", err) 36 | } 37 | defer fout.Close() 38 | 39 | if err := d.GenerateGo(fout); err != nil { 40 | log.Fatal("couldn't write to output: ", err) 41 | } 42 | } 43 | 44 | // helper to remove the extension from the base name 45 | func baseName(s string) string { 46 | return strings.TrimSuffix(filepath.Base(s), filepath.Ext(s)) 47 | } 48 | 49 | func findOutFile() string { 50 | if *outfile == "" { 51 | *outfile = strings.ToLower(baseName(*infile)) + ".go" 52 | } 53 | 54 | dir, err := os.Getwd() 55 | if err != nil { 56 | log.Fatal("Getwd(): ", err) 57 | } 58 | 59 | return filepath.Join(dir, strings.ToLower(*outfile)) 60 | } 61 | -------------------------------------------------------------------------------- /mavlink/dialect.go: -------------------------------------------------------------------------------- 1 | package mavlink 2 | 3 | // Dialect represents a set of message definitions. 4 | // Some dialects have conflicting definitions for given message IDs, 5 | // so a list of dialects must be provided to an Encoder/Decoder in 6 | // order to specify which packets to use for the conflicting IDs. 7 | // 8 | // The 'DialectCommon' dialect is added to all Encoders/Decoders by default. 9 | type Dialect struct { 10 | Name string 11 | crcExtras map[uint8]uint8 12 | } 13 | 14 | // Alias for a slice of Dialect pointers 15 | // Only really intended to be accessed as a field on Encoder/Decoder 16 | type DialectSlice []*Dialect 17 | 18 | // look up the crcextra for msgid 19 | func (ds *DialectSlice) findCrcX(msgid uint8) (uint8, error) { 20 | 21 | // http://www.mavlink.org/mavlink/crc_extra_calculation 22 | for _, d := range *ds { 23 | if crcx, ok := d.crcExtras[msgid]; ok { 24 | return crcx, nil 25 | } 26 | } 27 | 28 | return 0, ErrUnknownMsgID 29 | } 30 | 31 | // IndexOf returns the index of d or -1 if not found 32 | func (ds *DialectSlice) IndexOf(d *Dialect) int { 33 | for i, dlct := range *ds { 34 | if d.Name == dlct.Name { 35 | return i 36 | } 37 | } 38 | return -1 39 | } 40 | 41 | // Add appends d if not already present in ds 42 | func (ds *DialectSlice) Add(d *Dialect) { 43 | if ds.IndexOf(d) < 0 { 44 | *ds = append(*ds, d) 45 | } 46 | } 47 | 48 | // Remove removes d if present in ds 49 | func (ds *DialectSlice) Remove(d *Dialect) { 50 | if i := ds.IndexOf(d); i >= 0 { 51 | // https://github.com/golang/go/wiki/SliceTricks 52 | (*ds)[len(*ds)-1], *ds = nil, append((*ds)[:i], (*ds)[i+1:]...) 53 | } 54 | } 55 | -------------------------------------------------------------------------------- /mavgen/mavgen_test.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | import ( 4 | "testing" 5 | ) 6 | 7 | func TestTypeConversions(t *testing.T) { 8 | 9 | cases := []struct { 10 | in string 11 | name string 12 | bitsz, arraylen int 13 | }{ 14 | {"char", "byte", 8, 0}, 15 | {"uint8_t", "uint8", 8, 0}, 16 | {"uint16_t", "uint16", 16, 0}, 17 | {"uint32_t", "uint32", 32, 0}, 18 | {"uint64_t", "uint64", 64, 0}, 19 | {"float", "float32", 32, 0}, 20 | {"double", "float64", 64, 0}, 21 | {"char[10]", "[10]byte", 8, 10}, 22 | {"float[30]", "[30]float32", 32, 30}, 23 | } 24 | 25 | for _, c := range cases { 26 | name, bitsz, arraylen, err := GoTypeInfo(c.in) 27 | // XXX: should test some cases that generate errors... 28 | if err != nil { 29 | t.Error("Type conversion err:", err) 30 | } 31 | if name != c.name { 32 | t.Errorf("Type Conversion for %q, got name %q, want %q", c.in, name, c.name) 33 | } 34 | if bitsz != c.bitsz { 35 | t.Errorf("Type Conversion for %q, got bitsz %q, want %q", c.in, bitsz, c.bitsz) 36 | } 37 | if arraylen != c.arraylen { 38 | t.Errorf("Type Conversion for %q, got arraylen %q, want %q", c.in, arraylen, c.arraylen) 39 | } 40 | } 41 | } 42 | 43 | func TestNameConversion(t *testing.T) { 44 | 45 | cases := []struct{ in, want string }{ 46 | {"test", "Test"}, 47 | {"_test_", "Test"}, 48 | {"_test", "Test"}, 49 | {"test_", "Test"}, 50 | {"test_thing", "TestThing"}, 51 | {"test_thing_", "TestThing"}, 52 | {"TEST_THING", "TestThing"}, 53 | {"_TEST_", "Test"}, 54 | {"_TEST___THiNG__", "TestThing"}, 55 | {"_TEST___THiNG_A__", "TestThingA"}, 56 | } 57 | 58 | for _, c := range cases { 59 | got := UpperCamelCase(c.in) 60 | if got != c.want { 61 | t.Errorf("Upper Camel Conversion for %q, got %q, want %q", c.in, got, c.want) 62 | } 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /mavlink/definitions/test.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 3 4 | 5 | 6 | Test all field types 7 | char 8 | string 9 | uint8_t 10 | uint16_t 11 | uint32_t 12 | uint64_t 13 | int8_t 14 | int16_t 15 | int32_t 16 | int64_t 17 | float 18 | double 19 | uint8_t_array 20 | uint16_t_array 21 | uint32_t_array 22 | uint64_t_array 23 | int8_t_array 24 | int16_t_array 25 | int32_t_array 26 | int64_t_array 27 | float_array 28 | double_array 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /mavlink/message_test.go: -------------------------------------------------------------------------------- 1 | package mavlink 2 | 3 | import ( 4 | "bytes" 5 | "testing" 6 | ) 7 | 8 | func TestRoundTrip(t *testing.T) { 9 | 10 | cases := []struct{ seq uint32 }{ 11 | {12345}, 12 | } 13 | 14 | for _, c := range cases { 15 | p := Ping{ 16 | Seq: c.seq, 17 | } 18 | 19 | var pkt Packet 20 | if err := p.Pack(&pkt); err != nil { 21 | t.Errorf("Pack fail %q (%q)", pkt, err) 22 | } 23 | 24 | var buf bytes.Buffer 25 | 26 | if err := NewEncoder(&buf).EncodePacket(&pkt); err != nil { 27 | t.Errorf("Encode fail %q", err) 28 | } 29 | 30 | pktOut, err := NewDecoder(&buf).Decode() 31 | if err != nil { 32 | t.Errorf("Decode fail %q", err) 33 | } 34 | 35 | if pktOut.MsgID != MSG_ID_PING { 36 | t.Errorf("MsgID fail, want %q, got %q", MSG_ID_PING, pktOut.MsgID) 37 | } 38 | 39 | var pingOut Ping 40 | if err := pingOut.Unpack(pktOut); err != nil { 41 | t.Errorf("Unpack fail %q", err) 42 | } 43 | 44 | if pingOut.Seq != c.seq { 45 | t.Errorf("Mismatch msg field, got %q, want %q", pingOut.Seq, c.seq) 46 | } 47 | } 48 | } 49 | 50 | func TestDecode(t *testing.T) { 51 | // decode a known good byte stream 52 | pktbytes := []byte{0xfe, 0x09, 0x0, 0x01, 0xC8, 0x00, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x5A, 0x3E} 53 | _, err := NewDecoder(bytes.NewBuffer(pktbytes)).Decode() 54 | if err != nil { 55 | t.Errorf("Decode fail:", err) 56 | } 57 | } 58 | 59 | func TestDialects(t *testing.T) { 60 | 61 | var buf bytes.Buffer 62 | 63 | enc := NewEncoder(&buf) 64 | dec := NewDecoder(&buf) 65 | 66 | // try to encode an ardupilot msg before we've added that dialect, 67 | // ensure it fails as expected 68 | mi := &Meminfo{ 69 | Brkval: 1000, 70 | Freemem: 10, 71 | } 72 | 73 | err := enc.Encode(0x1, 0x1, mi) 74 | if err != ErrUnknownMsgID { 75 | t.Errorf("encode expected ErrUnknownMsgID, got %q", err) 76 | } 77 | 78 | buf.Reset() 79 | 80 | // add the dialect, and ensure it succeeds 81 | enc.Dialects.Add(DialectArdupilotmega) 82 | if err = enc.Encode(0x1, 0x1, mi); err != nil { 83 | t.Errorf("Encode fail %q", err) 84 | } 85 | 86 | _, err = NewDecoder(&buf).Decode() 87 | if err != ErrUnknownMsgID { 88 | t.Errorf("decode expected ErrUnknownMsgID, got %q", err) 89 | } 90 | 91 | dec.Dialects.Add(DialectArdupilotmega) 92 | 93 | // re-encode the msg, and decode it again after adding the required dialect 94 | if err = enc.Encode(0x1, 0x1, mi); err != nil { 95 | t.Errorf("Encode fail %q", err) 96 | } 97 | 98 | pktOut, err := dec.Decode() 99 | if err != nil { 100 | t.Errorf("Decode fail %q", err) 101 | } 102 | 103 | // make sure the output matches our original input for good measure 104 | var miOut Meminfo 105 | if err := miOut.Unpack(pktOut); err != nil { 106 | t.Errorf("Unpack fail %q", err) 107 | } 108 | 109 | if miOut.Brkval != mi.Brkval || miOut.Freemem != mi.Freemem { 110 | t.Errorf("Round trip fail") 111 | } 112 | } 113 | -------------------------------------------------------------------------------- /ReadMe.md: -------------------------------------------------------------------------------- 1 | # go-mavlink 2 | 3 | a [Go](http://golang.org/) package for reading/writing [mavlink](http://qgroundcontrol.org/mavlink/start) messages. 4 | 5 | [![Build Status](https://travis-ci.org/liamstask/go-mavlink.svg)](https://travis-ci.org/liamstask/go-mavlink) [![GoDoc](https://godoc.org/github.com/liamstask/go-mavlink/mavlink?status.svg)](https://godoc.org/github.com/liamstask/go-mavlink/mavlink) 6 | 7 | still under development, not necessarily ready for production use 8 | 9 | ## installation 10 | 11 | * grab the files: `go get github.com/liamstask/go-mavlink` 12 | * generate classes from mavlink xml definitions: `go generate` 13 | 14 | xml definitions can be updated from https://github.com/mavlink/mavlink 15 | 16 | ## usage 17 | 18 | package `go-mavlink/mavgen` is used only for generating classes from mavlink xml definitions, it is not generally intended to be used by anything other than the `go generate` command. 19 | 20 | package `go-mavlink/mavlink` is the main package used for encoding/decoding mavlink messages. 21 | 22 | `mavlink.Packet` is the struct that gets sent over the wire, and `mavlink.Message` is an interface implemented by all mavlink classes generated by `mavgen`. 23 | 24 | ### dialects 25 | 26 | Several mavlink dialects define messages for overlapping msg ids, so it is required to specify which dialects you'd like to use. `DialectCommon` is included by most of the available dialects, so is included by default. 27 | 28 | ```go 29 | dec := mavlink.NewDecoder(rdr) 30 | dec.Dialects.Add(DialectArdupilotmega) 31 | ``` 32 | 33 | Existing dialects are: 34 | * DialectArdupilotmega 35 | * DialectAsluav 36 | * DialectCommon (added to Encoders/Decoders by default, can be removed if desired) 37 | * DialectMatrixpilot 38 | * DialectPixhawk 39 | * DialectUalberta 40 | 41 | ### decode 42 | 43 | a typical decode loop might look like: 44 | 45 | ```go 46 | rdr := SomeIoReader() // any io.Reader: UDP, serial port, bytes.Buffer, etc 47 | dec := mavlink.NewDecoder(rdr) 48 | 49 | for { 50 | pkt, err := dec.Decode() 51 | if err != nil { 52 | log.Fatal("Decode fail:", err) 53 | } 54 | 55 | // handle packet types you're interested in... 56 | switch pkt.MsgID { 57 | case mavlink.MSG_ID_PARAM_VALUE: 58 | var pv mavlink.ParamValue 59 | if err := pv.Unpack(pkt); err == nil { 60 | // handle param value 61 | } 62 | } 63 | } 64 | ``` 65 | 66 | ### encode 67 | 68 | the most convenient way to encode is to use `Encoder.Encode()`: 69 | 70 | ```go 71 | w := SomeIoWriter() // any io.Writer: UDP, serial port, bytes.Buffer, etc 72 | enc := mavlink.NewEncoder(w) 73 | 74 | p := mavlink.Ping{ 75 | Seq: 12345, 76 | } 77 | 78 | if err := enc.Encode(0x1, 0x1, &p); err != nil { 79 | log.Fatal("Encode fail:", err) 80 | } 81 | ``` 82 | 83 | if for some reason you need to pass a `Packet` directly, use `Encoder.EncodePacket()`: 84 | 85 | ```go 86 | p := mavlink.Ping{ 87 | Seq: c.seq, 88 | } 89 | 90 | var pkt Packet 91 | if err := p.Pack(&pkt); err != nil { 92 | log.Fatal("Pack fail:", err) 93 | } 94 | 95 | if err := enc.EncodePacket(&pkt); err != nil { 96 | log.Fatal("Encode fail:", err) 97 | } 98 | ``` 99 | -------------------------------------------------------------------------------- /mavlink/definitions/python_array_test.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | common.xml 5 | 6 | 7 | Array test #0. 8 | Stub field 9 | Value array 10 | Value array 11 | Value array 12 | Value array 13 | 14 | 15 | Array test #1. 16 | Value array 17 | 18 | 19 | Array test #3. 20 | Stub field 21 | Value array 22 | 23 | 24 | Array test #4. 25 | Value array 26 | Stub field 27 | 28 | 29 | Array test #5. 30 | Value array 31 | Value array 32 | 33 | 34 | Array test #6. 35 | Stub field 36 | Stub field 37 | Stub field 38 | Value array 39 | Value array 40 | Value array 41 | Value array 42 | Value array 43 | Value array 44 | Value array 45 | Value array 46 | Value array 47 | 48 | 49 | Array test #7. 50 | Value array 51 | Value array 52 | Value array 53 | Value array 54 | Value array 55 | Value array 56 | Value array 57 | Value array 58 | Value array 59 | 60 | 61 | Array test #8. 62 | Stub field 63 | Value array 64 | Value array 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /mavlink/definitions/ualberta.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | common.xml 4 | 5 | 6 | Available autopilot modes for ualberta uav 7 | 8 | Raw input pulse widts sent to output 9 | 10 | 11 | Inputs are normalized using calibration, the converted back to raw pulse widths for output 12 | 13 | 14 | dfsdfs 15 | 16 | 17 | dfsfds 18 | 19 | 20 | dfsdfsdfs 21 | 22 | 23 | 24 | Navigation filter mode 25 | 26 | 27 | AHRS mode 28 | 29 | 30 | INS/GPS initialization mode 31 | 32 | 33 | INS/GPS mode 34 | 35 | 36 | 37 | Mode currently commanded by pilot 38 | 39 | sdf 40 | 41 | 42 | dfs 43 | 44 | 45 | Rotomotion mode 46 | 47 | 48 | 49 | 50 | 51 | Accelerometer and Gyro biases from the navigation filter 52 | Timestamp (microseconds) 53 | b_f[0] 54 | b_f[1] 55 | b_f[2] 56 | b_f[0] 57 | b_f[1] 58 | b_f[2] 59 | 60 | 61 | Complete set of calibration parameters for the radio 62 | Aileron setpoints: left, center, right 63 | Elevator setpoints: nose down, center, nose up 64 | Rudder setpoints: nose left, center, nose right 65 | Tail gyro mode/gain setpoints: heading hold, rate mode 66 | Pitch curve setpoints (every 25%) 67 | Throttle curve setpoints (every 25%) 68 | 69 | 70 | System status specific to ualberta uav 71 | System mode, see UALBERTA_AUTOPILOT_MODE ENUM 72 | Navigation mode, see UALBERTA_NAV_MODE ENUM 73 | Pilot mode, see UALBERTA_PILOT_MODE 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /mavlink/message.go: -------------------------------------------------------------------------------- 1 | package mavlink 2 | 3 | import ( 4 | "bufio" 5 | "errors" 6 | "io" 7 | 8 | "github.com/liamstask/go-mavlink/x25" 9 | ) 10 | 11 | //go:generate mavgen -f definitions/common.xml 12 | //go:generate mavgen -f definitions/ardupilotmega.xml 13 | //go:generate mavgen -f definitions/ASLUAV.xml 14 | //go:generate mavgen -f definitions/matrixpilot.xml 15 | //go:generate mavgen -f definitions/pixhawk.xml 16 | //go:generate mavgen -f definitions/ualberta.xml 17 | 18 | const ( 19 | startByte = 0xfe 20 | numChecksumBytes = 2 21 | hdrLen = 6 22 | ) 23 | 24 | var ( 25 | ErrUnknownMsgID = errors.New("unknown msg id") 26 | ErrCrcFail = errors.New("checksum did not match") 27 | ) 28 | 29 | // basic type for encoding/decoding mavlink messages. 30 | // use the Pack() and Unpack() routines on specific message 31 | // types to convert them to/from the Packet type. 32 | type Message interface { 33 | Pack(*Packet) error 34 | Unpack(*Packet) error 35 | } 36 | 37 | // wire type for encoding/decoding mavlink messages. 38 | // use the ToPacket() and FromPacket() routines on specific message 39 | // types to convert them to/from the Message type. 40 | type Packet struct { 41 | SeqID uint8 // Sequence of packet 42 | SysID uint8 // ID of message sender system/aircraft 43 | CompID uint8 // ID of the message sender component 44 | MsgID uint8 // ID of message in payload 45 | Payload []byte 46 | Checksum uint16 47 | } 48 | 49 | type Decoder struct { 50 | CurrSeqID uint8 // last seq id decoded 51 | Dialects DialectSlice // dialects that can be decoded 52 | br *bufio.Reader 53 | } 54 | 55 | type Encoder struct { 56 | CurrSeqID uint8 // last seq id encoded 57 | Dialects DialectSlice // dialects that can be encoded 58 | bw *bufio.Writer 59 | } 60 | 61 | func NewDecoder(r io.Reader) *Decoder { 62 | d := &Decoder{ 63 | Dialects: DialectSlice{DialectCommon}, 64 | } 65 | 66 | if v, ok := r.(*bufio.Reader); ok { 67 | d.br = v 68 | } else { 69 | d.br = bufio.NewReader(r) 70 | } 71 | 72 | return d 73 | } 74 | 75 | func NewEncoder(w io.Writer) *Encoder { 76 | 77 | e := &Encoder{ 78 | Dialects: DialectSlice{DialectCommon}, 79 | } 80 | 81 | if v, ok := w.(*bufio.Writer); ok { 82 | e.bw = v 83 | } else { 84 | e.bw = bufio.NewWriter(w) 85 | } 86 | 87 | return e 88 | } 89 | 90 | // helper to create packet w/header populated with received bytes 91 | func newPacketFromBytes(b []byte) (*Packet, int) { 92 | return &Packet{ 93 | SeqID: b[1], 94 | SysID: b[2], 95 | CompID: b[3], 96 | MsgID: b[4], 97 | }, int(b[0]) 98 | } 99 | 100 | // Decoder reads and parses from its reader 101 | // Typically, the caller will check the p.MsgID to see if it's 102 | // a message they're interested in, and convert it to the 103 | // corresponding type via Message.FromPacket() 104 | func (dec *Decoder) Decode() (*Packet, error) { 105 | 106 | // discard bytes until our start byte 107 | for { 108 | c, err := dec.br.ReadByte() 109 | if err != nil { 110 | return nil, err 111 | } 112 | if c == startByte { 113 | break 114 | } 115 | } 116 | 117 | // hdr contains LENGTH, SEQ, SYSID, COMPID, MSGID 118 | hdr := make([]byte, 5) 119 | if _, err := io.ReadFull(dec.br, hdr); err != nil { 120 | return nil, err 121 | } 122 | 123 | p, payloadLen := newPacketFromBytes(hdr) 124 | 125 | crc := x25.New() 126 | crc.Write(hdr) 127 | 128 | // read payload (if there is one) and checksum bytes 129 | buf := make([]byte, payloadLen+numChecksumBytes) 130 | if _, err := io.ReadFull(dec.br, buf); err != nil { 131 | return p, err 132 | } 133 | 134 | p.Payload = buf[:payloadLen] 135 | crc.Write(p.Payload) 136 | 137 | crcx, err := dec.Dialects.findCrcX(p.MsgID) 138 | if err != nil { 139 | return p, err 140 | } 141 | crc.WriteByte(crcx) 142 | 143 | p.Checksum = bytesToU16(buf[payloadLen:]) 144 | 145 | // does the transmitted checksum match our computed checksum? 146 | if p.Checksum != crc.Sum16() { 147 | return p, ErrCrcFail 148 | } 149 | 150 | dec.CurrSeqID = p.SeqID 151 | return p, nil 152 | } 153 | 154 | // Decode a packet from a previously received buffer (such as a UDP packet), 155 | // b must contain a complete message 156 | func (dec *Decoder) DecodeBytes(b []byte) (*Packet, error) { 157 | 158 | if len(b) < hdrLen || b[0] != startByte { 159 | return nil, errors.New("invalid header") 160 | } 161 | 162 | p, payloadLen := newPacketFromBytes(b[1:]) 163 | 164 | crc := x25.New() 165 | p.Payload = b[hdrLen: hdrLen+payloadLen] 166 | crc.Write(b[1:hdrLen+payloadLen]) 167 | 168 | crcx, err := dec.Dialects.findCrcX(p.MsgID) 169 | if err != nil { 170 | return p, err 171 | } 172 | crc.WriteByte(crcx) 173 | 174 | p.Checksum = bytesToU16(b[hdrLen+payloadLen:]) 175 | 176 | // does the transmitted checksum match our computed checksum? 177 | if p.Checksum != crc.Sum16() { 178 | return p, ErrCrcFail 179 | } 180 | 181 | dec.CurrSeqID = p.SeqID 182 | return p, nil 183 | } 184 | 185 | // helper that accepts a Message, internally converts it to a Packet, 186 | // sets the Packet's SeqID based on the 187 | // and then writes it to its writer via EncodePacket() 188 | func (enc *Encoder) Encode(sysID, compID uint8, m Message) error { 189 | var p Packet 190 | if err := m.Pack(&p); err != nil { 191 | return err 192 | } 193 | 194 | p.SysID, p.CompID = sysID, compID 195 | 196 | return enc.EncodePacket(&p) 197 | } 198 | 199 | // Encode writes p to its writer 200 | func (enc *Encoder) EncodePacket(p *Packet) error { 201 | 202 | crc := x25.New() 203 | 204 | // header 205 | hdr := []byte{startByte, byte(len(p.Payload)), enc.CurrSeqID, p.SysID, p.CompID, p.MsgID} 206 | if err := enc.writeAndCheck(hdr); err != nil { 207 | return err 208 | } 209 | crc.Write(hdr[1:]) // don't include start byte 210 | 211 | // payload 212 | if err := enc.writeAndCheck(p.Payload); err != nil { 213 | return err 214 | } 215 | crc.Write(p.Payload) 216 | 217 | // crc extra 218 | crcx, err := enc.Dialects.findCrcX(p.MsgID) 219 | if err != nil { 220 | return err 221 | } 222 | crc.WriteByte(crcx) 223 | 224 | // crc 225 | crcBytes := u16ToBytes(crc.Sum16()) 226 | if err := enc.writeAndCheck(crcBytes); err != nil { 227 | return err 228 | } 229 | 230 | err = enc.bw.Flush() 231 | if err == nil { 232 | enc.CurrSeqID++ 233 | } 234 | 235 | return err 236 | } 237 | 238 | // helper to check both the write and writelen status 239 | func (enc *Encoder) writeAndCheck(p []byte) error { 240 | n, err := enc.bw.Write(p) 241 | if err == nil && n != len(p) { 242 | return io.ErrShortWrite 243 | } 244 | 245 | return err 246 | } 247 | 248 | func u16ToBytes(v uint16) []byte { 249 | return []byte{byte(v & 0xff), byte(v >> 8)} 250 | } 251 | 252 | func bytesToU16(p []byte) uint16 { 253 | // NB: does not check size of p 254 | return (uint16(p[1]) << 8) | uint16(p[0]) 255 | } 256 | -------------------------------------------------------------------------------- /mavlink/ualberta.go: -------------------------------------------------------------------------------- 1 | package mavlink 2 | 3 | import ( 4 | "encoding/binary" 5 | "fmt" 6 | "math" 7 | ) 8 | 9 | ////////////////////////////////////////////////// 10 | // 11 | // NOTE: do not edit, 12 | // this file created automatically by mavgen.go 13 | // 14 | ////////////////////////////////////////////////// 15 | 16 | // UalbertaAutopilotMode: Available autopilot modes for ualberta uav 17 | const ( 18 | MODE_MANUAL_DIRECT = 0 // Raw input pulse widts sent to output 19 | MODE_MANUAL_SCALED = 1 // Inputs are normalized using calibration, the converted back to raw pulse widths for output 20 | MODE_AUTO_PID_ATT = 2 // dfsdfs 21 | MODE_AUTO_PID_VEL = 3 // dfsfds 22 | MODE_AUTO_PID_POS = 4 // dfsdfsdfs 23 | ) 24 | 25 | // UalbertaNavMode: Navigation filter mode 26 | const ( 27 | NAV_AHRS_INIT = 0 // 28 | NAV_AHRS = 1 // AHRS mode 29 | NAV_INS_GPS_INIT = 2 // INS/GPS initialization mode 30 | NAV_INS_GPS = 3 // INS/GPS mode 31 | ) 32 | 33 | // UalbertaPilotMode: Mode currently commanded by pilot 34 | const ( 35 | PILOT_MANUAL = 0 // sdf 36 | PILOT_AUTO = 1 // dfs 37 | PILOT_ROTO = 2 // Rotomotion mode 38 | ) 39 | 40 | // Accelerometer and Gyro biases from the navigation filter 41 | type NavFilterBias struct { 42 | Usec uint64 // Timestamp (microseconds) 43 | Accel0 float32 // b_f[0] 44 | Accel1 float32 // b_f[1] 45 | Accel2 float32 // b_f[2] 46 | Gyro0 float32 // b_f[0] 47 | Gyro1 float32 // b_f[1] 48 | Gyro2 float32 // b_f[2] 49 | } 50 | 51 | func (self *NavFilterBias) MsgID() uint8 { 52 | return 220 53 | } 54 | 55 | func (self *NavFilterBias) MsgName() string { 56 | return "NavFilterBias" 57 | } 58 | 59 | func (self *NavFilterBias) Pack(p *Packet) error { 60 | payload := make([]byte, 32) 61 | binary.LittleEndian.PutUint64(payload[0:], uint64(self.Usec)) 62 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.Accel0)) 63 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.Accel1)) 64 | binary.LittleEndian.PutUint32(payload[16:], math.Float32bits(self.Accel2)) 65 | binary.LittleEndian.PutUint32(payload[20:], math.Float32bits(self.Gyro0)) 66 | binary.LittleEndian.PutUint32(payload[24:], math.Float32bits(self.Gyro1)) 67 | binary.LittleEndian.PutUint32(payload[28:], math.Float32bits(self.Gyro2)) 68 | 69 | p.MsgID = self.MsgID() 70 | p.Payload = payload 71 | return nil 72 | } 73 | 74 | func (self *NavFilterBias) Unpack(p *Packet) error { 75 | if len(p.Payload) < 32 { 76 | return fmt.Errorf("payload too small") 77 | } 78 | self.Usec = uint64(binary.LittleEndian.Uint64(p.Payload[0:])) 79 | self.Accel0 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 80 | self.Accel1 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 81 | self.Accel2 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[16:])) 82 | self.Gyro0 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[20:])) 83 | self.Gyro1 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[24:])) 84 | self.Gyro2 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[28:])) 85 | return nil 86 | } 87 | 88 | // Complete set of calibration parameters for the radio 89 | type RadioCalibration struct { 90 | Aileron [3]uint16 // Aileron setpoints: left, center, right 91 | Elevator [3]uint16 // Elevator setpoints: nose down, center, nose up 92 | Rudder [3]uint16 // Rudder setpoints: nose left, center, nose right 93 | Gyro [2]uint16 // Tail gyro mode/gain setpoints: heading hold, rate mode 94 | Pitch [5]uint16 // Pitch curve setpoints (every 25%) 95 | Throttle [5]uint16 // Throttle curve setpoints (every 25%) 96 | } 97 | 98 | func (self *RadioCalibration) MsgID() uint8 { 99 | return 221 100 | } 101 | 102 | func (self *RadioCalibration) MsgName() string { 103 | return "RadioCalibration" 104 | } 105 | 106 | func (self *RadioCalibration) Pack(p *Packet) error { 107 | payload := make([]byte, 42) 108 | for i, v := range self.Aileron { 109 | binary.LittleEndian.PutUint16(payload[0+i*2:], uint16(v)) 110 | } 111 | for i, v := range self.Elevator { 112 | binary.LittleEndian.PutUint16(payload[6+i*2:], uint16(v)) 113 | } 114 | for i, v := range self.Rudder { 115 | binary.LittleEndian.PutUint16(payload[12+i*2:], uint16(v)) 116 | } 117 | for i, v := range self.Gyro { 118 | binary.LittleEndian.PutUint16(payload[18+i*2:], uint16(v)) 119 | } 120 | for i, v := range self.Pitch { 121 | binary.LittleEndian.PutUint16(payload[22+i*2:], uint16(v)) 122 | } 123 | for i, v := range self.Throttle { 124 | binary.LittleEndian.PutUint16(payload[32+i*2:], uint16(v)) 125 | } 126 | 127 | p.MsgID = self.MsgID() 128 | p.Payload = payload 129 | return nil 130 | } 131 | 132 | func (self *RadioCalibration) Unpack(p *Packet) error { 133 | if len(p.Payload) < 42 { 134 | return fmt.Errorf("payload too small") 135 | } 136 | for i := 0; i < len(self.Aileron); i++ { 137 | self.Aileron[i] = uint16(binary.LittleEndian.Uint16(p.Payload[0+i*2:])) 138 | } 139 | for i := 0; i < len(self.Elevator); i++ { 140 | self.Elevator[i] = uint16(binary.LittleEndian.Uint16(p.Payload[6+i*2:])) 141 | } 142 | for i := 0; i < len(self.Rudder); i++ { 143 | self.Rudder[i] = uint16(binary.LittleEndian.Uint16(p.Payload[12+i*2:])) 144 | } 145 | for i := 0; i < len(self.Gyro); i++ { 146 | self.Gyro[i] = uint16(binary.LittleEndian.Uint16(p.Payload[18+i*2:])) 147 | } 148 | for i := 0; i < len(self.Pitch); i++ { 149 | self.Pitch[i] = uint16(binary.LittleEndian.Uint16(p.Payload[22+i*2:])) 150 | } 151 | for i := 0; i < len(self.Throttle); i++ { 152 | self.Throttle[i] = uint16(binary.LittleEndian.Uint16(p.Payload[32+i*2:])) 153 | } 154 | return nil 155 | } 156 | 157 | // System status specific to ualberta uav 158 | type UalbertaSysStatus struct { 159 | Mode uint8 // System mode, see UALBERTA_AUTOPILOT_MODE ENUM 160 | NavMode uint8 // Navigation mode, see UALBERTA_NAV_MODE ENUM 161 | Pilot uint8 // Pilot mode, see UALBERTA_PILOT_MODE 162 | } 163 | 164 | func (self *UalbertaSysStatus) MsgID() uint8 { 165 | return 222 166 | } 167 | 168 | func (self *UalbertaSysStatus) MsgName() string { 169 | return "UalbertaSysStatus" 170 | } 171 | 172 | func (self *UalbertaSysStatus) Pack(p *Packet) error { 173 | payload := make([]byte, 3) 174 | payload[0] = byte(self.Mode) 175 | payload[1] = byte(self.NavMode) 176 | payload[2] = byte(self.Pilot) 177 | 178 | p.MsgID = self.MsgID() 179 | p.Payload = payload 180 | return nil 181 | } 182 | 183 | func (self *UalbertaSysStatus) Unpack(p *Packet) error { 184 | if len(p.Payload) < 3 { 185 | return fmt.Errorf("payload too small") 186 | } 187 | self.Mode = uint8(p.Payload[0]) 188 | self.NavMode = uint8(p.Payload[1]) 189 | self.Pilot = uint8(p.Payload[2]) 190 | return nil 191 | } 192 | 193 | // Message IDs 194 | const ( 195 | MSG_ID_NAV_FILTER_BIAS = 220 196 | MSG_ID_RADIO_CALIBRATION = 221 197 | MSG_ID_UALBERTA_SYS_STATUS = 222 198 | ) 199 | 200 | // DialectUalberta is the dialect represented by ualberta.xml 201 | var DialectUalberta *Dialect = &Dialect{ 202 | Name: "ualberta", 203 | crcExtras: map[uint8]uint8{ 204 | 220: 34, // MSG_ID_NAV_FILTER_BIAS 205 | 221: 71, // MSG_ID_RADIO_CALIBRATION 206 | 222: 15, // MSG_ID_UALBERTA_SYS_STATUS 207 | }, 208 | } 209 | -------------------------------------------------------------------------------- /mavlink/definitions/autoquad.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | common.xml 4 | 3 5 | 6 | 7 | Available operating modes/statuses for AutoQuad flight controller. 8 | Bitmask up to 32 bits. Low side bits for base modes, high side for 9 | additional active features/modifiers/constraints. 10 | 11 | System is initializing 12 | 13 | 14 | 15 | System is standing by, not active 16 | 17 | 18 | Stabilized, under full manual control 19 | 20 | 21 | Altitude hold engaged 22 | 23 | 24 | Position hold engaged 25 | 26 | 27 | Dynamic Velocity Hold is active 28 | 29 | 30 | Autonomous mission execution mode 31 | 32 | 33 | 34 | 35 | System is in failsafe recovery mode 36 | 37 | 38 | Automatic Return to Home is active 39 | 40 | 41 | Heading-Free locked mode active 42 | 43 | 44 | Heading-Free dynamic mode active 45 | 46 | 47 | Ceiling altitude is set 48 | 49 | 50 | Craft is at ceiling altitude 51 | 52 | 53 | 54 | 55 | 56 | Start/stop AutoQuad telemetry values stream. 57 | Start or stop (1 or 0) 58 | Stream frequency in us 59 | Dataset ID (refer to aq_mavlink.h::mavlinkCustomDataSets enum in AQ flight controller code) 60 | Empty 61 | Empty 62 | Empty 63 | Empty 64 | 65 | 66 | Command AutoQuad to go to a particular place at a set speed. 67 | Latitude 68 | Lontitude 69 | Altitude 70 | Speed 71 | Empty 72 | Empty 73 | Empty 74 | 75 | 76 | Request AutoQuad firmware version number. 77 | Empty 78 | Empty 79 | Empty 80 | Empty 81 | Empty 82 | Empty 83 | Empty 84 | 85 | 86 | 87 | 88 | 89 | Motor/ESC telemetry data. 90 | 91 | 92 | 93 | 94 | 95 | Sends up to 20 raw float values. 96 | Index of message 97 | value1 98 | value2 99 | value3 100 | value4 101 | value5 102 | value6 103 | value7 104 | value8 105 | value9 106 | value10 107 | value11 108 | value12 109 | value13 110 | value14 111 | value15 112 | value16 113 | value17 114 | value18 115 | value19 116 | value20 117 | 118 | 119 | Sends ESC32 telemetry data for up to 4 motors. Multiple messages may be sent in sequence when system has > 4 motors. Data is described as follows: 120 | // unsigned int state : 3; 121 | // unsigned int vin : 12; // x 100 122 | // unsigned int amps : 14; // x 100 123 | // unsigned int rpm : 15; 124 | // unsigned int duty : 8; // x (255/100) 125 | // - Data Version 2 - 126 | // unsigned int errors : 9; // Bad detects error count 127 | // - Data Version 3 - 128 | // unsigned int temp : 9; // (Deg C + 32) * 4 129 | // unsigned int errCode : 3; 130 | 131 | Timestamp of the component clock since boot time in ms. 132 | Sequence number of message (first set of 4 motors is #1, next 4 is #2, etc). 133 | Total number of active ESCs/motors on the system. 134 | Number of active ESCs in this sequence (1 through this many array members will be populated with data) 135 | ESC/Motor ID 136 | Age of each ESC telemetry reading in ms compared to boot time. A value of 0xFFFF means timeout/no data. 137 | Version of data structure (determines contents). 138 | Data bits 1-32 for each ESC. 139 | Data bits 33-64 for each ESC. 140 | 141 | 142 | 143 | -------------------------------------------------------------------------------- /mavlink/definitions/minimal.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 2 4 | 5 | 6 | Micro air vehicle / autopilot classes. This identifies the individual model. 7 | 8 | Generic autopilot, full support for everything 9 | 10 | 11 | PIXHAWK autopilot, http://pixhawk.ethz.ch 12 | 13 | 14 | SLUGS autopilot, http://slugsuav.soe.ucsc.edu 15 | 16 | 17 | ArduPilotMega / ArduCopter, http://diydrones.com 18 | 19 | 20 | OpenPilot, http://openpilot.org 21 | 22 | 23 | Generic autopilot only supporting simple waypoints 24 | 25 | 26 | Generic autopilot supporting waypoints and other simple navigation commands 27 | 28 | 29 | Generic autopilot supporting the full mission command set 30 | 31 | 32 | No valid autopilot, e.g. a GCS or other MAVLink component 33 | 34 | 35 | PPZ UAV - http://nongnu.org/paparazzi 36 | 37 | 38 | UAV Dev Board 39 | 40 | 41 | FlexiPilot 42 | 43 | 44 | 45 | 46 | Generic micro air vehicle. 47 | 48 | 49 | Fixed wing aircraft. 50 | 51 | 52 | Quadrotor 53 | 54 | 55 | Coaxial helicopter 56 | 57 | 58 | Normal helicopter with tail rotor. 59 | 60 | 61 | Ground installation 62 | 63 | 64 | Operator control unit / ground control station 65 | 66 | 67 | Airship, controlled 68 | 69 | 70 | Free balloon, uncontrolled 71 | 72 | 73 | Rocket 74 | 75 | 76 | Ground rover 77 | 78 | 79 | Surface vessel, boat, ship 80 | 81 | 82 | Submarine 83 | 84 | 85 | Hexarotor 86 | 87 | 88 | Octorotor 89 | 90 | 91 | Octorotor 92 | 93 | 94 | Flapping wing 95 | 96 | 97 | 98 | These flags encode the MAV mode. 99 | 100 | 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. 101 | 102 | 103 | 0b01000000 remote control input is enabled. 104 | 105 | 106 | 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. 107 | 108 | 109 | 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. 110 | 111 | 112 | 0b00001000 guided mode enabled, system flies MISSIONs / mission items. 113 | 114 | 115 | 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. 116 | 117 | 118 | 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. 119 | 120 | 121 | 0b00000001 Reserved for future use. 122 | 123 | 124 | 125 | These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. 126 | 127 | First bit: 10000000 128 | 129 | 130 | Second bit: 01000000 131 | 132 | 133 | Third bit: 00100000 134 | 135 | 136 | Fourth bit: 00010000 137 | 138 | 139 | Fifth bit: 00001000 140 | 141 | 142 | Sixt bit: 00000100 143 | 144 | 145 | Seventh bit: 00000010 146 | 147 | 148 | Eighth bit: 00000001 149 | 150 | 151 | 152 | 153 | Uninitialized system, state is unknown. 154 | 155 | 156 | System is booting up. 157 | 158 | 159 | System is calibrating and not flight-ready. 160 | 161 | 162 | System is grounded and on standby. It can be launched any time. 163 | 164 | 165 | System is active and might be already airborne. Motors are engaged. 166 | 167 | 168 | System is in a non-normal flight mode. It can however still navigate. 169 | 170 | 171 | System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. 172 | 173 | 174 | System just initialized its power-down sequence, will shut down now. 175 | 176 | 177 | 178 | 179 | 180 | The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). 181 | Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) 182 | Autopilot type / class. defined in MAV_AUTOPILOT ENUM 183 | System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h 184 | A bitfield for use for autopilot-specific flags. 185 | System status flag, see MAV_STATE ENUM 186 | MAVLink version 187 | 188 | 189 | 190 | -------------------------------------------------------------------------------- /mavlink/definitions/ASLUAV.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | common.xml 6 | 7 | 8 | 9 | 10 | Mission command to reset Maximum Power Point Tracker (MPPT) 11 | MPPT number 12 | Empty 13 | Empty 14 | Empty 15 | Empty 16 | Empty 17 | Empty 18 | 19 | 20 | Mission command to perform a power cycle on payload 21 | Complete power cycle 22 | VISensor power cycle 23 | Empty 24 | Empty 25 | Empty 26 | Empty 27 | Empty 28 | 29 | 30 | 31 | 32 | 33 | Voltage and current sensor data 34 | Power board voltage sensor reading in volts 35 | Power board current sensor reading in amps 36 | Board current sensor 1 reading in amps 37 | Board current sensor 2 reading in amps 38 | 39 | 40 | Maximum Power Point Tracker (MPPT) sensor data for solar module power performance tracking 41 | MPPT last timestamp 42 | MPPT1 voltage 43 | MPPT1 current 44 | MPPT1 pwm 45 | MPPT1 status 46 | MPPT2 voltage 47 | MPPT2 current 48 | MPPT2 pwm 49 | MPPT2 status 50 | MPPT3 voltage 51 | MPPT3 current 52 | MPPT3 pwm 53 | MPPT3 status 54 | 55 | 56 | ASL-fixed-wing controller data 57 | Timestamp 58 | ASLCTRL control-mode (manual, stabilized, auto, etc...) 59 | See sourcecode for a description of these values... 60 | 61 | 62 | Pitch angle [deg] 63 | Pitch angle reference[deg] 64 | 65 | 66 | 67 | 68 | 69 | 70 | Airspeed reference [m/s] 71 | 72 | Yaw angle [deg] 73 | Yaw angle reference[deg] 74 | Roll angle [deg] 75 | Roll angle reference[deg] 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | ASL-fixed-wing controller debug data 85 | Debug data 86 | Debug data 87 | Debug data 88 | Debug data 89 | Debug data 90 | Debug data 91 | Debug data 92 | Debug data 93 | Debug data 94 | Debug data 95 | Debug data 96 | 97 | 98 | Extended state information for ASLUAVs 99 | Status of the position-indicator LEDs 100 | Status of the IRIDIUM satellite communication system 101 | Status vector for up to 8 servos 102 | Motor RPM 103 | 104 | 105 | 106 | Extended EKF state estimates for ASLUAVs 107 | Time since system start [us] 108 | Magnitude of wind velocity (in lateral inertial plane) [m/s] 109 | Wind heading angle from North [rad] 110 | Z (Down) component of inertial wind velocity [m/s] 111 | Magnitude of air velocity [m/s] 112 | Sideslip angle [rad] 113 | Angle of attack [rad] 114 | 115 | 116 | Off-board controls/commands for ASLUAVs 117 | Time since system start [us] 118 | Elevator command [~] 119 | Throttle command [~] 120 | Throttle 2 command [~] 121 | Left aileron command [~] 122 | Right aileron command [~] 123 | Rudder command [~] 124 | Off-board computer status 125 | 126 | 127 | Atmospheric sensors (temperature, humidity, ...) 128 | Ambient temperature [degrees Celsius] 129 | Relative humidity [%] 130 | 131 | 132 | Battery pack monitoring data for Li-Ion batteries 133 | Battery pack temperature in [deg C] 134 | Battery pack voltage in [mV] 135 | Battery pack current in [mA] 136 | Battery pack state-of-charge 137 | Battery monitor status report bits in Hex 138 | Battery monitor serial number in Hex 139 | Battery monitor sensor host FET control in Hex 140 | Battery pack cell 1 voltage in [mV] 141 | Battery pack cell 2 voltage in [mV] 142 | Battery pack cell 3 voltage in [mV] 143 | Battery pack cell 4 voltage in [mV] 144 | Battery pack cell 5 voltage in [mV] 145 | Battery pack cell 6 voltage in [mV] 146 | 147 | 148 | Fixed-wing soaring (i.e. thermal seeking) data 149 | Timestamp [ms] 150 | Timestamp since last mode change[ms] 151 | Updraft speed at current/local airplane position [m/s] 152 | Thermal core updraft strength [m/s] 153 | Thermal radius [m] 154 | Thermal center latitude [deg] 155 | Thermal center longitude [deg] 156 | Variance W 157 | Variance R 158 | Variance Lat 159 | Variance Lon 160 | Suggested loiter radius [m] 161 | Control Mode [-] 162 | Data valid [-] 163 | 164 | 165 | Monitoring of sensorpod status 166 | Timestamp in linuxtime [ms] (since 1.1.1970) 167 | Rate of ROS topic 1 168 | Rate of ROS topic 2 169 | Rate of ROS topic 3 170 | Rate of ROS topic 4 171 | Number of recording nodes 172 | Temperature of sensorpod CPU in [deg C] 173 | Free space available in recordings directory in [Gb] * 1e2 174 | 175 | 176 | 177 | -------------------------------------------------------------------------------- /mavgen/mavgen.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | import ( 4 | "bytes" 5 | "encoding/xml" 6 | "errors" 7 | "fmt" 8 | "go/format" 9 | "io" 10 | "io/ioutil" 11 | "sort" 12 | "strconv" 13 | "strings" 14 | "text/template" 15 | 16 | "github.com/liamstask/go-mavlink/x25" 17 | ) 18 | 19 | type Dialect struct { 20 | Name string 21 | StringSizes map[int]bool 22 | 23 | XMLName xml.Name `xml:"mavlink"` 24 | Version string `xml:"version"` 25 | Include string `xml:"include"` 26 | Enums []*Enum `xml:"enums>enum"` 27 | Messages []*Message `xml:"messages>message"` 28 | } 29 | 30 | type Enum struct { 31 | Name string `xml:"name,attr"` 32 | Description string `xml:"description"` 33 | Entries []*EnumEntry `xml:"entry"` 34 | } 35 | 36 | type EnumEntry struct { 37 | Value uint8 `xml:"value,attr"` 38 | Name string `xml:"name,attr"` 39 | Description string `xml:"description"` 40 | Params []*EnumEntryParam `xml:"param"` 41 | } 42 | 43 | type EnumEntryParam struct { 44 | Index uint8 `xml:"index,attr"` 45 | Description string `xml:",innerxml"` 46 | } 47 | 48 | type Message struct { 49 | ID uint8 `xml:"id,attr"` 50 | Name string `xml:"name,attr"` 51 | Description string `xml:"description"` 52 | Fields []*MessageField `xml:"field"` 53 | } 54 | 55 | type MessageField struct { 56 | CType string `xml:"type,attr"` 57 | Name string `xml:"name,attr"` 58 | Enum string `xml:"enum,attr"` 59 | Description string `xml:",innerxml"` 60 | GoType string 61 | BitSize int 62 | ArrayLen int 63 | ByteOffset int // from beginning of payload 64 | } 65 | 66 | var funcMap = template.FuncMap{ 67 | "UpperCamelCase": UpperCamelCase, 68 | } 69 | 70 | func (f *MessageField) SizeInBytes() int { 71 | if f.ArrayLen > 0 { 72 | return f.BitSize / 8 * f.ArrayLen 73 | } else { 74 | return f.BitSize / 8 75 | } 76 | } 77 | 78 | func (m *Message) Size() int { 79 | sz := 0 80 | for _, f := range m.Fields { 81 | sz += f.SizeInBytes() 82 | } 83 | return sz 84 | } 85 | 86 | // CRC extra calculation: http://www.mavlink.org/mavlink/crc_extra_calculation 87 | func (m *Message) CRCExtra() uint8 { 88 | hash := x25.New() 89 | 90 | fmt.Fprint(hash, m.Name+" ") 91 | for _, f := range m.Fields { 92 | cType := f.CType 93 | if cType == "uint8_t_mavlink_version" { 94 | cType = "uint8_t" 95 | } 96 | // type name for crc extra purposes does not include array portion 97 | if idx := strings.IndexByte(cType, '['); idx >= 0 { 98 | cType = cType[:idx] 99 | } 100 | fmt.Fprint(hash, cType+" "+f.Name+" ") 101 | if f.ArrayLen > 0 { 102 | hash.WriteByte(byte(f.ArrayLen)) 103 | } 104 | } 105 | 106 | crc := hash.Sum16() 107 | return uint8((crc & 0xFF) ^ (crc >> 8)) 108 | } 109 | 110 | // implementation of sort.Interface for Message 111 | func (m *Message) Len() int { 112 | return len(m.Fields) 113 | } 114 | 115 | func (m *Message) Less(i, j int) bool { 116 | return m.Fields[i].BitSize < m.Fields[j].BitSize 117 | } 118 | 119 | func (m *Message) Swap(i, j int) { 120 | m.Fields[i], m.Fields[j] = m.Fields[j], m.Fields[i] 121 | } 122 | 123 | // convert names to upper camel case 124 | func UpperCamelCase(s string) string { 125 | var b bytes.Buffer 126 | for _, frag := range strings.Split(s, "_") { 127 | if frag != "" { 128 | b.WriteString(strings.ToUpper(frag[:1])) 129 | b.WriteString(strings.ToLower(frag[1:])) 130 | } 131 | } 132 | return b.String() 133 | } 134 | 135 | // helper to pack a single element into a payload. 136 | // can be called for a single field, or an element within a field's array. 137 | func (f *MessageField) payloadPackPrimitive(offset, name string) string { 138 | 139 | if f.BitSize == 8 { 140 | return fmt.Sprintf("payload[%s] = byte(%s)", offset, name) 141 | } 142 | 143 | if f.IsFloat() { 144 | switch f.BitSize { 145 | case 32, 64: 146 | return fmt.Sprintf("binary.LittleEndian.PutUint%d(payload[%s:], math.Float%dbits(%s))", f.BitSize, offset, f.BitSize, name) 147 | } 148 | } else { 149 | switch f.BitSize { 150 | case 16, 32, 64: 151 | return fmt.Sprintf("binary.LittleEndian.PutUint%d(payload[%s:], uint%d(%s))", f.BitSize, offset, f.BitSize, name) 152 | } 153 | } 154 | 155 | panic("unhandled bitsize") 156 | } 157 | 158 | // produce a string that will pack this message's fields 159 | // into a byte slice called 'payload' 160 | func (f *MessageField) PayloadPackSequence() string { 161 | name := UpperCamelCase(f.Name) 162 | 163 | if f.ArrayLen > 0 { 164 | // optimize to copy() if possible 165 | if strings.HasSuffix(f.GoType, "byte") || strings.HasSuffix(f.GoType, "uint8") { 166 | return fmt.Sprintf("copy(payload[%d:], self.%s[:])", f.ByteOffset, name) 167 | } 168 | 169 | // pack each element in the array 170 | s := fmt.Sprintf("for i, v := range self.%s {\n", name) 171 | off := fmt.Sprintf("%d + i * %d", f.ByteOffset, f.BitSize/8) 172 | s += f.payloadPackPrimitive(off, "v") + "\n" 173 | s += fmt.Sprintf("}") 174 | return s 175 | } 176 | 177 | // pack a single field 178 | return f.payloadPackPrimitive(fmt.Sprintf("%d", f.ByteOffset), "self."+name) 179 | } 180 | 181 | func (f *MessageField) payloadUnpackPrimitive(offset string) string { 182 | 183 | if f.BitSize == 8 { 184 | return fmt.Sprintf("%s(p.Payload[%s])", goArrayType(f.GoType), offset) 185 | } 186 | 187 | if f.IsFloat() { 188 | switch f.BitSize { 189 | case 32, 64: 190 | return fmt.Sprintf("math.Float%dfrombits(binary.LittleEndian.Uint%d(p.Payload[%s:]))", f.BitSize, f.BitSize, offset) 191 | } 192 | } else { 193 | switch f.BitSize { 194 | case 16, 32, 64: 195 | return fmt.Sprintf("%s(binary.LittleEndian.Uint%d(p.Payload[%s:]))", goArrayType(f.GoType), f.BitSize, offset) 196 | } 197 | } 198 | 199 | panic("unhandled bitsize") 200 | } 201 | 202 | func (f *MessageField) PayloadUnpackSequence() string { 203 | name := UpperCamelCase(f.Name) 204 | 205 | if f.ArrayLen > 0 { 206 | // optimize to copy() if possible 207 | if strings.HasSuffix(f.GoType, "byte") || strings.HasSuffix(f.GoType, "uint8") { 208 | return fmt.Sprintf("copy(self.%s[:], p.Payload[%d:%d])", name, f.ByteOffset, f.ByteOffset+f.ArrayLen) 209 | } 210 | 211 | // unpack each element in the array 212 | s := fmt.Sprintf("for i := 0; i < len(self.%s); i++ {\n", name) 213 | off := fmt.Sprintf("%d + i * %d", f.ByteOffset, f.BitSize/8) 214 | s += fmt.Sprintf("self.%s[i] = %s\n", name, f.payloadUnpackPrimitive(off)) 215 | s += fmt.Sprintf("}") 216 | return s 217 | } 218 | 219 | return fmt.Sprintf("self.%s = %s", name, f.payloadUnpackPrimitive(fmt.Sprintf("%d", f.ByteOffset))) 220 | } 221 | 222 | func SanitizeComments(s string) string { 223 | return strings.Replace(s, "\n", "\n// ", -1) 224 | } 225 | 226 | // read in an xml-based dialect file, 227 | // and populate a Dialect struct with its contents 228 | func ParseDialect(in io.Reader, name string) (*Dialect, error) { 229 | 230 | filebytes, err := ioutil.ReadAll(in) 231 | if err != nil { 232 | return nil, err 233 | } 234 | 235 | dialect := &Dialect{ 236 | Name: name, 237 | } 238 | 239 | if err := xml.Unmarshal(filebytes, &dialect); err != nil { 240 | return nil, err 241 | } 242 | 243 | if dialect.Include != "" { 244 | // generate(protocol.IncludeName()) 245 | } 246 | 247 | return dialect, nil 248 | } 249 | 250 | // convert a C primitive type to its corresponding Go type. 251 | // do not handle arrays or other constructs...just primitives. 252 | func c2goPrimitive(ctype string) string { 253 | switch ctype { 254 | case "uint8_t", "uint16_t", "uint32_t", "uint64_t", 255 | "int8_t", "int16_t", "int32_t", "int64_t": 256 | idx := strings.IndexByte(ctype, '_') 257 | return ctype[:idx] 258 | case "char": 259 | return "byte" 260 | case "float": 261 | return "float32" 262 | case "double": 263 | return "float64" 264 | case "uint8_t_mavlink_version": 265 | return "uint8" 266 | default: 267 | panic(fmt.Sprintf("c2goPrimitive: unhandled primitive type - %s", ctype)) 268 | } 269 | } 270 | 271 | func goArrayType(s string) string { 272 | idx := strings.IndexByte(s, ']') 273 | if idx < 0 { 274 | return s 275 | } 276 | return s[idx+1:] 277 | } 278 | 279 | func (f *MessageField) IsFloat() bool { 280 | return strings.HasPrefix(goArrayType(f.GoType), "float") 281 | } 282 | 283 | func GoTypeInfo(s string) (string, int, int, error) { 284 | 285 | var name string 286 | var bitsz, arraylen int 287 | var err error 288 | 289 | // array? leave the [N] but convert the primitive type name 290 | if idx := strings.IndexByte(s, '['); idx < 0 { 291 | name = c2goPrimitive(s) 292 | } else { 293 | name = s[idx:] + c2goPrimitive(s[:idx]) 294 | if arraylen, err = strconv.Atoi(s[idx+1 : len(s)-1]); err != nil { 295 | return "", 0, 0, err 296 | } 297 | } 298 | 299 | // determine bit size for this type 300 | if strings.HasSuffix(name, "byte") { 301 | bitsz = 8 302 | } else { 303 | t := name[strings.IndexByte(name, ']')+1:] 304 | if sizeStart := strings.IndexAny(t, "8136"); sizeStart != -1 { 305 | if bitsz, err = strconv.Atoi(t[sizeStart:]); err != nil { 306 | return "", 0, 0, err 307 | } 308 | } else { 309 | return "", 0, 0, errors.New("Unknown message field size") 310 | } 311 | } 312 | 313 | return name, bitsz, arraylen, nil 314 | } 315 | 316 | // generate a .go source file from the given dialect 317 | func (d *Dialect) GenerateGo(w io.Writer) error { 318 | // templatize to buffer, format it, then write out 319 | 320 | var bb bytes.Buffer 321 | 322 | bb.WriteString("package mavlink\n\n") 323 | 324 | bb.WriteString("import (\n") 325 | bb.WriteString("\"encoding/binary\"\n") 326 | bb.WriteString("\"fmt\"\n") 327 | bb.WriteString("\"math\"\n") 328 | bb.WriteString(")\n") 329 | 330 | bb.WriteString("//////////////////////////////////////////////////\n") 331 | bb.WriteString("//\n") 332 | bb.WriteString("// NOTE: do not edit,\n") 333 | bb.WriteString("// this file created automatically by mavgen.go\n") 334 | bb.WriteString("//\n") 335 | bb.WriteString("//////////////////////////////////////////////////\n\n") 336 | 337 | err := d.generateEnums(&bb) 338 | d.generateClasses(&bb) 339 | d.generateMsgIds(&bb) 340 | 341 | dofmt := true 342 | formatted := bb.Bytes() 343 | 344 | if dofmt { 345 | formatted, err = format.Source(bb.Bytes()) 346 | if err != nil { 347 | return err 348 | } 349 | } 350 | 351 | n, err := w.Write(formatted) 352 | if err == nil && n != len(formatted) { 353 | return io.ErrShortWrite 354 | } 355 | 356 | return err 357 | } 358 | 359 | func (d *Dialect) generateEnums(w io.Writer) error { 360 | enumTmpl := ` 361 | {{range .Enums}} 362 | // {{.Name}}: {{.Description}} 363 | const ({{range .Entries}} 364 | {{.Name}} = {{.Value}} // {{.Description}}{{end}} 365 | ) 366 | {{end}} 367 | ` 368 | // fill in missing enum values if necessary, and ensure description strings are valid. 369 | for _, e := range d.Enums { 370 | e.Description = strings.Replace(e.Description, "\n", " ", -1) 371 | e.Name = UpperCamelCase(e.Name) 372 | for i, ee := range e.Entries { 373 | if ee.Value == 0 { 374 | ee.Value = uint8(i) 375 | } 376 | ee.Description = strings.Replace(ee.Description, "\n", " ", -1) 377 | } 378 | } 379 | 380 | return template.Must(template.New("enums").Parse(enumTmpl)).Execute(w, d) 381 | } 382 | 383 | func (d *Dialect) generateMsgIds(w io.Writer) error { 384 | msgIdTmpl := ` 385 | // Message IDs 386 | const ({{range .Messages}} 387 | MSG_ID_{{.Name}} = {{.ID}}{{end}} 388 | ) 389 | 390 | // Dialect{{.Name | UpperCamelCase}} is the dialect represented by {{.Name}}.xml 391 | var Dialect{{.Name | UpperCamelCase}} *Dialect = &Dialect{ 392 | Name: "{{.Name}}", 393 | crcExtras: map[uint8]uint8{ {{range .Messages}} 394 | {{.ID}}: {{.CRCExtra}}, // MSG_ID_{{.Name}}{{end}} 395 | }, 396 | } 397 | ` 398 | return template.Must(template.New("msgIds").Funcs(funcMap).Parse(msgIdTmpl)).Execute(w, d) 399 | } 400 | 401 | // generate class definitions for each msg id. 402 | // for now, pack/unpack payloads via encoding/binary since it 403 | // is expedient and correct. optimize this if/when needed. 404 | func (d *Dialect) generateClasses(w io.Writer) error { 405 | 406 | classesTmpl := ` 407 | {{range .Messages}} 408 | {{$name := .Name | UpperCamelCase}} 409 | // {{.Description}} 410 | type {{$name}} struct { {{range .Fields}} 411 | {{.Name | UpperCamelCase}} {{.GoType}} // {{.Description}}{{end}} 412 | } 413 | 414 | func (self *{{$name}}) MsgID() uint8 { 415 | return {{.ID}} 416 | } 417 | 418 | func (self *{{$name}}) MsgName() string { 419 | return "{{.Name | UpperCamelCase}}" 420 | } 421 | 422 | func (self *{{$name}}) Pack(p *Packet) error { 423 | payload := make([]byte, {{ .Size }}){{range .Fields}} 424 | {{.PayloadPackSequence}}{{end}} 425 | 426 | p.MsgID = self.MsgID() 427 | p.Payload = payload 428 | return nil 429 | } 430 | 431 | func (self *{{$name}}) Unpack(p *Packet) error { 432 | if len(p.Payload) < {{ .Size }} { 433 | return fmt.Errorf("payload too small") 434 | }{{range .Fields}} 435 | {{.PayloadUnpackSequence}}{{end}} 436 | return nil 437 | } 438 | {{end}} 439 | ` 440 | for _, m := range d.Messages { 441 | m.Description = strings.Replace(m.Description, "\n", "\n// ", -1) 442 | 443 | for _, f := range m.Fields { 444 | f.Description = strings.Replace(f.Description, "\n", " ", -1) 445 | goname, gosz, golen, err := GoTypeInfo(f.CType) 446 | if err != nil { 447 | return err 448 | } 449 | f.GoType, f.BitSize, f.ArrayLen = goname, gosz, golen 450 | } 451 | 452 | // ensure fields are sorted according to their size, 453 | // http://www.mavlink.org/mavlink/crc_extra_calculation 454 | sort.Stable(sort.Reverse(m)) 455 | 456 | // once sorted, calculate offsets for use in payload packing/unpacking 457 | offset := 0 458 | for _, f := range m.Fields { 459 | f.ByteOffset = offset 460 | offset += f.SizeInBytes() 461 | } 462 | } 463 | 464 | return template.Must(template.New("classesTmpl").Funcs(funcMap).Parse(classesTmpl)).Execute(w, d) 465 | } 466 | -------------------------------------------------------------------------------- /mavlink/definitions/pixhawk.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | common.xml 4 | 5 | 6 | Content Types for data transmission handshake 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | Starts a search 15 | 1 to arm, 0 to disarm 16 | 17 | 18 | Starts a search 19 | 1 to arm, 0 to disarm 20 | 21 | 22 | Starts a search 23 | 1 to arm, 0 to disarm 24 | 25 | 26 | 27 | 28 | 29 | Camera id 30 | Camera mode: 0 = auto, 1 = manual 31 | Trigger pin, 0-3 for PtGrey FireFly 32 | Shutter interval, in microseconds 33 | Exposure time, in microseconds 34 | Camera gain 35 | 36 | 37 | Timestamp 38 | IMU seq 39 | Roll angle in rad 40 | Pitch angle in rad 41 | Yaw angle in rad 42 | Local frame Z coordinate (height over ground) 43 | GPS X coordinate 44 | GPS Y coordinate 45 | Global frame altitude 46 | Ground truth X 47 | Ground truth Y 48 | Ground truth Z 49 | 50 | 51 | 0 to disable, 1 to enable 52 | 53 | 54 | Camera id 55 | Camera # (starts with 0) 56 | Timestamp 57 | Until which timestamp this buffer will stay valid 58 | The image sequence number 59 | Position of the image in the buffer, starts with 0 60 | Image width 61 | Image height 62 | Image depth 63 | Image channels 64 | Shared memory area key 65 | Exposure time, in microseconds 66 | Camera gain 67 | Roll angle in rad 68 | Pitch angle in rad 69 | Yaw angle in rad 70 | Local frame Z coordinate (height over ground) 71 | GPS X coordinate 72 | GPS Y coordinate 73 | Global frame altitude 74 | Ground truth X 75 | Ground truth Y 76 | Ground truth Z 77 | 78 | 79 | Message sent to the MAV to set a new offset from the currently controlled position 80 | System ID 81 | Component ID 82 | x position offset 83 | y position offset 84 | z position offset 85 | yaw orientation offset in radians, 0 = NORTH 86 | 87 | 88 | 89 | ID of waypoint, 0 for plain position 90 | x position 91 | y position 92 | z position 93 | yaw orientation in radians, 0 = NORTH 94 | 95 | 96 | ID 97 | x position 98 | y position 99 | z position 100 | roll orientation 101 | pitch orientation 102 | yaw orientation 103 | 104 | 105 | ADC1 (J405 ADC3, LPC2148 AD0.6) 106 | ADC2 (J405 ADC5, LPC2148 AD0.2) 107 | ADC3 (J405 ADC6, LPC2148 AD0.1) 108 | ADC4 (J405 ADC7, LPC2148 AD1.3) 109 | Battery voltage 110 | Temperature (degrees celcius) 111 | Barometric pressure (hecto Pascal) 112 | 113 | 114 | Watchdog ID 115 | Number of processes 116 | 117 | 118 | Watchdog ID 119 | Process ID 120 | Process name 121 | Process arguments 122 | Timeout (seconds) 123 | 124 | 125 | Watchdog ID 126 | Process ID 127 | Is running / finished / suspended / crashed 128 | Is muted 129 | PID 130 | Number of crashes 131 | 132 | 133 | Target system ID 134 | Watchdog ID 135 | Process ID 136 | Command ID 137 | 138 | 139 | 0: Pattern, 1: Letter 140 | Confidence of detection 141 | Pattern file name 142 | Accepted as true detection, 0 no, 1 yes 143 | 144 | 145 | Notifies the operator about a point of interest (POI). This can be anything detected by the 146 | system. This generic message is intented to help interfacing to generic visualizations and to display 147 | the POI on a map. 148 | 149 | 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug 150 | 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta 151 | 0: global, 1:local 152 | 0: no timeout, >1: timeout in seconds 153 | X Position 154 | Y Position 155 | Z Position 156 | POI name 157 | 158 | 159 | Notifies the operator about the connection of two point of interests (POI). This can be anything detected by the 160 | system. This generic message is intented to help interfacing to generic visualizations and to display 161 | the POI on a map. 162 | 163 | 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug 164 | 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta 165 | 0: global, 1:local 166 | 0: no timeout, >1: timeout in seconds 167 | X1 Position 168 | Y1 Position 169 | Z1 Position 170 | X2 Position 171 | Y2 Position 172 | Z2 Position 173 | POI connection name 174 | 175 | 176 | x position in m 177 | y position in m 178 | z position in m 179 | Orientation assignment 0: false, 1:true 180 | Size in pixels 181 | Orientation 182 | Descriptor 183 | Harris operator response at this location 184 | 185 | 186 | The system to be controlled 187 | roll 188 | pitch 189 | yaw 190 | thrust 191 | roll control enabled auto:0, manual:1 192 | pitch auto:0, manual:1 193 | yaw auto:0, manual:1 194 | thrust auto:0, manual:1 195 | 196 | 197 | Number of detections 198 | Number of cluster iterations 199 | Best score 200 | Latitude of the best detection * 1E7 201 | Longitude of the best detection * 1E7 202 | Altitude of the best detection * 1E3 203 | Best detection ID 204 | Best cluster ID 205 | Best cluster ID 206 | Number of images already processed 207 | Number of images still to process 208 | Average images per seconds processed 209 | 210 | 211 | Uptime of system 212 | CPU frequency 213 | CPU load in percent 214 | RAM usage in percent 215 | RAM size in GiB 216 | Swap usage in percent 217 | Swap size in GiB 218 | Disk health (-1: N/A, 0: ERR, 1: RO, 2: RW) 219 | Disk usage in percent 220 | Disk total in GiB 221 | Temperature 222 | Supply voltage V 223 | Network load inbound KiB/s 224 | Network load outbound in KiB/s 225 | 226 | 227 | 228 | -------------------------------------------------------------------------------- /mavlink/definitions/slugs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | common.xml 4 | 5 | 6 | 7 | 8 | 9 | Does nothing. 10 | 1 to arm, 0 to disarm 11 | 12 | 13 | 30 | 31 | 32 | Return vehicle to base. 33 | 0: return to base, 1: track mobile base 34 | 35 | 36 | Stops the vehicle from returning to base and resumes flight. 37 | 38 | 39 | Turns the vehicle's visible or infrared lights on or off. 40 | 0: visible lights, 1: infrared lights 41 | 0: turn on, 1: turn off 42 | 43 | 44 | Requests vehicle to send current mid-level commands to ground station. 45 | 46 | 47 | Requests storage of mid-level commands. 48 | Mid-level command storage: 0: read from flash/EEPROM, 1: write to flash/EEPROM 49 | 50 | 51 | 52 | 53 | 54 | Slugs-specific navigation modes. 55 | 56 | No change to SLUGS mode. 57 | 58 | 59 | Vehicle is in liftoff mode. 60 | 61 | 62 | Vehicle is in passthrough mode, being controlled by a pilot. 63 | 64 | 65 | Vehicle is in waypoint mode, navigating to waypoints. 66 | 67 | 68 | Vehicle is executing mid-level commands. 69 | 70 | 71 | Vehicle is returning to the home location. 72 | 73 | 74 | Vehicle is landing. 75 | 76 | 77 | Lost connection with vehicle. 78 | 79 | 80 | Vehicle is in selective passthrough mode, where selected surfaces are being manually controlled. 81 | 82 | 83 | Vehicle is in ISR mode, performing reconaissance at a point specified by ISR_LOCATION message. 84 | 85 | 86 | Vehicle is patrolling along lines between waypoints. 87 | 88 | 89 | Vehicle is grounded or an error has occurred. 90 | 91 | 92 | 93 | 94 | These flags encode the control surfaces for selective passthrough mode. If a bit is set then the pilot console 95 | has control of the surface, and if not then the autopilot has control of the surface. 96 | 97 | 0b10000000 Throttle control passes through to pilot console. 98 | 99 | 100 | 0b01000000 Left aileron control passes through to pilot console. 101 | 102 | 103 | 0b00100000 Right aileron control passes through to pilot console. 104 | 105 | 106 | 0b00010000 Rudder control passes through to pilot console. 107 | 108 | 109 | 0b00001000 Left elevator control passes through to pilot console. 110 | 111 | 112 | 0b00000100 Right elevator control passes through to pilot console. 113 | 114 | 115 | 0b00000010 Left flap control passes through to pilot console. 116 | 117 | 118 | 0b00000001 Right flap control passes through to pilot console. 119 | 120 | 121 | 122 | 134 | 135 | 136 | 137 | Sensor and DSC control loads. 138 | Sensor DSC Load 139 | Control DSC Load 140 | Battery Voltage in millivolts 141 | 142 | 143 | 144 | Accelerometer and gyro biases. 145 | Accelerometer X bias (m/s) 146 | Accelerometer Y bias (m/s) 147 | Accelerometer Z bias (m/s) 148 | Gyro X bias (rad/s) 149 | Gyro Y bias (rad/s) 150 | Gyro Z bias (rad/s) 151 | 152 | 153 | 154 | Configurable diagnostic messages. 155 | Diagnostic float 1 156 | Diagnostic float 2 157 | Diagnostic float 3 158 | Diagnostic short 1 159 | Diagnostic short 2 160 | Diagnostic short 3 161 | 162 | 163 | 164 | Data used in the navigation algorithm. 165 | Measured Airspeed prior to the nav filter in m/s 166 | Commanded Roll 167 | Commanded Pitch 168 | Commanded Turn rate 169 | Y component of the body acceleration 170 | Total Distance to Run on this leg of Navigation 171 | Remaining distance to Run on this leg of Navigation 172 | Origin WP 173 | Destination WP 174 | Commanded altitude in 0.1 m 175 | 176 | 177 | 178 | Configurable data log probes to be used inside Simulink 179 | Log value 1 180 | Log value 2 181 | Log value 3 182 | Log value 4 183 | Log value 5 184 | Log value 6 185 | 186 | 187 | 188 | Pilot console PWM messges. 189 | Year reported by Gps 190 | Month reported by Gps 191 | Day reported by Gps 192 | Hour reported by Gps 193 | Min reported by Gps 194 | Sec reported by Gps 195 | Clock Status. See table 47 page 211 OEMStar Manual 196 | Visible satellites reported by Gps 197 | Used satellites in Solution 198 | GPS+GLONASS satellites in Solution 199 | GPS and GLONASS usage mask (bit 0 GPS_used? bit_4 GLONASS_used?) 200 | Percent used GPS 201 | 202 | 203 | 204 | Mid Level commands sent from the GS to the autopilot. These are only sent when being operated in mid-level commands mode from the ground. 205 | The system setting the commands 206 | Commanded Altitude in meters 207 | Commanded Airspeed in m/s 208 | Commanded Turnrate in rad/s 209 | 210 | 211 | 212 | This message sets the control surfaces for selective passthrough mode. 213 | The system setting the commands 214 | Bitfield containing the passthrough configuration, see CONTROL_SURFACE_FLAG ENUM. 215 | 216 | 217 | 218 | Orders generated to the SLUGS camera mount. 219 | The system reporting the action 220 | Order the mount to pan: -1 left, 0 No pan motion, +1 right 221 | Order the mount to tilt: -1 down, 0 No tilt motion, +1 up 222 | Order the zoom values 0 to 10 223 | Orders the camera mount to move home. The other fields are ignored when this field is set. 1: move home, 0 ignored 224 | 225 | 226 | 227 | Control for surface; pending and order to origin. 228 | The system setting the commands 229 | ID control surface send 0: throttle 1: aileron 2: elevator 3: rudder 230 | Pending 231 | Order to origin 232 | 233 | 234 | 235 | 243 | 244 | 245 | Transmits the last known position of the mobile GS to the UAV. Very relevant when Track Mobile is enabled 246 | The system reporting the action 247 | Mobile Latitude 248 | Mobile Longitude 249 | 250 | 251 | 252 | Control for camara. 253 | The system setting the commands 254 | ID 0: brightness 1: aperture 2: iris 3: ICR 4: backlight 255 | 1: up/on 2: down/off 3: auto/reset/no action 256 | 257 | 258 | 259 | Transmits the position of watch 260 | The system reporting the action 261 | ISR Latitude 262 | ISR Longitude 263 | ISR Height 264 | Option 1 265 | Option 2 266 | Option 3 267 | 268 | 269 | 270 | 278 | 279 | 280 | Transmits the readings from the voltage and current sensors 281 | It is the value of reading 2: 0 - Current, 1 - Foreward Sonar, 2 - Back Sonar, 3 - RPM 282 | Voltage in uS of PWM. 0 uS = 0V, 20 uS = 21.5V 283 | Depends on the value of r2Type (0) Current consumption in uS of PWM, 20 uS = 90Amp (1) Distance in cm (2) Distance in cm (3) Absolute value 284 | 285 | 286 | 287 | Transmits the actual Pan, Tilt and Zoom values of the camera unit 288 | The actual Zoom Value 289 | The Pan value in 10ths of degree 290 | The Tilt value in 10ths of degree 291 | 292 | 293 | 294 | Transmits the actual status values UAV in flight 295 | The ID system reporting the action 296 | Latitude UAV 297 | Longitude UAV 298 | Altitude UAV 299 | Speed UAV 300 | Course UAV 301 | 302 | 303 | 304 | This contains the status of the GPS readings 305 | Number of times checksum has failed 306 | The quality indicator, 0=fix not available or invalid, 1=GPS fix, 2=C/A differential GPS, 6=Dead reckoning mode, 7=Manual input mode (fixed position), 8=Simulator mode, 9= WAAS a 307 | Indicates if GN, GL or GP messages are being received 308 | A = data valid, V = data invalid 309 | Magnetic variation, degrees 310 | Magnetic variation direction E/W. Easterly variation (E) subtracts from True course and Westerly variation (W) adds to True course 311 | Positioning system mode indicator. A - Autonomous;D-Differential; E-Estimated (dead reckoning) mode;M-Manual input; N-Data not valid 312 | 313 | 314 | 315 | Transmits the diagnostics data from the Novatel OEMStar GPS 316 | The Time Status. See Table 8 page 27 Novatel OEMStar Manual 317 | Status Bitfield. See table 69 page 350 Novatel OEMstar Manual 318 | solution Status. See table 44 page 197 319 | position type. See table 43 page 196 320 | velocity type. See table 43 page 196 321 | Age of the position solution in seconds 322 | Times the CRC has failed since boot 323 | 324 | 325 | 326 | Diagnostic data Sensor MCU 327 | Float field 1 328 | Float field 2 329 | Int 16 field 1 330 | Int 8 field 1 331 | 332 | 333 | 334 | 335 | The boot message indicates that a system is starting. The onboard software version allows to keep track of onboard soft/firmware revisions. This message allows the sensor and control MCUs to communicate version numbers on startup. 336 | The onboard software version 337 | 338 | 339 | 340 | -------------------------------------------------------------------------------- /mavlink/definitions/matrixpilot.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | common.xml 4 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | Action required when performing CMD_PREFLIGHT_STORAGE 18 | 19 | Read all parameters from storage 20 | 21 | 22 | Write all parameters to storage 23 | 24 | 25 | Clear all parameters in storage 26 | 27 | 28 | Read specific parameters from storage 29 | 30 | 31 | Write specific parameters to storage 32 | 33 | 34 | Clear specific parameters in storage 35 | 36 | 37 | do nothing 38 | 39 | 40 | 41 | 42 | 43 | Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. 44 | Storage action: Action defined by MAV_PREFLIGHT_STORAGE_ACTION_ADVANCED 45 | Storage area as defined by parameter database 46 | Storage flags as defined by parameter database 47 | Empty 48 | Empty 49 | Empty 50 | Empty 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | Depreciated but used as a compiler flag. Do not remove 60 | System ID 61 | Component ID 62 | 63 | 64 | Reqest reading of flexifunction data 65 | System ID 66 | Component ID 67 | Type of flexifunction data requested 68 | index into data where needed 69 | 70 | 71 | Flexifunction type and parameters for component at function index from buffer 72 | System ID 73 | Component ID 74 | Function index 75 | Total count of functions 76 | Address in the flexifunction data, Set to 0xFFFF to use address in target memory 77 | Size of the 78 | Settings data 79 | 80 | 81 | Flexifunction type and parameters for component at function index from buffer 82 | System ID 83 | Component ID 84 | Function index 85 | result of acknowledge, 0=fail, 1=good 86 | 87 | 88 | Acknowldge sucess or failure of a flexifunction command 89 | System ID 90 | Component ID 91 | 0=inputs, 1=outputs 92 | index of first directory entry to write 93 | count of directory entries to write 94 | Settings data 95 | 96 | 97 | Acknowldge sucess or failure of a flexifunction command 98 | System ID 99 | Component ID 100 | 0=inputs, 1=outputs 101 | index of first directory entry to write 102 | count of directory entries to write 103 | result of acknowledge, 0=fail, 1=good 104 | 105 | 106 | Acknowldge sucess or failure of a flexifunction command 107 | System ID 108 | Component ID 109 | Flexifunction command type 110 | 111 | 112 | Acknowldge sucess or failure of a flexifunction command 113 | Command acknowledged 114 | result of acknowledge 115 | 116 | 117 | Backwards compatible MAVLink version of SERIAL_UDB_EXTRA - F2: Format Part A 118 | Serial UDB Extra Time 119 | Serial UDB Extra Status 120 | Serial UDB Extra Latitude 121 | Serial UDB Extra Longitude 122 | Serial UDB Extra Altitude 123 | Serial UDB Extra Waypoint Index 124 | Serial UDB Extra Rmat 0 125 | Serial UDB Extra Rmat 1 126 | Serial UDB Extra Rmat 2 127 | Serial UDB Extra Rmat 3 128 | Serial UDB Extra Rmat 4 129 | Serial UDB Extra Rmat 5 130 | Serial UDB Extra Rmat 6 131 | Serial UDB Extra Rmat 7 132 | Serial UDB Extra Rmat 8 133 | Serial UDB Extra GPS Course Over Ground 134 | Serial UDB Extra Speed Over Ground 135 | Serial UDB Extra CPU Load 136 | Serial UDB Extra Voltage in MilliVolts 137 | Serial UDB Extra 3D IMU Air Speed 138 | Serial UDB Extra Estimated Wind 0 139 | Serial UDB Extra Estimated Wind 1 140 | Serial UDB Extra Estimated Wind 2 141 | Serial UDB Extra Magnetic Field Earth 0 142 | Serial UDB Extra Magnetic Field Earth 1 143 | Serial UDB Extra Magnetic Field Earth 2 144 | Serial UDB Extra Number of Sattelites in View 145 | Serial UDB Extra GPS Horizontal Dilution of Precision 146 | 147 | 148 | Backwards compatible version of SERIAL_UDB_EXTRA - F2: Part B 149 | Serial UDB Extra Time 150 | Serial UDB Extra PWM Input Channel 1 151 | Serial UDB Extra PWM Input Channel 2 152 | Serial UDB Extra PWM Input Channel 3 153 | Serial UDB Extra PWM Input Channel 4 154 | Serial UDB Extra PWM Input Channel 5 155 | Serial UDB Extra PWM Input Channel 6 156 | Serial UDB Extra PWM Input Channel 7 157 | Serial UDB Extra PWM Input Channel 8 158 | Serial UDB Extra PWM Input Channel 9 159 | Serial UDB Extra PWM Input Channel 10 160 | Serial UDB Extra PWM Output Channel 1 161 | Serial UDB Extra PWM Output Channel 2 162 | Serial UDB Extra PWM Output Channel 3 163 | Serial UDB Extra PWM Output Channel 4 164 | Serial UDB Extra PWM Output Channel 5 165 | Serial UDB Extra PWM Output Channel 6 166 | Serial UDB Extra PWM Output Channel 7 167 | Serial UDB Extra PWM Output Channel 8 168 | Serial UDB Extra PWM Output Channel 9 169 | Serial UDB Extra PWM Output Channel 10 170 | Serial UDB Extra IMU Location X 171 | Serial UDB Extra IMU Location Y 172 | Serial UDB Extra IMU Location Z 173 | Serial UDB Extra Status Flags 174 | Serial UDB Extra Oscillator Failure Count 175 | Serial UDB Extra IMU Velocity X 176 | Serial UDB Extra IMU Velocity Y 177 | Serial UDB Extra IMU Velocity Z 178 | Serial UDB Extra Current Waypoint Goal X 179 | Serial UDB Extra Current Waypoint Goal Y 180 | Serial UDB Extra Current Waypoint Goal Z 181 | Serial UDB Extra Stack Memory Free 182 | 183 | 184 | Backwards compatible version of SERIAL_UDB_EXTRA F4: format 185 | Serial UDB Extra Roll Stabilization with Ailerons Enabled 186 | Serial UDB Extra Roll Stabilization with Rudder Enabled 187 | Serial UDB Extra Pitch Stabilization Enabled 188 | Serial UDB Extra Yaw Stabilization using Rudder Enabled 189 | Serial UDB Extra Yaw Stabilization using Ailerons Enabled 190 | Serial UDB Extra Navigation with Ailerons Enabled 191 | Serial UDB Extra Navigation with Rudder Enabled 192 | Serial UDB Extra Type of Alitude Hold when in Stabilized Mode 193 | Serial UDB Extra Type of Alitude Hold when in Waypoint Mode 194 | Serial UDB Extra Firmware racing mode enabled 195 | 196 | 197 | Backwards compatible version of SERIAL_UDB_EXTRA F5: format 198 | Serial UDB YAWKP_AILERON Gain for Proporional control of navigation 199 | Serial UDB YAWKD_AILERON Gain for Rate control of navigation 200 | Serial UDB Extra ROLLKP Gain for Proportional control of roll stabilization 201 | Serial UDB Extra ROLLKD Gain for Rate control of roll stabilization 202 | YAW_STABILIZATION_AILERON Proportional control 203 | Gain For Boosting Manual Aileron control When Plane Stabilized 204 | 205 | 206 | Backwards compatible version of SERIAL_UDB_EXTRA F6: format 207 | Serial UDB Extra PITCHGAIN Proportional Control 208 | Serial UDB Extra Pitch Rate Control 209 | Serial UDB Extra Rudder to Elevator Mix 210 | Serial UDB Extra Roll to Elevator Mix 211 | Gain For Boosting Manual Elevator control When Plane Stabilized 212 | 213 | 214 | Backwards compatible version of SERIAL_UDB_EXTRA F7: format 215 | Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation 216 | Serial UDB YAWKD_RUDDER Gain for Rate control of navigation 217 | Serial UDB Extra ROLLKP_RUDDER Gain for Proportional control of roll stabilization 218 | Serial UDB Extra ROLLKD_RUDDER Gain for Rate control of roll stabilization 219 | SERIAL UDB EXTRA Rudder Boost Gain to Manual Control when stabilized 220 | Serial UDB Extra Return To Landing - Angle to Pitch Plane Down 221 | 222 | 223 | Backwards compatible version of SERIAL_UDB_EXTRA F8: format 224 | Serial UDB Extra HEIGHT_TARGET_MAX 225 | Serial UDB Extra HEIGHT_TARGET_MIN 226 | Serial UDB Extra ALT_HOLD_THROTTLE_MIN 227 | Serial UDB Extra ALT_HOLD_THROTTLE_MAX 228 | Serial UDB Extra ALT_HOLD_PITCH_MIN 229 | Serial UDB Extra ALT_HOLD_PITCH_MAX 230 | Serial UDB Extra ALT_HOLD_PITCH_HIGH 231 | 232 | 233 | Backwards compatible version of SERIAL_UDB_EXTRA F13: format 234 | Serial UDB Extra GPS Week Number 235 | Serial UDB Extra MP Origin Latitude 236 | Serial UDB Extra MP Origin Longitude 237 | Serial UDB Extra MP Origin Altitude Above Sea Level 238 | 239 | 240 | Backwards compatible version of SERIAL_UDB_EXTRA F14: format 241 | Serial UDB Extra Wind Estimation Enabled 242 | Serial UDB Extra Type of GPS Unit 243 | Serial UDB Extra Dead Reckoning Enabled 244 | Serial UDB Extra Type of UDB Hardware 245 | Serial UDB Extra Type of Airframe 246 | Serial UDB Extra Reboot Regitster of DSPIC 247 | Serial UDB Extra Last dspic Trap Flags 248 | Serial UDB Extra Type Program Address of Last Trap 249 | Serial UDB Extra Number of Ocillator Failures 250 | Serial UDB Extra UDB Internal Clock Configuration 251 | Serial UDB Extra Type of Flight Plan 252 | 253 | 254 | Backwards compatible version of SERIAL_UDB_EXTRA F15 and F16: format 255 | Serial UDB Extra Model Name Of Vehicle 256 | Serial UDB Extra Registraton Number of Vehicle 257 | 258 | 259 | Serial UDB Extra Name of Expected Lead Pilot 260 | Serial UDB Extra URL of Lead Pilot or Team 261 | 262 | 263 | The altitude measured by sensors and IMU 264 | Timestamp (milliseconds since system boot) 265 | GPS altitude in meters, expressed as * 1000 (millimeters), above MSL 266 | IMU altitude above ground in meters, expressed as * 1000 (millimeters) 267 | barometeric altitude above ground in meters, expressed as * 1000 (millimeters) 268 | Optical flow altitude above ground in meters, expressed as * 1000 (millimeters) 269 | Rangefinder Altitude above ground in meters, expressed as * 1000 (millimeters) 270 | Extra altitude above ground in meters, expressed as * 1000 (millimeters) 271 | 272 | 273 | The airspeed measured by sensors and IMU 274 | Timestamp (milliseconds since system boot) 275 | Airspeed estimate from IMU, cm/s 276 | Pitot measured forward airpseed, cm/s 277 | Hot wire anenometer measured airspeed, cm/s 278 | Ultrasonic measured airspeed, cm/s 279 | Angle of attack sensor, degrees * 10 280 | Yaw angle sensor, degrees * 10 281 | 282 | 283 | 284 | 285 | -------------------------------------------------------------------------------- /mavlink/asluav.go: -------------------------------------------------------------------------------- 1 | package mavlink 2 | 3 | import ( 4 | "encoding/binary" 5 | "fmt" 6 | "math" 7 | ) 8 | 9 | ////////////////////////////////////////////////// 10 | // 11 | // NOTE: do not edit, 12 | // this file created automatically by mavgen.go 13 | // 14 | ////////////////////////////////////////////////// 15 | 16 | // MavCmd: 17 | const ( 18 | MAV_CMD_RESET_MPPT = 0 // Mission command to reset Maximum Power Point Tracker (MPPT) 19 | MAV_CMD_PAYLOAD_CONTROL = 1 // Mission command to perform a power cycle on payload 20 | ) 21 | 22 | // Voltage and current sensor data 23 | type SensPower struct { 24 | Adc121VspbVolt float32 // Power board voltage sensor reading in volts 25 | Adc121CspbAmp float32 // Power board current sensor reading in amps 26 | Adc121Cs1Amp float32 // Board current sensor 1 reading in amps 27 | Adc121Cs2Amp float32 // Board current sensor 2 reading in amps 28 | } 29 | 30 | func (self *SensPower) MsgID() uint8 { 31 | return 201 32 | } 33 | 34 | func (self *SensPower) MsgName() string { 35 | return "SensPower" 36 | } 37 | 38 | func (self *SensPower) Pack(p *Packet) error { 39 | payload := make([]byte, 16) 40 | binary.LittleEndian.PutUint32(payload[0:], math.Float32bits(self.Adc121VspbVolt)) 41 | binary.LittleEndian.PutUint32(payload[4:], math.Float32bits(self.Adc121CspbAmp)) 42 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.Adc121Cs1Amp)) 43 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.Adc121Cs2Amp)) 44 | 45 | p.MsgID = self.MsgID() 46 | p.Payload = payload 47 | return nil 48 | } 49 | 50 | func (self *SensPower) Unpack(p *Packet) error { 51 | if len(p.Payload) < 16 { 52 | return fmt.Errorf("payload too small") 53 | } 54 | self.Adc121VspbVolt = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[0:])) 55 | self.Adc121CspbAmp = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[4:])) 56 | self.Adc121Cs1Amp = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 57 | self.Adc121Cs2Amp = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 58 | return nil 59 | } 60 | 61 | // Maximum Power Point Tracker (MPPT) sensor data for solar module power performance tracking 62 | type SensMppt struct { 63 | MpptTimestamp uint64 // MPPT last timestamp 64 | Mppt1Volt float32 // MPPT1 voltage 65 | Mppt1Amp float32 // MPPT1 current 66 | Mppt2Volt float32 // MPPT2 voltage 67 | Mppt2Amp float32 // MPPT2 current 68 | Mppt3Volt float32 // MPPT3 voltage 69 | Mppt3Amp float32 // MPPT3 current 70 | Mppt1Pwm uint16 // MPPT1 pwm 71 | Mppt2Pwm uint16 // MPPT2 pwm 72 | Mppt3Pwm uint16 // MPPT3 pwm 73 | Mppt1Status uint8 // MPPT1 status 74 | Mppt2Status uint8 // MPPT2 status 75 | Mppt3Status uint8 // MPPT3 status 76 | } 77 | 78 | func (self *SensMppt) MsgID() uint8 { 79 | return 202 80 | } 81 | 82 | func (self *SensMppt) MsgName() string { 83 | return "SensMppt" 84 | } 85 | 86 | func (self *SensMppt) Pack(p *Packet) error { 87 | payload := make([]byte, 41) 88 | binary.LittleEndian.PutUint64(payload[0:], uint64(self.MpptTimestamp)) 89 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.Mppt1Volt)) 90 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.Mppt1Amp)) 91 | binary.LittleEndian.PutUint32(payload[16:], math.Float32bits(self.Mppt2Volt)) 92 | binary.LittleEndian.PutUint32(payload[20:], math.Float32bits(self.Mppt2Amp)) 93 | binary.LittleEndian.PutUint32(payload[24:], math.Float32bits(self.Mppt3Volt)) 94 | binary.LittleEndian.PutUint32(payload[28:], math.Float32bits(self.Mppt3Amp)) 95 | binary.LittleEndian.PutUint16(payload[32:], uint16(self.Mppt1Pwm)) 96 | binary.LittleEndian.PutUint16(payload[34:], uint16(self.Mppt2Pwm)) 97 | binary.LittleEndian.PutUint16(payload[36:], uint16(self.Mppt3Pwm)) 98 | payload[38] = byte(self.Mppt1Status) 99 | payload[39] = byte(self.Mppt2Status) 100 | payload[40] = byte(self.Mppt3Status) 101 | 102 | p.MsgID = self.MsgID() 103 | p.Payload = payload 104 | return nil 105 | } 106 | 107 | func (self *SensMppt) Unpack(p *Packet) error { 108 | if len(p.Payload) < 41 { 109 | return fmt.Errorf("payload too small") 110 | } 111 | self.MpptTimestamp = uint64(binary.LittleEndian.Uint64(p.Payload[0:])) 112 | self.Mppt1Volt = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 113 | self.Mppt1Amp = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 114 | self.Mppt2Volt = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[16:])) 115 | self.Mppt2Amp = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[20:])) 116 | self.Mppt3Volt = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[24:])) 117 | self.Mppt3Amp = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[28:])) 118 | self.Mppt1Pwm = uint16(binary.LittleEndian.Uint16(p.Payload[32:])) 119 | self.Mppt2Pwm = uint16(binary.LittleEndian.Uint16(p.Payload[34:])) 120 | self.Mppt3Pwm = uint16(binary.LittleEndian.Uint16(p.Payload[36:])) 121 | self.Mppt1Status = uint8(p.Payload[38]) 122 | self.Mppt2Status = uint8(p.Payload[39]) 123 | self.Mppt3Status = uint8(p.Payload[40]) 124 | return nil 125 | } 126 | 127 | // ASL-fixed-wing controller data 128 | type AslctrlData struct { 129 | Timestamp uint64 // Timestamp 130 | H float32 // See sourcecode for a description of these values... 131 | Href float32 // 132 | HrefT float32 // 133 | Pitchangle float32 // Pitch angle [deg] 134 | Pitchangleref float32 // Pitch angle reference[deg] 135 | Q float32 // 136 | Qref float32 // 137 | Uelev float32 // 138 | Uthrot float32 // 139 | Uthrot2 float32 // 140 | Az float32 // 141 | Airspeedref float32 // Airspeed reference [m/s] 142 | Yawangle float32 // Yaw angle [deg] 143 | Yawangleref float32 // Yaw angle reference[deg] 144 | Rollangle float32 // Roll angle [deg] 145 | Rollangleref float32 // Roll angle reference[deg] 146 | P float32 // 147 | Pref float32 // 148 | R float32 // 149 | Rref float32 // 150 | Uail float32 // 151 | Urud float32 // 152 | AslctrlMode uint8 // ASLCTRL control-mode (manual, stabilized, auto, etc...) 153 | Spoilersengaged uint8 // 154 | } 155 | 156 | func (self *AslctrlData) MsgID() uint8 { 157 | return 203 158 | } 159 | 160 | func (self *AslctrlData) MsgName() string { 161 | return "AslctrlData" 162 | } 163 | 164 | func (self *AslctrlData) Pack(p *Packet) error { 165 | payload := make([]byte, 98) 166 | binary.LittleEndian.PutUint64(payload[0:], uint64(self.Timestamp)) 167 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.H)) 168 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.Href)) 169 | binary.LittleEndian.PutUint32(payload[16:], math.Float32bits(self.HrefT)) 170 | binary.LittleEndian.PutUint32(payload[20:], math.Float32bits(self.Pitchangle)) 171 | binary.LittleEndian.PutUint32(payload[24:], math.Float32bits(self.Pitchangleref)) 172 | binary.LittleEndian.PutUint32(payload[28:], math.Float32bits(self.Q)) 173 | binary.LittleEndian.PutUint32(payload[32:], math.Float32bits(self.Qref)) 174 | binary.LittleEndian.PutUint32(payload[36:], math.Float32bits(self.Uelev)) 175 | binary.LittleEndian.PutUint32(payload[40:], math.Float32bits(self.Uthrot)) 176 | binary.LittleEndian.PutUint32(payload[44:], math.Float32bits(self.Uthrot2)) 177 | binary.LittleEndian.PutUint32(payload[48:], math.Float32bits(self.Az)) 178 | binary.LittleEndian.PutUint32(payload[52:], math.Float32bits(self.Airspeedref)) 179 | binary.LittleEndian.PutUint32(payload[56:], math.Float32bits(self.Yawangle)) 180 | binary.LittleEndian.PutUint32(payload[60:], math.Float32bits(self.Yawangleref)) 181 | binary.LittleEndian.PutUint32(payload[64:], math.Float32bits(self.Rollangle)) 182 | binary.LittleEndian.PutUint32(payload[68:], math.Float32bits(self.Rollangleref)) 183 | binary.LittleEndian.PutUint32(payload[72:], math.Float32bits(self.P)) 184 | binary.LittleEndian.PutUint32(payload[76:], math.Float32bits(self.Pref)) 185 | binary.LittleEndian.PutUint32(payload[80:], math.Float32bits(self.R)) 186 | binary.LittleEndian.PutUint32(payload[84:], math.Float32bits(self.Rref)) 187 | binary.LittleEndian.PutUint32(payload[88:], math.Float32bits(self.Uail)) 188 | binary.LittleEndian.PutUint32(payload[92:], math.Float32bits(self.Urud)) 189 | payload[96] = byte(self.AslctrlMode) 190 | payload[97] = byte(self.Spoilersengaged) 191 | 192 | p.MsgID = self.MsgID() 193 | p.Payload = payload 194 | return nil 195 | } 196 | 197 | func (self *AslctrlData) Unpack(p *Packet) error { 198 | if len(p.Payload) < 98 { 199 | return fmt.Errorf("payload too small") 200 | } 201 | self.Timestamp = uint64(binary.LittleEndian.Uint64(p.Payload[0:])) 202 | self.H = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 203 | self.Href = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 204 | self.HrefT = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[16:])) 205 | self.Pitchangle = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[20:])) 206 | self.Pitchangleref = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[24:])) 207 | self.Q = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[28:])) 208 | self.Qref = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[32:])) 209 | self.Uelev = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[36:])) 210 | self.Uthrot = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[40:])) 211 | self.Uthrot2 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[44:])) 212 | self.Az = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[48:])) 213 | self.Airspeedref = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[52:])) 214 | self.Yawangle = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[56:])) 215 | self.Yawangleref = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[60:])) 216 | self.Rollangle = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[64:])) 217 | self.Rollangleref = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[68:])) 218 | self.P = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[72:])) 219 | self.Pref = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[76:])) 220 | self.R = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[80:])) 221 | self.Rref = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[84:])) 222 | self.Uail = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[88:])) 223 | self.Urud = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[92:])) 224 | self.AslctrlMode = uint8(p.Payload[96]) 225 | self.Spoilersengaged = uint8(p.Payload[97]) 226 | return nil 227 | } 228 | 229 | // ASL-fixed-wing controller debug data 230 | type AslctrlDebug struct { 231 | I321 uint32 // Debug data 232 | F1 float32 // Debug data 233 | F2 float32 // Debug data 234 | F3 float32 // Debug data 235 | F4 float32 // Debug data 236 | F5 float32 // Debug data 237 | F6 float32 // Debug data 238 | F7 float32 // Debug data 239 | F8 float32 // Debug data 240 | I81 uint8 // Debug data 241 | I82 uint8 // Debug data 242 | } 243 | 244 | func (self *AslctrlDebug) MsgID() uint8 { 245 | return 204 246 | } 247 | 248 | func (self *AslctrlDebug) MsgName() string { 249 | return "AslctrlDebug" 250 | } 251 | 252 | func (self *AslctrlDebug) Pack(p *Packet) error { 253 | payload := make([]byte, 38) 254 | binary.LittleEndian.PutUint32(payload[0:], uint32(self.I321)) 255 | binary.LittleEndian.PutUint32(payload[4:], math.Float32bits(self.F1)) 256 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.F2)) 257 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.F3)) 258 | binary.LittleEndian.PutUint32(payload[16:], math.Float32bits(self.F4)) 259 | binary.LittleEndian.PutUint32(payload[20:], math.Float32bits(self.F5)) 260 | binary.LittleEndian.PutUint32(payload[24:], math.Float32bits(self.F6)) 261 | binary.LittleEndian.PutUint32(payload[28:], math.Float32bits(self.F7)) 262 | binary.LittleEndian.PutUint32(payload[32:], math.Float32bits(self.F8)) 263 | payload[36] = byte(self.I81) 264 | payload[37] = byte(self.I82) 265 | 266 | p.MsgID = self.MsgID() 267 | p.Payload = payload 268 | return nil 269 | } 270 | 271 | func (self *AslctrlDebug) Unpack(p *Packet) error { 272 | if len(p.Payload) < 38 { 273 | return fmt.Errorf("payload too small") 274 | } 275 | self.I321 = uint32(binary.LittleEndian.Uint32(p.Payload[0:])) 276 | self.F1 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[4:])) 277 | self.F2 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 278 | self.F3 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 279 | self.F4 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[16:])) 280 | self.F5 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[20:])) 281 | self.F6 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[24:])) 282 | self.F7 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[28:])) 283 | self.F8 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[32:])) 284 | self.I81 = uint8(p.Payload[36]) 285 | self.I82 = uint8(p.Payload[37]) 286 | return nil 287 | } 288 | 289 | // Extended state information for ASLUAVs 290 | type AsluavStatus struct { 291 | MotorRpm float32 // Motor RPM 292 | LedStatus uint8 // Status of the position-indicator LEDs 293 | SatcomStatus uint8 // Status of the IRIDIUM satellite communication system 294 | ServoStatus [8]uint8 // Status vector for up to 8 servos 295 | } 296 | 297 | func (self *AsluavStatus) MsgID() uint8 { 298 | return 205 299 | } 300 | 301 | func (self *AsluavStatus) MsgName() string { 302 | return "AsluavStatus" 303 | } 304 | 305 | func (self *AsluavStatus) Pack(p *Packet) error { 306 | payload := make([]byte, 14) 307 | binary.LittleEndian.PutUint32(payload[0:], math.Float32bits(self.MotorRpm)) 308 | payload[4] = byte(self.LedStatus) 309 | payload[5] = byte(self.SatcomStatus) 310 | copy(payload[6:], self.ServoStatus[:]) 311 | 312 | p.MsgID = self.MsgID() 313 | p.Payload = payload 314 | return nil 315 | } 316 | 317 | func (self *AsluavStatus) Unpack(p *Packet) error { 318 | if len(p.Payload) < 14 { 319 | return fmt.Errorf("payload too small") 320 | } 321 | self.MotorRpm = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[0:])) 322 | self.LedStatus = uint8(p.Payload[4]) 323 | self.SatcomStatus = uint8(p.Payload[5]) 324 | copy(self.ServoStatus[:], p.Payload[6:14]) 325 | return nil 326 | } 327 | 328 | // Extended EKF state estimates for ASLUAVs 329 | type EkfExt struct { 330 | Timestamp uint64 // Time since system start [us] 331 | Windspeed float32 // Magnitude of wind velocity (in lateral inertial plane) [m/s] 332 | Winddir float32 // Wind heading angle from North [rad] 333 | Windz float32 // Z (Down) component of inertial wind velocity [m/s] 334 | Airspeed float32 // Magnitude of air velocity [m/s] 335 | Beta float32 // Sideslip angle [rad] 336 | Alpha float32 // Angle of attack [rad] 337 | } 338 | 339 | func (self *EkfExt) MsgID() uint8 { 340 | return 206 341 | } 342 | 343 | func (self *EkfExt) MsgName() string { 344 | return "EkfExt" 345 | } 346 | 347 | func (self *EkfExt) Pack(p *Packet) error { 348 | payload := make([]byte, 32) 349 | binary.LittleEndian.PutUint64(payload[0:], uint64(self.Timestamp)) 350 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.Windspeed)) 351 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.Winddir)) 352 | binary.LittleEndian.PutUint32(payload[16:], math.Float32bits(self.Windz)) 353 | binary.LittleEndian.PutUint32(payload[20:], math.Float32bits(self.Airspeed)) 354 | binary.LittleEndian.PutUint32(payload[24:], math.Float32bits(self.Beta)) 355 | binary.LittleEndian.PutUint32(payload[28:], math.Float32bits(self.Alpha)) 356 | 357 | p.MsgID = self.MsgID() 358 | p.Payload = payload 359 | return nil 360 | } 361 | 362 | func (self *EkfExt) Unpack(p *Packet) error { 363 | if len(p.Payload) < 32 { 364 | return fmt.Errorf("payload too small") 365 | } 366 | self.Timestamp = uint64(binary.LittleEndian.Uint64(p.Payload[0:])) 367 | self.Windspeed = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 368 | self.Winddir = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 369 | self.Windz = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[16:])) 370 | self.Airspeed = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[20:])) 371 | self.Beta = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[24:])) 372 | self.Alpha = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[28:])) 373 | return nil 374 | } 375 | 376 | // Off-board controls/commands for ASLUAVs 377 | type AslObctrl struct { 378 | Timestamp uint64 // Time since system start [us] 379 | Uelev float32 // Elevator command [~] 380 | Uthrot float32 // Throttle command [~] 381 | Uthrot2 float32 // Throttle 2 command [~] 382 | Uaill float32 // Left aileron command [~] 383 | Uailr float32 // Right aileron command [~] 384 | Urud float32 // Rudder command [~] 385 | ObctrlStatus uint8 // Off-board computer status 386 | } 387 | 388 | func (self *AslObctrl) MsgID() uint8 { 389 | return 207 390 | } 391 | 392 | func (self *AslObctrl) MsgName() string { 393 | return "AslObctrl" 394 | } 395 | 396 | func (self *AslObctrl) Pack(p *Packet) error { 397 | payload := make([]byte, 33) 398 | binary.LittleEndian.PutUint64(payload[0:], uint64(self.Timestamp)) 399 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.Uelev)) 400 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.Uthrot)) 401 | binary.LittleEndian.PutUint32(payload[16:], math.Float32bits(self.Uthrot2)) 402 | binary.LittleEndian.PutUint32(payload[20:], math.Float32bits(self.Uaill)) 403 | binary.LittleEndian.PutUint32(payload[24:], math.Float32bits(self.Uailr)) 404 | binary.LittleEndian.PutUint32(payload[28:], math.Float32bits(self.Urud)) 405 | payload[32] = byte(self.ObctrlStatus) 406 | 407 | p.MsgID = self.MsgID() 408 | p.Payload = payload 409 | return nil 410 | } 411 | 412 | func (self *AslObctrl) Unpack(p *Packet) error { 413 | if len(p.Payload) < 33 { 414 | return fmt.Errorf("payload too small") 415 | } 416 | self.Timestamp = uint64(binary.LittleEndian.Uint64(p.Payload[0:])) 417 | self.Uelev = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 418 | self.Uthrot = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 419 | self.Uthrot2 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[16:])) 420 | self.Uaill = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[20:])) 421 | self.Uailr = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[24:])) 422 | self.Urud = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[28:])) 423 | self.ObctrlStatus = uint8(p.Payload[32]) 424 | return nil 425 | } 426 | 427 | // Atmospheric sensors (temperature, humidity, ...) 428 | type SensAtmos struct { 429 | Tempambient float32 // Ambient temperature [degrees Celsius] 430 | Humidity float32 // Relative humidity [%] 431 | } 432 | 433 | func (self *SensAtmos) MsgID() uint8 { 434 | return 208 435 | } 436 | 437 | func (self *SensAtmos) MsgName() string { 438 | return "SensAtmos" 439 | } 440 | 441 | func (self *SensAtmos) Pack(p *Packet) error { 442 | payload := make([]byte, 8) 443 | binary.LittleEndian.PutUint32(payload[0:], math.Float32bits(self.Tempambient)) 444 | binary.LittleEndian.PutUint32(payload[4:], math.Float32bits(self.Humidity)) 445 | 446 | p.MsgID = self.MsgID() 447 | p.Payload = payload 448 | return nil 449 | } 450 | 451 | func (self *SensAtmos) Unpack(p *Packet) error { 452 | if len(p.Payload) < 8 { 453 | return fmt.Errorf("payload too small") 454 | } 455 | self.Tempambient = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[0:])) 456 | self.Humidity = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[4:])) 457 | return nil 458 | } 459 | 460 | // Battery pack monitoring data for Li-Ion batteries 461 | type SensBatmon struct { 462 | Temperature float32 // Battery pack temperature in [deg C] 463 | Voltage uint16 // Battery pack voltage in [mV] 464 | Current int16 // Battery pack current in [mA] 465 | Batterystatus uint16 // Battery monitor status report bits in Hex 466 | Serialnumber uint16 // Battery monitor serial number in Hex 467 | Hostfetcontrol uint16 // Battery monitor sensor host FET control in Hex 468 | Cellvoltage1 uint16 // Battery pack cell 1 voltage in [mV] 469 | Cellvoltage2 uint16 // Battery pack cell 2 voltage in [mV] 470 | Cellvoltage3 uint16 // Battery pack cell 3 voltage in [mV] 471 | Cellvoltage4 uint16 // Battery pack cell 4 voltage in [mV] 472 | Cellvoltage5 uint16 // Battery pack cell 5 voltage in [mV] 473 | Cellvoltage6 uint16 // Battery pack cell 6 voltage in [mV] 474 | Soc uint8 // Battery pack state-of-charge 475 | } 476 | 477 | func (self *SensBatmon) MsgID() uint8 { 478 | return 209 479 | } 480 | 481 | func (self *SensBatmon) MsgName() string { 482 | return "SensBatmon" 483 | } 484 | 485 | func (self *SensBatmon) Pack(p *Packet) error { 486 | payload := make([]byte, 27) 487 | binary.LittleEndian.PutUint32(payload[0:], math.Float32bits(self.Temperature)) 488 | binary.LittleEndian.PutUint16(payload[4:], uint16(self.Voltage)) 489 | binary.LittleEndian.PutUint16(payload[6:], uint16(self.Current)) 490 | binary.LittleEndian.PutUint16(payload[8:], uint16(self.Batterystatus)) 491 | binary.LittleEndian.PutUint16(payload[10:], uint16(self.Serialnumber)) 492 | binary.LittleEndian.PutUint16(payload[12:], uint16(self.Hostfetcontrol)) 493 | binary.LittleEndian.PutUint16(payload[14:], uint16(self.Cellvoltage1)) 494 | binary.LittleEndian.PutUint16(payload[16:], uint16(self.Cellvoltage2)) 495 | binary.LittleEndian.PutUint16(payload[18:], uint16(self.Cellvoltage3)) 496 | binary.LittleEndian.PutUint16(payload[20:], uint16(self.Cellvoltage4)) 497 | binary.LittleEndian.PutUint16(payload[22:], uint16(self.Cellvoltage5)) 498 | binary.LittleEndian.PutUint16(payload[24:], uint16(self.Cellvoltage6)) 499 | payload[26] = byte(self.Soc) 500 | 501 | p.MsgID = self.MsgID() 502 | p.Payload = payload 503 | return nil 504 | } 505 | 506 | func (self *SensBatmon) Unpack(p *Packet) error { 507 | if len(p.Payload) < 27 { 508 | return fmt.Errorf("payload too small") 509 | } 510 | self.Temperature = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[0:])) 511 | self.Voltage = uint16(binary.LittleEndian.Uint16(p.Payload[4:])) 512 | self.Current = int16(binary.LittleEndian.Uint16(p.Payload[6:])) 513 | self.Batterystatus = uint16(binary.LittleEndian.Uint16(p.Payload[8:])) 514 | self.Serialnumber = uint16(binary.LittleEndian.Uint16(p.Payload[10:])) 515 | self.Hostfetcontrol = uint16(binary.LittleEndian.Uint16(p.Payload[12:])) 516 | self.Cellvoltage1 = uint16(binary.LittleEndian.Uint16(p.Payload[14:])) 517 | self.Cellvoltage2 = uint16(binary.LittleEndian.Uint16(p.Payload[16:])) 518 | self.Cellvoltage3 = uint16(binary.LittleEndian.Uint16(p.Payload[18:])) 519 | self.Cellvoltage4 = uint16(binary.LittleEndian.Uint16(p.Payload[20:])) 520 | self.Cellvoltage5 = uint16(binary.LittleEndian.Uint16(p.Payload[22:])) 521 | self.Cellvoltage6 = uint16(binary.LittleEndian.Uint16(p.Payload[24:])) 522 | self.Soc = uint8(p.Payload[26]) 523 | return nil 524 | } 525 | 526 | // Fixed-wing soaring (i.e. thermal seeking) data 527 | type FwSoaringData struct { 528 | Timestamp uint64 // Timestamp [ms] 529 | Timestampmodechanged uint64 // Timestamp since last mode change[ms] 530 | Currentupdraftspeed float32 // Updraft speed at current/local airplane position [m/s] 531 | Xw float32 // Thermal core updraft strength [m/s] 532 | Xr float32 // Thermal radius [m] 533 | Xlat float32 // Thermal center latitude [deg] 534 | Xlon float32 // Thermal center longitude [deg] 535 | Varw float32 // Variance W 536 | Varr float32 // Variance R 537 | Varlat float32 // Variance Lat 538 | Varlon float32 // Variance Lon 539 | Loiterradius float32 // Suggested loiter radius [m] 540 | Controlmode uint8 // Control Mode [-] 541 | Valid uint8 // Data valid [-] 542 | } 543 | 544 | func (self *FwSoaringData) MsgID() uint8 { 545 | return 210 546 | } 547 | 548 | func (self *FwSoaringData) MsgName() string { 549 | return "FwSoaringData" 550 | } 551 | 552 | func (self *FwSoaringData) Pack(p *Packet) error { 553 | payload := make([]byte, 58) 554 | binary.LittleEndian.PutUint64(payload[0:], uint64(self.Timestamp)) 555 | binary.LittleEndian.PutUint64(payload[8:], uint64(self.Timestampmodechanged)) 556 | binary.LittleEndian.PutUint32(payload[16:], math.Float32bits(self.Currentupdraftspeed)) 557 | binary.LittleEndian.PutUint32(payload[20:], math.Float32bits(self.Xw)) 558 | binary.LittleEndian.PutUint32(payload[24:], math.Float32bits(self.Xr)) 559 | binary.LittleEndian.PutUint32(payload[28:], math.Float32bits(self.Xlat)) 560 | binary.LittleEndian.PutUint32(payload[32:], math.Float32bits(self.Xlon)) 561 | binary.LittleEndian.PutUint32(payload[36:], math.Float32bits(self.Varw)) 562 | binary.LittleEndian.PutUint32(payload[40:], math.Float32bits(self.Varr)) 563 | binary.LittleEndian.PutUint32(payload[44:], math.Float32bits(self.Varlat)) 564 | binary.LittleEndian.PutUint32(payload[48:], math.Float32bits(self.Varlon)) 565 | binary.LittleEndian.PutUint32(payload[52:], math.Float32bits(self.Loiterradius)) 566 | payload[56] = byte(self.Controlmode) 567 | payload[57] = byte(self.Valid) 568 | 569 | p.MsgID = self.MsgID() 570 | p.Payload = payload 571 | return nil 572 | } 573 | 574 | func (self *FwSoaringData) Unpack(p *Packet) error { 575 | if len(p.Payload) < 58 { 576 | return fmt.Errorf("payload too small") 577 | } 578 | self.Timestamp = uint64(binary.LittleEndian.Uint64(p.Payload[0:])) 579 | self.Timestampmodechanged = uint64(binary.LittleEndian.Uint64(p.Payload[8:])) 580 | self.Currentupdraftspeed = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[16:])) 581 | self.Xw = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[20:])) 582 | self.Xr = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[24:])) 583 | self.Xlat = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[28:])) 584 | self.Xlon = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[32:])) 585 | self.Varw = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[36:])) 586 | self.Varr = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[40:])) 587 | self.Varlat = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[44:])) 588 | self.Varlon = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[48:])) 589 | self.Loiterradius = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[52:])) 590 | self.Controlmode = uint8(p.Payload[56]) 591 | self.Valid = uint8(p.Payload[57]) 592 | return nil 593 | } 594 | 595 | // Monitoring of sensorpod status 596 | type SensorpodStatus struct { 597 | Timestamp uint64 // Timestamp in linuxtime [ms] (since 1.1.1970) 598 | FreeSpace uint16 // Free space available in recordings directory in [Gb] * 1e2 599 | VisensorRate1 uint8 // Rate of ROS topic 1 600 | VisensorRate2 uint8 // Rate of ROS topic 2 601 | VisensorRate3 uint8 // Rate of ROS topic 3 602 | VisensorRate4 uint8 // Rate of ROS topic 4 603 | RecordingNodesCount uint8 // Number of recording nodes 604 | CpuTemp uint8 // Temperature of sensorpod CPU in [deg C] 605 | } 606 | 607 | func (self *SensorpodStatus) MsgID() uint8 { 608 | return 211 609 | } 610 | 611 | func (self *SensorpodStatus) MsgName() string { 612 | return "SensorpodStatus" 613 | } 614 | 615 | func (self *SensorpodStatus) Pack(p *Packet) error { 616 | payload := make([]byte, 16) 617 | binary.LittleEndian.PutUint64(payload[0:], uint64(self.Timestamp)) 618 | binary.LittleEndian.PutUint16(payload[8:], uint16(self.FreeSpace)) 619 | payload[10] = byte(self.VisensorRate1) 620 | payload[11] = byte(self.VisensorRate2) 621 | payload[12] = byte(self.VisensorRate3) 622 | payload[13] = byte(self.VisensorRate4) 623 | payload[14] = byte(self.RecordingNodesCount) 624 | payload[15] = byte(self.CpuTemp) 625 | 626 | p.MsgID = self.MsgID() 627 | p.Payload = payload 628 | return nil 629 | } 630 | 631 | func (self *SensorpodStatus) Unpack(p *Packet) error { 632 | if len(p.Payload) < 16 { 633 | return fmt.Errorf("payload too small") 634 | } 635 | self.Timestamp = uint64(binary.LittleEndian.Uint64(p.Payload[0:])) 636 | self.FreeSpace = uint16(binary.LittleEndian.Uint16(p.Payload[8:])) 637 | self.VisensorRate1 = uint8(p.Payload[10]) 638 | self.VisensorRate2 = uint8(p.Payload[11]) 639 | self.VisensorRate3 = uint8(p.Payload[12]) 640 | self.VisensorRate4 = uint8(p.Payload[13]) 641 | self.RecordingNodesCount = uint8(p.Payload[14]) 642 | self.CpuTemp = uint8(p.Payload[15]) 643 | return nil 644 | } 645 | 646 | // Message IDs 647 | const ( 648 | MSG_ID_SENS_POWER = 201 649 | MSG_ID_SENS_MPPT = 202 650 | MSG_ID_ASLCTRL_DATA = 203 651 | MSG_ID_ASLCTRL_DEBUG = 204 652 | MSG_ID_ASLUAV_STATUS = 205 653 | MSG_ID_EKF_EXT = 206 654 | MSG_ID_ASL_OBCTRL = 207 655 | MSG_ID_SENS_ATMOS = 208 656 | MSG_ID_SENS_BATMON = 209 657 | MSG_ID_FW_SOARING_DATA = 210 658 | MSG_ID_SENSORPOD_STATUS = 211 659 | ) 660 | 661 | // DialectAsluav is the dialect represented by ASLUAV.xml 662 | var DialectAsluav *Dialect = &Dialect{ 663 | Name: "ASLUAV", 664 | crcExtras: map[uint8]uint8{ 665 | 201: 218, // MSG_ID_SENS_POWER 666 | 202: 231, // MSG_ID_SENS_MPPT 667 | 203: 0, // MSG_ID_ASLCTRL_DATA 668 | 204: 251, // MSG_ID_ASLCTRL_DEBUG 669 | 205: 97, // MSG_ID_ASLUAV_STATUS 670 | 206: 64, // MSG_ID_EKF_EXT 671 | 207: 234, // MSG_ID_ASL_OBCTRL 672 | 208: 175, // MSG_ID_SENS_ATMOS 673 | 209: 62, // MSG_ID_SENS_BATMON 674 | 210: 129, // MSG_ID_FW_SOARING_DATA 675 | 211: 54, // MSG_ID_SENSORPOD_STATUS 676 | }, 677 | } 678 | -------------------------------------------------------------------------------- /mavlink/pixhawk.go: -------------------------------------------------------------------------------- 1 | package mavlink 2 | 3 | import ( 4 | "encoding/binary" 5 | "fmt" 6 | "math" 7 | ) 8 | 9 | ////////////////////////////////////////////////// 10 | // 11 | // NOTE: do not edit, 12 | // this file created automatically by mavgen.go 13 | // 14 | ////////////////////////////////////////////////// 15 | 16 | // DataTypes: Content Types for data transmission handshake 17 | const ( 18 | DATA_TYPE_JPEG_IMAGE = 1 // 19 | DATA_TYPE_RAW_IMAGE = 2 // 20 | DATA_TYPE_KINECT = 3 // 21 | ) 22 | 23 | // MavCmd: 24 | const ( 25 | MAV_CMD_DO_START_SEARCH = 0 // Starts a search 26 | MAV_CMD_DO_FINISH_SEARCH = 1 // Starts a search 27 | MAV_CMD_NAV_SWEEP = 2 // Starts a search 28 | ) 29 | 30 | // 31 | type SetCamShutter struct { 32 | Gain float32 // Camera gain 33 | Interval uint16 // Shutter interval, in microseconds 34 | Exposure uint16 // Exposure time, in microseconds 35 | CamNo uint8 // Camera id 36 | CamMode uint8 // Camera mode: 0 = auto, 1 = manual 37 | TriggerPin uint8 // Trigger pin, 0-3 for PtGrey FireFly 38 | } 39 | 40 | func (self *SetCamShutter) MsgID() uint8 { 41 | return 151 42 | } 43 | 44 | func (self *SetCamShutter) MsgName() string { 45 | return "SetCamShutter" 46 | } 47 | 48 | func (self *SetCamShutter) Pack(p *Packet) error { 49 | payload := make([]byte, 11) 50 | binary.LittleEndian.PutUint32(payload[0:], math.Float32bits(self.Gain)) 51 | binary.LittleEndian.PutUint16(payload[4:], uint16(self.Interval)) 52 | binary.LittleEndian.PutUint16(payload[6:], uint16(self.Exposure)) 53 | payload[8] = byte(self.CamNo) 54 | payload[9] = byte(self.CamMode) 55 | payload[10] = byte(self.TriggerPin) 56 | 57 | p.MsgID = self.MsgID() 58 | p.Payload = payload 59 | return nil 60 | } 61 | 62 | func (self *SetCamShutter) Unpack(p *Packet) error { 63 | if len(p.Payload) < 11 { 64 | return fmt.Errorf("payload too small") 65 | } 66 | self.Gain = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[0:])) 67 | self.Interval = uint16(binary.LittleEndian.Uint16(p.Payload[4:])) 68 | self.Exposure = uint16(binary.LittleEndian.Uint16(p.Payload[6:])) 69 | self.CamNo = uint8(p.Payload[8]) 70 | self.CamMode = uint8(p.Payload[9]) 71 | self.TriggerPin = uint8(p.Payload[10]) 72 | return nil 73 | } 74 | 75 | // 76 | type ImageTriggered struct { 77 | Timestamp uint64 // Timestamp 78 | Seq uint32 // IMU seq 79 | Roll float32 // Roll angle in rad 80 | Pitch float32 // Pitch angle in rad 81 | Yaw float32 // Yaw angle in rad 82 | LocalZ float32 // Local frame Z coordinate (height over ground) 83 | Lat float32 // GPS X coordinate 84 | Lon float32 // GPS Y coordinate 85 | Alt float32 // Global frame altitude 86 | GroundX float32 // Ground truth X 87 | GroundY float32 // Ground truth Y 88 | GroundZ float32 // Ground truth Z 89 | } 90 | 91 | func (self *ImageTriggered) MsgID() uint8 { 92 | return 152 93 | } 94 | 95 | func (self *ImageTriggered) MsgName() string { 96 | return "ImageTriggered" 97 | } 98 | 99 | func (self *ImageTriggered) Pack(p *Packet) error { 100 | payload := make([]byte, 52) 101 | binary.LittleEndian.PutUint64(payload[0:], uint64(self.Timestamp)) 102 | binary.LittleEndian.PutUint32(payload[8:], uint32(self.Seq)) 103 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.Roll)) 104 | binary.LittleEndian.PutUint32(payload[16:], math.Float32bits(self.Pitch)) 105 | binary.LittleEndian.PutUint32(payload[20:], math.Float32bits(self.Yaw)) 106 | binary.LittleEndian.PutUint32(payload[24:], math.Float32bits(self.LocalZ)) 107 | binary.LittleEndian.PutUint32(payload[28:], math.Float32bits(self.Lat)) 108 | binary.LittleEndian.PutUint32(payload[32:], math.Float32bits(self.Lon)) 109 | binary.LittleEndian.PutUint32(payload[36:], math.Float32bits(self.Alt)) 110 | binary.LittleEndian.PutUint32(payload[40:], math.Float32bits(self.GroundX)) 111 | binary.LittleEndian.PutUint32(payload[44:], math.Float32bits(self.GroundY)) 112 | binary.LittleEndian.PutUint32(payload[48:], math.Float32bits(self.GroundZ)) 113 | 114 | p.MsgID = self.MsgID() 115 | p.Payload = payload 116 | return nil 117 | } 118 | 119 | func (self *ImageTriggered) Unpack(p *Packet) error { 120 | if len(p.Payload) < 52 { 121 | return fmt.Errorf("payload too small") 122 | } 123 | self.Timestamp = uint64(binary.LittleEndian.Uint64(p.Payload[0:])) 124 | self.Seq = uint32(binary.LittleEndian.Uint32(p.Payload[8:])) 125 | self.Roll = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 126 | self.Pitch = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[16:])) 127 | self.Yaw = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[20:])) 128 | self.LocalZ = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[24:])) 129 | self.Lat = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[28:])) 130 | self.Lon = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[32:])) 131 | self.Alt = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[36:])) 132 | self.GroundX = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[40:])) 133 | self.GroundY = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[44:])) 134 | self.GroundZ = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[48:])) 135 | return nil 136 | } 137 | 138 | // 139 | type ImageTriggerControl struct { 140 | Enable uint8 // 0 to disable, 1 to enable 141 | } 142 | 143 | func (self *ImageTriggerControl) MsgID() uint8 { 144 | return 153 145 | } 146 | 147 | func (self *ImageTriggerControl) MsgName() string { 148 | return "ImageTriggerControl" 149 | } 150 | 151 | func (self *ImageTriggerControl) Pack(p *Packet) error { 152 | payload := make([]byte, 1) 153 | payload[0] = byte(self.Enable) 154 | 155 | p.MsgID = self.MsgID() 156 | p.Payload = payload 157 | return nil 158 | } 159 | 160 | func (self *ImageTriggerControl) Unpack(p *Packet) error { 161 | if len(p.Payload) < 1 { 162 | return fmt.Errorf("payload too small") 163 | } 164 | self.Enable = uint8(p.Payload[0]) 165 | return nil 166 | } 167 | 168 | // 169 | type ImageAvailable struct { 170 | CamId uint64 // Camera id 171 | Timestamp uint64 // Timestamp 172 | ValidUntil uint64 // Until which timestamp this buffer will stay valid 173 | ImgSeq uint32 // The image sequence number 174 | ImgBufIndex uint32 // Position of the image in the buffer, starts with 0 175 | Key uint32 // Shared memory area key 176 | Exposure uint32 // Exposure time, in microseconds 177 | Gain float32 // Camera gain 178 | Roll float32 // Roll angle in rad 179 | Pitch float32 // Pitch angle in rad 180 | Yaw float32 // Yaw angle in rad 181 | LocalZ float32 // Local frame Z coordinate (height over ground) 182 | Lat float32 // GPS X coordinate 183 | Lon float32 // GPS Y coordinate 184 | Alt float32 // Global frame altitude 185 | GroundX float32 // Ground truth X 186 | GroundY float32 // Ground truth Y 187 | GroundZ float32 // Ground truth Z 188 | Width uint16 // Image width 189 | Height uint16 // Image height 190 | Depth uint16 // Image depth 191 | CamNo uint8 // Camera # (starts with 0) 192 | Channels uint8 // Image channels 193 | } 194 | 195 | func (self *ImageAvailable) MsgID() uint8 { 196 | return 154 197 | } 198 | 199 | func (self *ImageAvailable) MsgName() string { 200 | return "ImageAvailable" 201 | } 202 | 203 | func (self *ImageAvailable) Pack(p *Packet) error { 204 | payload := make([]byte, 92) 205 | binary.LittleEndian.PutUint64(payload[0:], uint64(self.CamId)) 206 | binary.LittleEndian.PutUint64(payload[8:], uint64(self.Timestamp)) 207 | binary.LittleEndian.PutUint64(payload[16:], uint64(self.ValidUntil)) 208 | binary.LittleEndian.PutUint32(payload[24:], uint32(self.ImgSeq)) 209 | binary.LittleEndian.PutUint32(payload[28:], uint32(self.ImgBufIndex)) 210 | binary.LittleEndian.PutUint32(payload[32:], uint32(self.Key)) 211 | binary.LittleEndian.PutUint32(payload[36:], uint32(self.Exposure)) 212 | binary.LittleEndian.PutUint32(payload[40:], math.Float32bits(self.Gain)) 213 | binary.LittleEndian.PutUint32(payload[44:], math.Float32bits(self.Roll)) 214 | binary.LittleEndian.PutUint32(payload[48:], math.Float32bits(self.Pitch)) 215 | binary.LittleEndian.PutUint32(payload[52:], math.Float32bits(self.Yaw)) 216 | binary.LittleEndian.PutUint32(payload[56:], math.Float32bits(self.LocalZ)) 217 | binary.LittleEndian.PutUint32(payload[60:], math.Float32bits(self.Lat)) 218 | binary.LittleEndian.PutUint32(payload[64:], math.Float32bits(self.Lon)) 219 | binary.LittleEndian.PutUint32(payload[68:], math.Float32bits(self.Alt)) 220 | binary.LittleEndian.PutUint32(payload[72:], math.Float32bits(self.GroundX)) 221 | binary.LittleEndian.PutUint32(payload[76:], math.Float32bits(self.GroundY)) 222 | binary.LittleEndian.PutUint32(payload[80:], math.Float32bits(self.GroundZ)) 223 | binary.LittleEndian.PutUint16(payload[84:], uint16(self.Width)) 224 | binary.LittleEndian.PutUint16(payload[86:], uint16(self.Height)) 225 | binary.LittleEndian.PutUint16(payload[88:], uint16(self.Depth)) 226 | payload[90] = byte(self.CamNo) 227 | payload[91] = byte(self.Channels) 228 | 229 | p.MsgID = self.MsgID() 230 | p.Payload = payload 231 | return nil 232 | } 233 | 234 | func (self *ImageAvailable) Unpack(p *Packet) error { 235 | if len(p.Payload) < 92 { 236 | return fmt.Errorf("payload too small") 237 | } 238 | self.CamId = uint64(binary.LittleEndian.Uint64(p.Payload[0:])) 239 | self.Timestamp = uint64(binary.LittleEndian.Uint64(p.Payload[8:])) 240 | self.ValidUntil = uint64(binary.LittleEndian.Uint64(p.Payload[16:])) 241 | self.ImgSeq = uint32(binary.LittleEndian.Uint32(p.Payload[24:])) 242 | self.ImgBufIndex = uint32(binary.LittleEndian.Uint32(p.Payload[28:])) 243 | self.Key = uint32(binary.LittleEndian.Uint32(p.Payload[32:])) 244 | self.Exposure = uint32(binary.LittleEndian.Uint32(p.Payload[36:])) 245 | self.Gain = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[40:])) 246 | self.Roll = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[44:])) 247 | self.Pitch = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[48:])) 248 | self.Yaw = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[52:])) 249 | self.LocalZ = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[56:])) 250 | self.Lat = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[60:])) 251 | self.Lon = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[64:])) 252 | self.Alt = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[68:])) 253 | self.GroundX = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[72:])) 254 | self.GroundY = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[76:])) 255 | self.GroundZ = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[80:])) 256 | self.Width = uint16(binary.LittleEndian.Uint16(p.Payload[84:])) 257 | self.Height = uint16(binary.LittleEndian.Uint16(p.Payload[86:])) 258 | self.Depth = uint16(binary.LittleEndian.Uint16(p.Payload[88:])) 259 | self.CamNo = uint8(p.Payload[90]) 260 | self.Channels = uint8(p.Payload[91]) 261 | return nil 262 | } 263 | 264 | // Message sent to the MAV to set a new offset from the currently controlled position 265 | type SetPositionControlOffset struct { 266 | X float32 // x position offset 267 | Y float32 // y position offset 268 | Z float32 // z position offset 269 | Yaw float32 // yaw orientation offset in radians, 0 = NORTH 270 | TargetSystem uint8 // System ID 271 | TargetComponent uint8 // Component ID 272 | } 273 | 274 | func (self *SetPositionControlOffset) MsgID() uint8 { 275 | return 160 276 | } 277 | 278 | func (self *SetPositionControlOffset) MsgName() string { 279 | return "SetPositionControlOffset" 280 | } 281 | 282 | func (self *SetPositionControlOffset) Pack(p *Packet) error { 283 | payload := make([]byte, 18) 284 | binary.LittleEndian.PutUint32(payload[0:], math.Float32bits(self.X)) 285 | binary.LittleEndian.PutUint32(payload[4:], math.Float32bits(self.Y)) 286 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.Z)) 287 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.Yaw)) 288 | payload[16] = byte(self.TargetSystem) 289 | payload[17] = byte(self.TargetComponent) 290 | 291 | p.MsgID = self.MsgID() 292 | p.Payload = payload 293 | return nil 294 | } 295 | 296 | func (self *SetPositionControlOffset) Unpack(p *Packet) error { 297 | if len(p.Payload) < 18 { 298 | return fmt.Errorf("payload too small") 299 | } 300 | self.X = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[0:])) 301 | self.Y = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[4:])) 302 | self.Z = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 303 | self.Yaw = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 304 | self.TargetSystem = uint8(p.Payload[16]) 305 | self.TargetComponent = uint8(p.Payload[17]) 306 | return nil 307 | } 308 | 309 | // 310 | type PositionControlSetpoint struct { 311 | X float32 // x position 312 | Y float32 // y position 313 | Z float32 // z position 314 | Yaw float32 // yaw orientation in radians, 0 = NORTH 315 | Id uint16 // ID of waypoint, 0 for plain position 316 | } 317 | 318 | func (self *PositionControlSetpoint) MsgID() uint8 { 319 | return 170 320 | } 321 | 322 | func (self *PositionControlSetpoint) MsgName() string { 323 | return "PositionControlSetpoint" 324 | } 325 | 326 | func (self *PositionControlSetpoint) Pack(p *Packet) error { 327 | payload := make([]byte, 18) 328 | binary.LittleEndian.PutUint32(payload[0:], math.Float32bits(self.X)) 329 | binary.LittleEndian.PutUint32(payload[4:], math.Float32bits(self.Y)) 330 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.Z)) 331 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.Yaw)) 332 | binary.LittleEndian.PutUint16(payload[16:], uint16(self.Id)) 333 | 334 | p.MsgID = self.MsgID() 335 | p.Payload = payload 336 | return nil 337 | } 338 | 339 | func (self *PositionControlSetpoint) Unpack(p *Packet) error { 340 | if len(p.Payload) < 18 { 341 | return fmt.Errorf("payload too small") 342 | } 343 | self.X = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[0:])) 344 | self.Y = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[4:])) 345 | self.Z = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 346 | self.Yaw = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 347 | self.Id = uint16(binary.LittleEndian.Uint16(p.Payload[16:])) 348 | return nil 349 | } 350 | 351 | // 352 | type Marker struct { 353 | X float32 // x position 354 | Y float32 // y position 355 | Z float32 // z position 356 | Roll float32 // roll orientation 357 | Pitch float32 // pitch orientation 358 | Yaw float32 // yaw orientation 359 | Id uint16 // ID 360 | } 361 | 362 | func (self *Marker) MsgID() uint8 { 363 | return 171 364 | } 365 | 366 | func (self *Marker) MsgName() string { 367 | return "Marker" 368 | } 369 | 370 | func (self *Marker) Pack(p *Packet) error { 371 | payload := make([]byte, 26) 372 | binary.LittleEndian.PutUint32(payload[0:], math.Float32bits(self.X)) 373 | binary.LittleEndian.PutUint32(payload[4:], math.Float32bits(self.Y)) 374 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.Z)) 375 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.Roll)) 376 | binary.LittleEndian.PutUint32(payload[16:], math.Float32bits(self.Pitch)) 377 | binary.LittleEndian.PutUint32(payload[20:], math.Float32bits(self.Yaw)) 378 | binary.LittleEndian.PutUint16(payload[24:], uint16(self.Id)) 379 | 380 | p.MsgID = self.MsgID() 381 | p.Payload = payload 382 | return nil 383 | } 384 | 385 | func (self *Marker) Unpack(p *Packet) error { 386 | if len(p.Payload) < 26 { 387 | return fmt.Errorf("payload too small") 388 | } 389 | self.X = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[0:])) 390 | self.Y = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[4:])) 391 | self.Z = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 392 | self.Roll = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 393 | self.Pitch = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[16:])) 394 | self.Yaw = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[20:])) 395 | self.Id = uint16(binary.LittleEndian.Uint16(p.Payload[24:])) 396 | return nil 397 | } 398 | 399 | // 400 | type RawAux struct { 401 | Baro int32 // Barometric pressure (hecto Pascal) 402 | Adc1 uint16 // ADC1 (J405 ADC3, LPC2148 AD0.6) 403 | Adc2 uint16 // ADC2 (J405 ADC5, LPC2148 AD0.2) 404 | Adc3 uint16 // ADC3 (J405 ADC6, LPC2148 AD0.1) 405 | Adc4 uint16 // ADC4 (J405 ADC7, LPC2148 AD1.3) 406 | Vbat uint16 // Battery voltage 407 | Temp int16 // Temperature (degrees celcius) 408 | } 409 | 410 | func (self *RawAux) MsgID() uint8 { 411 | return 172 412 | } 413 | 414 | func (self *RawAux) MsgName() string { 415 | return "RawAux" 416 | } 417 | 418 | func (self *RawAux) Pack(p *Packet) error { 419 | payload := make([]byte, 16) 420 | binary.LittleEndian.PutUint32(payload[0:], uint32(self.Baro)) 421 | binary.LittleEndian.PutUint16(payload[4:], uint16(self.Adc1)) 422 | binary.LittleEndian.PutUint16(payload[6:], uint16(self.Adc2)) 423 | binary.LittleEndian.PutUint16(payload[8:], uint16(self.Adc3)) 424 | binary.LittleEndian.PutUint16(payload[10:], uint16(self.Adc4)) 425 | binary.LittleEndian.PutUint16(payload[12:], uint16(self.Vbat)) 426 | binary.LittleEndian.PutUint16(payload[14:], uint16(self.Temp)) 427 | 428 | p.MsgID = self.MsgID() 429 | p.Payload = payload 430 | return nil 431 | } 432 | 433 | func (self *RawAux) Unpack(p *Packet) error { 434 | if len(p.Payload) < 16 { 435 | return fmt.Errorf("payload too small") 436 | } 437 | self.Baro = int32(binary.LittleEndian.Uint32(p.Payload[0:])) 438 | self.Adc1 = uint16(binary.LittleEndian.Uint16(p.Payload[4:])) 439 | self.Adc2 = uint16(binary.LittleEndian.Uint16(p.Payload[6:])) 440 | self.Adc3 = uint16(binary.LittleEndian.Uint16(p.Payload[8:])) 441 | self.Adc4 = uint16(binary.LittleEndian.Uint16(p.Payload[10:])) 442 | self.Vbat = uint16(binary.LittleEndian.Uint16(p.Payload[12:])) 443 | self.Temp = int16(binary.LittleEndian.Uint16(p.Payload[14:])) 444 | return nil 445 | } 446 | 447 | // 448 | type WatchdogHeartbeat struct { 449 | WatchdogId uint16 // Watchdog ID 450 | ProcessCount uint16 // Number of processes 451 | } 452 | 453 | func (self *WatchdogHeartbeat) MsgID() uint8 { 454 | return 180 455 | } 456 | 457 | func (self *WatchdogHeartbeat) MsgName() string { 458 | return "WatchdogHeartbeat" 459 | } 460 | 461 | func (self *WatchdogHeartbeat) Pack(p *Packet) error { 462 | payload := make([]byte, 4) 463 | binary.LittleEndian.PutUint16(payload[0:], uint16(self.WatchdogId)) 464 | binary.LittleEndian.PutUint16(payload[2:], uint16(self.ProcessCount)) 465 | 466 | p.MsgID = self.MsgID() 467 | p.Payload = payload 468 | return nil 469 | } 470 | 471 | func (self *WatchdogHeartbeat) Unpack(p *Packet) error { 472 | if len(p.Payload) < 4 { 473 | return fmt.Errorf("payload too small") 474 | } 475 | self.WatchdogId = uint16(binary.LittleEndian.Uint16(p.Payload[0:])) 476 | self.ProcessCount = uint16(binary.LittleEndian.Uint16(p.Payload[2:])) 477 | return nil 478 | } 479 | 480 | // 481 | type WatchdogProcessInfo struct { 482 | Timeout int32 // Timeout (seconds) 483 | WatchdogId uint16 // Watchdog ID 484 | ProcessId uint16 // Process ID 485 | Name [100]byte // Process name 486 | Arguments [147]byte // Process arguments 487 | } 488 | 489 | func (self *WatchdogProcessInfo) MsgID() uint8 { 490 | return 181 491 | } 492 | 493 | func (self *WatchdogProcessInfo) MsgName() string { 494 | return "WatchdogProcessInfo" 495 | } 496 | 497 | func (self *WatchdogProcessInfo) Pack(p *Packet) error { 498 | payload := make([]byte, 255) 499 | binary.LittleEndian.PutUint32(payload[0:], uint32(self.Timeout)) 500 | binary.LittleEndian.PutUint16(payload[4:], uint16(self.WatchdogId)) 501 | binary.LittleEndian.PutUint16(payload[6:], uint16(self.ProcessId)) 502 | copy(payload[8:], self.Name[:]) 503 | copy(payload[108:], self.Arguments[:]) 504 | 505 | p.MsgID = self.MsgID() 506 | p.Payload = payload 507 | return nil 508 | } 509 | 510 | func (self *WatchdogProcessInfo) Unpack(p *Packet) error { 511 | if len(p.Payload) < 255 { 512 | return fmt.Errorf("payload too small") 513 | } 514 | self.Timeout = int32(binary.LittleEndian.Uint32(p.Payload[0:])) 515 | self.WatchdogId = uint16(binary.LittleEndian.Uint16(p.Payload[4:])) 516 | self.ProcessId = uint16(binary.LittleEndian.Uint16(p.Payload[6:])) 517 | copy(self.Name[:], p.Payload[8:108]) 518 | copy(self.Arguments[:], p.Payload[108:255]) 519 | return nil 520 | } 521 | 522 | // 523 | type WatchdogProcessStatus struct { 524 | Pid int32 // PID 525 | WatchdogId uint16 // Watchdog ID 526 | ProcessId uint16 // Process ID 527 | Crashes uint16 // Number of crashes 528 | State uint8 // Is running / finished / suspended / crashed 529 | Muted uint8 // Is muted 530 | } 531 | 532 | func (self *WatchdogProcessStatus) MsgID() uint8 { 533 | return 182 534 | } 535 | 536 | func (self *WatchdogProcessStatus) MsgName() string { 537 | return "WatchdogProcessStatus" 538 | } 539 | 540 | func (self *WatchdogProcessStatus) Pack(p *Packet) error { 541 | payload := make([]byte, 12) 542 | binary.LittleEndian.PutUint32(payload[0:], uint32(self.Pid)) 543 | binary.LittleEndian.PutUint16(payload[4:], uint16(self.WatchdogId)) 544 | binary.LittleEndian.PutUint16(payload[6:], uint16(self.ProcessId)) 545 | binary.LittleEndian.PutUint16(payload[8:], uint16(self.Crashes)) 546 | payload[10] = byte(self.State) 547 | payload[11] = byte(self.Muted) 548 | 549 | p.MsgID = self.MsgID() 550 | p.Payload = payload 551 | return nil 552 | } 553 | 554 | func (self *WatchdogProcessStatus) Unpack(p *Packet) error { 555 | if len(p.Payload) < 12 { 556 | return fmt.Errorf("payload too small") 557 | } 558 | self.Pid = int32(binary.LittleEndian.Uint32(p.Payload[0:])) 559 | self.WatchdogId = uint16(binary.LittleEndian.Uint16(p.Payload[4:])) 560 | self.ProcessId = uint16(binary.LittleEndian.Uint16(p.Payload[6:])) 561 | self.Crashes = uint16(binary.LittleEndian.Uint16(p.Payload[8:])) 562 | self.State = uint8(p.Payload[10]) 563 | self.Muted = uint8(p.Payload[11]) 564 | return nil 565 | } 566 | 567 | // 568 | type WatchdogCommand struct { 569 | WatchdogId uint16 // Watchdog ID 570 | ProcessId uint16 // Process ID 571 | TargetSystemId uint8 // Target system ID 572 | CommandId uint8 // Command ID 573 | } 574 | 575 | func (self *WatchdogCommand) MsgID() uint8 { 576 | return 183 577 | } 578 | 579 | func (self *WatchdogCommand) MsgName() string { 580 | return "WatchdogCommand" 581 | } 582 | 583 | func (self *WatchdogCommand) Pack(p *Packet) error { 584 | payload := make([]byte, 6) 585 | binary.LittleEndian.PutUint16(payload[0:], uint16(self.WatchdogId)) 586 | binary.LittleEndian.PutUint16(payload[2:], uint16(self.ProcessId)) 587 | payload[4] = byte(self.TargetSystemId) 588 | payload[5] = byte(self.CommandId) 589 | 590 | p.MsgID = self.MsgID() 591 | p.Payload = payload 592 | return nil 593 | } 594 | 595 | func (self *WatchdogCommand) Unpack(p *Packet) error { 596 | if len(p.Payload) < 6 { 597 | return fmt.Errorf("payload too small") 598 | } 599 | self.WatchdogId = uint16(binary.LittleEndian.Uint16(p.Payload[0:])) 600 | self.ProcessId = uint16(binary.LittleEndian.Uint16(p.Payload[2:])) 601 | self.TargetSystemId = uint8(p.Payload[4]) 602 | self.CommandId = uint8(p.Payload[5]) 603 | return nil 604 | } 605 | 606 | // 607 | type PatternDetected struct { 608 | Confidence float32 // Confidence of detection 609 | Type uint8 // 0: Pattern, 1: Letter 610 | File [100]byte // Pattern file name 611 | Detected uint8 // Accepted as true detection, 0 no, 1 yes 612 | } 613 | 614 | func (self *PatternDetected) MsgID() uint8 { 615 | return 190 616 | } 617 | 618 | func (self *PatternDetected) MsgName() string { 619 | return "PatternDetected" 620 | } 621 | 622 | func (self *PatternDetected) Pack(p *Packet) error { 623 | payload := make([]byte, 106) 624 | binary.LittleEndian.PutUint32(payload[0:], math.Float32bits(self.Confidence)) 625 | payload[4] = byte(self.Type) 626 | copy(payload[5:], self.File[:]) 627 | payload[105] = byte(self.Detected) 628 | 629 | p.MsgID = self.MsgID() 630 | p.Payload = payload 631 | return nil 632 | } 633 | 634 | func (self *PatternDetected) Unpack(p *Packet) error { 635 | if len(p.Payload) < 106 { 636 | return fmt.Errorf("payload too small") 637 | } 638 | self.Confidence = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[0:])) 639 | self.Type = uint8(p.Payload[4]) 640 | copy(self.File[:], p.Payload[5:105]) 641 | self.Detected = uint8(p.Payload[105]) 642 | return nil 643 | } 644 | 645 | // Notifies the operator about a point of interest (POI). This can be anything detected by the 646 | // system. This generic message is intented to help interfacing to generic visualizations and to display 647 | // the POI on a map. 648 | // 649 | type PointOfInterest struct { 650 | X float32 // X Position 651 | Y float32 // Y Position 652 | Z float32 // Z Position 653 | Timeout uint16 // 0: no timeout, >1: timeout in seconds 654 | Type uint8 // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug 655 | Color uint8 // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta 656 | CoordinateSystem uint8 // 0: global, 1:local 657 | Name [26]byte // POI name 658 | } 659 | 660 | func (self *PointOfInterest) MsgID() uint8 { 661 | return 191 662 | } 663 | 664 | func (self *PointOfInterest) MsgName() string { 665 | return "PointOfInterest" 666 | } 667 | 668 | func (self *PointOfInterest) Pack(p *Packet) error { 669 | payload := make([]byte, 43) 670 | binary.LittleEndian.PutUint32(payload[0:], math.Float32bits(self.X)) 671 | binary.LittleEndian.PutUint32(payload[4:], math.Float32bits(self.Y)) 672 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.Z)) 673 | binary.LittleEndian.PutUint16(payload[12:], uint16(self.Timeout)) 674 | payload[14] = byte(self.Type) 675 | payload[15] = byte(self.Color) 676 | payload[16] = byte(self.CoordinateSystem) 677 | copy(payload[17:], self.Name[:]) 678 | 679 | p.MsgID = self.MsgID() 680 | p.Payload = payload 681 | return nil 682 | } 683 | 684 | func (self *PointOfInterest) Unpack(p *Packet) error { 685 | if len(p.Payload) < 43 { 686 | return fmt.Errorf("payload too small") 687 | } 688 | self.X = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[0:])) 689 | self.Y = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[4:])) 690 | self.Z = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 691 | self.Timeout = uint16(binary.LittleEndian.Uint16(p.Payload[12:])) 692 | self.Type = uint8(p.Payload[14]) 693 | self.Color = uint8(p.Payload[15]) 694 | self.CoordinateSystem = uint8(p.Payload[16]) 695 | copy(self.Name[:], p.Payload[17:43]) 696 | return nil 697 | } 698 | 699 | // Notifies the operator about the connection of two point of interests (POI). This can be anything detected by the 700 | // system. This generic message is intented to help interfacing to generic visualizations and to display 701 | // the POI on a map. 702 | // 703 | type PointOfInterestConnection struct { 704 | Xp1 float32 // X1 Position 705 | Yp1 float32 // Y1 Position 706 | Zp1 float32 // Z1 Position 707 | Xp2 float32 // X2 Position 708 | Yp2 float32 // Y2 Position 709 | Zp2 float32 // Z2 Position 710 | Timeout uint16 // 0: no timeout, >1: timeout in seconds 711 | Type uint8 // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug 712 | Color uint8 // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta 713 | CoordinateSystem uint8 // 0: global, 1:local 714 | Name [26]byte // POI connection name 715 | } 716 | 717 | func (self *PointOfInterestConnection) MsgID() uint8 { 718 | return 192 719 | } 720 | 721 | func (self *PointOfInterestConnection) MsgName() string { 722 | return "PointOfInterestConnection" 723 | } 724 | 725 | func (self *PointOfInterestConnection) Pack(p *Packet) error { 726 | payload := make([]byte, 55) 727 | binary.LittleEndian.PutUint32(payload[0:], math.Float32bits(self.Xp1)) 728 | binary.LittleEndian.PutUint32(payload[4:], math.Float32bits(self.Yp1)) 729 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.Zp1)) 730 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.Xp2)) 731 | binary.LittleEndian.PutUint32(payload[16:], math.Float32bits(self.Yp2)) 732 | binary.LittleEndian.PutUint32(payload[20:], math.Float32bits(self.Zp2)) 733 | binary.LittleEndian.PutUint16(payload[24:], uint16(self.Timeout)) 734 | payload[26] = byte(self.Type) 735 | payload[27] = byte(self.Color) 736 | payload[28] = byte(self.CoordinateSystem) 737 | copy(payload[29:], self.Name[:]) 738 | 739 | p.MsgID = self.MsgID() 740 | p.Payload = payload 741 | return nil 742 | } 743 | 744 | func (self *PointOfInterestConnection) Unpack(p *Packet) error { 745 | if len(p.Payload) < 55 { 746 | return fmt.Errorf("payload too small") 747 | } 748 | self.Xp1 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[0:])) 749 | self.Yp1 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[4:])) 750 | self.Zp1 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 751 | self.Xp2 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 752 | self.Yp2 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[16:])) 753 | self.Zp2 = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[20:])) 754 | self.Timeout = uint16(binary.LittleEndian.Uint16(p.Payload[24:])) 755 | self.Type = uint8(p.Payload[26]) 756 | self.Color = uint8(p.Payload[27]) 757 | self.CoordinateSystem = uint8(p.Payload[28]) 758 | copy(self.Name[:], p.Payload[29:55]) 759 | return nil 760 | } 761 | 762 | // 763 | type BriefFeature struct { 764 | X float32 // x position in m 765 | Y float32 // y position in m 766 | Z float32 // z position in m 767 | Response float32 // Harris operator response at this location 768 | Size uint16 // Size in pixels 769 | Orientation uint16 // Orientation 770 | OrientationAssignment uint8 // Orientation assignment 0: false, 1:true 771 | Descriptor [32]uint8 // Descriptor 772 | } 773 | 774 | func (self *BriefFeature) MsgID() uint8 { 775 | return 195 776 | } 777 | 778 | func (self *BriefFeature) MsgName() string { 779 | return "BriefFeature" 780 | } 781 | 782 | func (self *BriefFeature) Pack(p *Packet) error { 783 | payload := make([]byte, 53) 784 | binary.LittleEndian.PutUint32(payload[0:], math.Float32bits(self.X)) 785 | binary.LittleEndian.PutUint32(payload[4:], math.Float32bits(self.Y)) 786 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.Z)) 787 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.Response)) 788 | binary.LittleEndian.PutUint16(payload[16:], uint16(self.Size)) 789 | binary.LittleEndian.PutUint16(payload[18:], uint16(self.Orientation)) 790 | payload[20] = byte(self.OrientationAssignment) 791 | copy(payload[21:], self.Descriptor[:]) 792 | 793 | p.MsgID = self.MsgID() 794 | p.Payload = payload 795 | return nil 796 | } 797 | 798 | func (self *BriefFeature) Unpack(p *Packet) error { 799 | if len(p.Payload) < 53 { 800 | return fmt.Errorf("payload too small") 801 | } 802 | self.X = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[0:])) 803 | self.Y = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[4:])) 804 | self.Z = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 805 | self.Response = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 806 | self.Size = uint16(binary.LittleEndian.Uint16(p.Payload[16:])) 807 | self.Orientation = uint16(binary.LittleEndian.Uint16(p.Payload[18:])) 808 | self.OrientationAssignment = uint8(p.Payload[20]) 809 | copy(self.Descriptor[:], p.Payload[21:53]) 810 | return nil 811 | } 812 | 813 | // 814 | type AttitudeControl struct { 815 | Roll float32 // roll 816 | Pitch float32 // pitch 817 | Yaw float32 // yaw 818 | Thrust float32 // thrust 819 | Target uint8 // The system to be controlled 820 | RollManual uint8 // roll control enabled auto:0, manual:1 821 | PitchManual uint8 // pitch auto:0, manual:1 822 | YawManual uint8 // yaw auto:0, manual:1 823 | ThrustManual uint8 // thrust auto:0, manual:1 824 | } 825 | 826 | func (self *AttitudeControl) MsgID() uint8 { 827 | return 200 828 | } 829 | 830 | func (self *AttitudeControl) MsgName() string { 831 | return "AttitudeControl" 832 | } 833 | 834 | func (self *AttitudeControl) Pack(p *Packet) error { 835 | payload := make([]byte, 21) 836 | binary.LittleEndian.PutUint32(payload[0:], math.Float32bits(self.Roll)) 837 | binary.LittleEndian.PutUint32(payload[4:], math.Float32bits(self.Pitch)) 838 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.Yaw)) 839 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.Thrust)) 840 | payload[16] = byte(self.Target) 841 | payload[17] = byte(self.RollManual) 842 | payload[18] = byte(self.PitchManual) 843 | payload[19] = byte(self.YawManual) 844 | payload[20] = byte(self.ThrustManual) 845 | 846 | p.MsgID = self.MsgID() 847 | p.Payload = payload 848 | return nil 849 | } 850 | 851 | func (self *AttitudeControl) Unpack(p *Packet) error { 852 | if len(p.Payload) < 21 { 853 | return fmt.Errorf("payload too small") 854 | } 855 | self.Roll = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[0:])) 856 | self.Pitch = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[4:])) 857 | self.Yaw = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 858 | self.Thrust = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 859 | self.Target = uint8(p.Payload[16]) 860 | self.RollManual = uint8(p.Payload[17]) 861 | self.PitchManual = uint8(p.Payload[18]) 862 | self.YawManual = uint8(p.Payload[19]) 863 | self.ThrustManual = uint8(p.Payload[20]) 864 | return nil 865 | } 866 | 867 | // 868 | type DetectionStats struct { 869 | Detections uint32 // Number of detections 870 | ClusterIters uint32 // Number of cluster iterations 871 | BestScore float32 // Best score 872 | BestLat int32 // Latitude of the best detection * 1E7 873 | BestLon int32 // Longitude of the best detection * 1E7 874 | BestAlt int32 // Altitude of the best detection * 1E3 875 | BestDetectionId uint32 // Best detection ID 876 | BestClusterId uint32 // Best cluster ID 877 | BestClusterIterId uint32 // Best cluster ID 878 | ImagesDone uint32 // Number of images already processed 879 | ImagesTodo uint32 // Number of images still to process 880 | Fps float32 // Average images per seconds processed 881 | } 882 | 883 | func (self *DetectionStats) MsgID() uint8 { 884 | return 205 885 | } 886 | 887 | func (self *DetectionStats) MsgName() string { 888 | return "DetectionStats" 889 | } 890 | 891 | func (self *DetectionStats) Pack(p *Packet) error { 892 | payload := make([]byte, 48) 893 | binary.LittleEndian.PutUint32(payload[0:], uint32(self.Detections)) 894 | binary.LittleEndian.PutUint32(payload[4:], uint32(self.ClusterIters)) 895 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.BestScore)) 896 | binary.LittleEndian.PutUint32(payload[12:], uint32(self.BestLat)) 897 | binary.LittleEndian.PutUint32(payload[16:], uint32(self.BestLon)) 898 | binary.LittleEndian.PutUint32(payload[20:], uint32(self.BestAlt)) 899 | binary.LittleEndian.PutUint32(payload[24:], uint32(self.BestDetectionId)) 900 | binary.LittleEndian.PutUint32(payload[28:], uint32(self.BestClusterId)) 901 | binary.LittleEndian.PutUint32(payload[32:], uint32(self.BestClusterIterId)) 902 | binary.LittleEndian.PutUint32(payload[36:], uint32(self.ImagesDone)) 903 | binary.LittleEndian.PutUint32(payload[40:], uint32(self.ImagesTodo)) 904 | binary.LittleEndian.PutUint32(payload[44:], math.Float32bits(self.Fps)) 905 | 906 | p.MsgID = self.MsgID() 907 | p.Payload = payload 908 | return nil 909 | } 910 | 911 | func (self *DetectionStats) Unpack(p *Packet) error { 912 | if len(p.Payload) < 48 { 913 | return fmt.Errorf("payload too small") 914 | } 915 | self.Detections = uint32(binary.LittleEndian.Uint32(p.Payload[0:])) 916 | self.ClusterIters = uint32(binary.LittleEndian.Uint32(p.Payload[4:])) 917 | self.BestScore = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 918 | self.BestLat = int32(binary.LittleEndian.Uint32(p.Payload[12:])) 919 | self.BestLon = int32(binary.LittleEndian.Uint32(p.Payload[16:])) 920 | self.BestAlt = int32(binary.LittleEndian.Uint32(p.Payload[20:])) 921 | self.BestDetectionId = uint32(binary.LittleEndian.Uint32(p.Payload[24:])) 922 | self.BestClusterId = uint32(binary.LittleEndian.Uint32(p.Payload[28:])) 923 | self.BestClusterIterId = uint32(binary.LittleEndian.Uint32(p.Payload[32:])) 924 | self.ImagesDone = uint32(binary.LittleEndian.Uint32(p.Payload[36:])) 925 | self.ImagesTodo = uint32(binary.LittleEndian.Uint32(p.Payload[40:])) 926 | self.Fps = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[44:])) 927 | return nil 928 | } 929 | 930 | // 931 | type OnboardHealth struct { 932 | Uptime uint32 // Uptime of system 933 | RamTotal float32 // RAM size in GiB 934 | SwapTotal float32 // Swap size in GiB 935 | DiskTotal float32 // Disk total in GiB 936 | Temp float32 // Temperature 937 | Voltage float32 // Supply voltage V 938 | NetworkLoadIn float32 // Network load inbound KiB/s 939 | NetworkLoadOut float32 // Network load outbound in KiB/s 940 | CpuFreq uint16 // CPU frequency 941 | CpuLoad uint8 // CPU load in percent 942 | RamUsage uint8 // RAM usage in percent 943 | SwapUsage uint8 // Swap usage in percent 944 | DiskHealth int8 // Disk health (-1: N/A, 0: ERR, 1: RO, 2: RW) 945 | DiskUsage uint8 // Disk usage in percent 946 | } 947 | 948 | func (self *OnboardHealth) MsgID() uint8 { 949 | return 206 950 | } 951 | 952 | func (self *OnboardHealth) MsgName() string { 953 | return "OnboardHealth" 954 | } 955 | 956 | func (self *OnboardHealth) Pack(p *Packet) error { 957 | payload := make([]byte, 39) 958 | binary.LittleEndian.PutUint32(payload[0:], uint32(self.Uptime)) 959 | binary.LittleEndian.PutUint32(payload[4:], math.Float32bits(self.RamTotal)) 960 | binary.LittleEndian.PutUint32(payload[8:], math.Float32bits(self.SwapTotal)) 961 | binary.LittleEndian.PutUint32(payload[12:], math.Float32bits(self.DiskTotal)) 962 | binary.LittleEndian.PutUint32(payload[16:], math.Float32bits(self.Temp)) 963 | binary.LittleEndian.PutUint32(payload[20:], math.Float32bits(self.Voltage)) 964 | binary.LittleEndian.PutUint32(payload[24:], math.Float32bits(self.NetworkLoadIn)) 965 | binary.LittleEndian.PutUint32(payload[28:], math.Float32bits(self.NetworkLoadOut)) 966 | binary.LittleEndian.PutUint16(payload[32:], uint16(self.CpuFreq)) 967 | payload[34] = byte(self.CpuLoad) 968 | payload[35] = byte(self.RamUsage) 969 | payload[36] = byte(self.SwapUsage) 970 | payload[37] = byte(self.DiskHealth) 971 | payload[38] = byte(self.DiskUsage) 972 | 973 | p.MsgID = self.MsgID() 974 | p.Payload = payload 975 | return nil 976 | } 977 | 978 | func (self *OnboardHealth) Unpack(p *Packet) error { 979 | if len(p.Payload) < 39 { 980 | return fmt.Errorf("payload too small") 981 | } 982 | self.Uptime = uint32(binary.LittleEndian.Uint32(p.Payload[0:])) 983 | self.RamTotal = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[4:])) 984 | self.SwapTotal = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[8:])) 985 | self.DiskTotal = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[12:])) 986 | self.Temp = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[16:])) 987 | self.Voltage = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[20:])) 988 | self.NetworkLoadIn = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[24:])) 989 | self.NetworkLoadOut = math.Float32frombits(binary.LittleEndian.Uint32(p.Payload[28:])) 990 | self.CpuFreq = uint16(binary.LittleEndian.Uint16(p.Payload[32:])) 991 | self.CpuLoad = uint8(p.Payload[34]) 992 | self.RamUsage = uint8(p.Payload[35]) 993 | self.SwapUsage = uint8(p.Payload[36]) 994 | self.DiskHealth = int8(p.Payload[37]) 995 | self.DiskUsage = uint8(p.Payload[38]) 996 | return nil 997 | } 998 | 999 | // Message IDs 1000 | const ( 1001 | MSG_ID_SET_CAM_SHUTTER = 151 1002 | MSG_ID_IMAGE_TRIGGERED = 152 1003 | MSG_ID_IMAGE_TRIGGER_CONTROL = 153 1004 | MSG_ID_IMAGE_AVAILABLE = 154 1005 | MSG_ID_SET_POSITION_CONTROL_OFFSET = 160 1006 | MSG_ID_POSITION_CONTROL_SETPOINT = 170 1007 | MSG_ID_MARKER = 171 1008 | MSG_ID_RAW_AUX = 172 1009 | MSG_ID_WATCHDOG_HEARTBEAT = 180 1010 | MSG_ID_WATCHDOG_PROCESS_INFO = 181 1011 | MSG_ID_WATCHDOG_PROCESS_STATUS = 182 1012 | MSG_ID_WATCHDOG_COMMAND = 183 1013 | MSG_ID_PATTERN_DETECTED = 190 1014 | MSG_ID_POINT_OF_INTEREST = 191 1015 | MSG_ID_POINT_OF_INTEREST_CONNECTION = 192 1016 | MSG_ID_BRIEF_FEATURE = 195 1017 | MSG_ID_ATTITUDE_CONTROL = 200 1018 | MSG_ID_DETECTION_STATS = 205 1019 | MSG_ID_ONBOARD_HEALTH = 206 1020 | ) 1021 | 1022 | // DialectPixhawk is the dialect represented by pixhawk.xml 1023 | var DialectPixhawk *Dialect = &Dialect{ 1024 | Name: "pixhawk", 1025 | crcExtras: map[uint8]uint8{ 1026 | 151: 108, // MSG_ID_SET_CAM_SHUTTER 1027 | 152: 86, // MSG_ID_IMAGE_TRIGGERED 1028 | 153: 95, // MSG_ID_IMAGE_TRIGGER_CONTROL 1029 | 154: 224, // MSG_ID_IMAGE_AVAILABLE 1030 | 160: 22, // MSG_ID_SET_POSITION_CONTROL_OFFSET 1031 | 170: 28, // MSG_ID_POSITION_CONTROL_SETPOINT 1032 | 171: 249, // MSG_ID_MARKER 1033 | 172: 182, // MSG_ID_RAW_AUX 1034 | 180: 153, // MSG_ID_WATCHDOG_HEARTBEAT 1035 | 181: 16, // MSG_ID_WATCHDOG_PROCESS_INFO 1036 | 182: 29, // MSG_ID_WATCHDOG_PROCESS_STATUS 1037 | 183: 162, // MSG_ID_WATCHDOG_COMMAND 1038 | 190: 90, // MSG_ID_PATTERN_DETECTED 1039 | 191: 95, // MSG_ID_POINT_OF_INTEREST 1040 | 192: 36, // MSG_ID_POINT_OF_INTEREST_CONNECTION 1041 | 195: 88, // MSG_ID_BRIEF_FEATURE 1042 | 200: 254, // MSG_ID_ATTITUDE_CONTROL 1043 | 205: 87, // MSG_ID_DETECTION_STATS 1044 | 206: 19, // MSG_ID_ONBOARD_HEALTH 1045 | }, 1046 | } 1047 | --------------------------------------------------------------------------------