|
| 1 | +package ubloxm8 |
| 2 | + |
| 3 | +import ( |
| 4 | + "bufio" |
| 5 | + "bytes" |
| 6 | + "encoding/binary" |
| 7 | + "log" |
| 8 | + |
| 9 | + "go.bug.st/serial" |
| 10 | +) |
| 11 | + |
| 12 | +type Receiver struct { |
| 13 | + port serial.Port |
| 14 | +} |
| 15 | + |
| 16 | +func NewReceiver(device string, baudrate int) (*Receiver, error) { |
| 17 | + mode := &serial.Mode{BaudRate: baudrate} |
| 18 | + port, err := serial.Open(device, mode) |
| 19 | + if err != nil { |
| 20 | + return nil, err |
| 21 | + } |
| 22 | + |
| 23 | + return &Receiver{ |
| 24 | + port: port, |
| 25 | + }, nil |
| 26 | +} |
| 27 | + |
| 28 | +func (r *Receiver) Close() error { |
| 29 | + return r.port.Close() |
| 30 | +} |
| 31 | + |
| 32 | +func (r *Receiver) SendUbxCommand(class, id byte, payload []byte) error { |
| 33 | + frame := ubxCommand(class, id, payload) |
| 34 | + _, err := r.port.Write(frame) |
| 35 | + return err |
| 36 | +} |
| 37 | + |
| 38 | +func (r *Receiver) EnableQZSSL1S() error { |
| 39 | + return r.SendUbxCommand(0x06, 0x3e, // UBX-CFG-GNSS |
| 40 | + []byte{ |
| 41 | + 0x00, // msgVer |
| 42 | + 0x20, // numTrkChHw |
| 43 | + 0x20, // numTrkChUse |
| 44 | + 0x02, // numConfigBlocks = 1 |
| 45 | + // config block |
| 46 | + 0x00, // gnssId = 0 (GPS) |
| 47 | + 0x08, // resTrkCh |
| 48 | + 0x10, // maxTrkCh |
| 49 | + 0x00, // reserved1 |
| 50 | + 0x01, 0x00, 0x01, 0x01, // flags |
| 51 | + // config block |
| 52 | + 0x05, // gnssId = 5 (QZSS) |
| 53 | + 0x00, // resTrkCh |
| 54 | + 0x03, // maxTrkCh |
| 55 | + 0x00, // reserved1 |
| 56 | + 0x01, 0x00, 0x05, 0x05, // flags; enable L1C/A and L1S (in sigCfgMask) and enable=1 |
| 57 | + }) |
| 58 | +} |
| 59 | + |
| 60 | +func (r *Receiver) EnableRXMSFRBXOnURAT1() error { |
| 61 | + return r.SendUbxCommand(0x06, 0x01, // UBX-CFG-MSG; Set message rate(s) |
| 62 | + []byte{ |
| 63 | + 0x02, 0x13, // RXM-SFRBX |
| 64 | + 0x00, // DDC/I2C |
| 65 | + 0x01, // UART 1: set 1 |
| 66 | + 0x00, // UART 2 |
| 67 | + 0x00, // USB |
| 68 | + 0x00, // SPI |
| 69 | + }) |
| 70 | +} |
| 71 | + |
| 72 | +const ( |
| 73 | + initial = iota |
| 74 | + nmea |
| 75 | + expect_lf |
| 76 | + ubx |
| 77 | +) |
| 78 | + |
| 79 | +type MessageHandler func(string) |
| 80 | + |
| 81 | +func (r *Receiver) Receive(onMessage MessageHandler, all bool, verbose bool) error { |
| 82 | + reader := bufio.NewReader(r.port) |
| 83 | + |
| 84 | + nmeaBuf := "" |
| 85 | + ubxBuf := bytes.Buffer{} |
| 86 | + state := initial |
| 87 | + ubxBytesToRead := 0 |
| 88 | + for { |
| 89 | + b, err := reader.ReadByte() |
| 90 | + if err != nil { |
| 91 | + return err |
| 92 | + } |
| 93 | + |
| 94 | + switch state { |
| 95 | + case initial: |
| 96 | + bytes, err := reader.Peek(5) // SYNC CHAR 2 (0x62; 1 byte) + CLASS (1 byte) + ID(1 byte) + LENGTH (2 bytes) |
| 97 | + if err != nil { |
| 98 | + return err |
| 99 | + } |
| 100 | + |
| 101 | + if b == '$' { |
| 102 | + state = nmea |
| 103 | + nmeaBuf = "$" |
| 104 | + } else if b == 0xb5 && bytes[0] == 0x62 { |
| 105 | + state = ubx |
| 106 | + payloadLength := int(binary.LittleEndian.Uint16(bytes[3:5])) |
| 107 | + ubxBytesToRead = 2 + 2 + 2 + payloadLength + 2 |
| 108 | + // 1 byte SYNC CHAR 1 (0xB5) |
| 109 | + // 1 byte SYNC CHAR 2 (0x62) |
| 110 | + // 1 byte CLASS |
| 111 | + // 1 byte ID |
| 112 | + // 2 byte LENGTH |
| 113 | + // PAYLOAD |
| 114 | + // 1 byte CK_A |
| 115 | + // 1 byte CK_B |
| 116 | + ubxBuf.Reset() |
| 117 | + ubxBuf.WriteByte(b) |
| 118 | + ubxBytesToRead -= 1 // already read 1 byte (SYNC CHAR 1; 0xB5) |
| 119 | + } |
| 120 | + case nmea: |
| 121 | + if b == '\r' { |
| 122 | + state = expect_lf |
| 123 | + } else { |
| 124 | + nmeaBuf += string(b) |
| 125 | + } |
| 126 | + case expect_lf: |
| 127 | + if b == '\n' { |
| 128 | + state = initial |
| 129 | + } else { |
| 130 | + // unexpected |
| 131 | + log.Println("Warning: LF following CR is missing") |
| 132 | + state = initial |
| 133 | + } |
| 134 | + if all { |
| 135 | + onMessage(nmeaBuf) |
| 136 | + } |
| 137 | + case ubx: |
| 138 | + ubxBuf.WriteByte(b) |
| 139 | + ubxBytesToRead -= 1 |
| 140 | + if ubxBytesToRead == 0 { |
| 141 | + state = initial |
| 142 | + |
| 143 | + buf := ubxBuf.Bytes() |
| 144 | + if ubxBuf.Len() < 8 { |
| 145 | + log.Println("Warning: UBX message too short", buf) |
| 146 | + continue |
| 147 | + } |
| 148 | + // Calculate the UBX checksum from the sequence of CLASS, ID, LENGTH, PAYLOAD |
| 149 | + ck_a, ck_b := ubxChecksum(buf[2 : len(buf)-2]) |
| 150 | + if ck_a != buf[len(buf)-2] || ck_b != buf[len(buf)-1] { |
| 151 | + log.Println("Warning: UBX checksum error", buf) |
| 152 | + continue |
| 153 | + } |
| 154 | + if buf[2] == 0x05 && buf[3] == 0x01 { |
| 155 | + if verbose { |
| 156 | + log.Println("UBX-ACK-ACK") |
| 157 | + } |
| 158 | + } else if buf[2] == 0x05 && buf[3] == 0x00 { |
| 159 | + if verbose { |
| 160 | + log.Println("UBX-ACK-NAK") |
| 161 | + } |
| 162 | + } else if buf[2] == 0x02 && buf[3] == 0x13 { // UBX-RXM-SFRBX |
| 163 | + if buf[6] == 0x05 { // QZSS |
| 164 | + qzqsm := ubx2qzqsm(buf) |
| 165 | + if qzqsm != "" { |
| 166 | + onMessage(qzqsm) |
| 167 | + } |
| 168 | + } |
| 169 | + } |
| 170 | + } |
| 171 | + } |
| 172 | + } |
| 173 | +} |
0 commit comments