125 lines
3.0 KiB
Go
125 lines
3.0 KiB
Go
package serial_ubx
|
|
|
|
import (
|
|
"errors"
|
|
"fmt"
|
|
"git.timovolkmann.de/gyrogpsc/ublox"
|
|
"go.bug.st/serial"
|
|
"log"
|
|
"time"
|
|
)
|
|
|
|
type Measurement struct {
|
|
//Timestamp int64 `json:"timestamp"`
|
|
Timestamp time.Time `json:"timestamp"`
|
|
Position [3]int32 `json:"position"` // Latitude, Longitude, Height
|
|
Orientation [3]int32 `json:"orientation"` // Pitch, Roll, Heading
|
|
}
|
|
|
|
type ubxReceiver struct {
|
|
decoder *ublox.Decoder
|
|
currentMeas Measurement
|
|
}
|
|
|
|
func Setup(portname string) (*ubxReceiver, error) {
|
|
mode := &serial.Mode{
|
|
BaudRate: 115200,
|
|
}
|
|
port, err := serial.Open(portname, mode)
|
|
if err != nil {
|
|
return nil, err
|
|
}
|
|
return &ubxReceiver{
|
|
decoder: ublox.NewDecoder(port),
|
|
}, nil
|
|
}
|
|
|
|
func (u *ubxReceiver) Next() (ublox.Message, error) {
|
|
return u.decoder.Decode()
|
|
}
|
|
|
|
var (
|
|
errNotImplemented = errors.New("message not implemented")
|
|
)
|
|
|
|
// TODO: additional callback with adjustable timing
|
|
func (u *ubxReceiver) NextMeasurement() (*Measurement, error) {
|
|
|
|
msg, err := u.decoder.Decode()
|
|
if err != nil {
|
|
return nil, err
|
|
}
|
|
//t := time.Time{}
|
|
switch v := msg.(type) {
|
|
case *ublox.NavPvt:
|
|
t, err := time.Parse(time.RFC3339Nano, formatTime(v.Year_y, v.Month_month, v.Day_d, v.Hour_h, v.Min_min, v.Sec_s, v.Nano_ns))
|
|
if err != nil {
|
|
log.Println(err)
|
|
}
|
|
//u.currentMeas.Timestamp = t.UnixNano()
|
|
u.currentMeas.Timestamp = t
|
|
u.currentMeas.Position[0] = v.Lat_dege7
|
|
u.currentMeas.Position[1] = v.Lon_dege7
|
|
u.currentMeas.Position[2] = v.Height_mm
|
|
//fmt.Printf("%T %v\n", *v, *v)
|
|
case *ublox.HnrPvt:
|
|
t, err := time.Parse(time.RFC3339Nano, formatTime(v.Year_y, v.Month_month, v.Day_d, v.Hour_h, v.Min_min, v.Sec_s, v.Nano_ns))
|
|
if err != nil {
|
|
log.Println(err)
|
|
}
|
|
u.currentMeas.Timestamp = t
|
|
u.currentMeas.Position[0] = v.Lat_dege7
|
|
u.currentMeas.Position[1] = v.Lon_dege7
|
|
u.currentMeas.Position[2] = v.Height_mm
|
|
//fmt.Printf("%T %v\n", *v, *v)
|
|
case *ublox.NavAtt:
|
|
u.currentMeas.Orientation[0] = v.Pitch_deg
|
|
u.currentMeas.Orientation[1] = v.Roll_deg
|
|
u.currentMeas.Orientation[2] = v.Heading_deg
|
|
//fmt.Printf("%T %v\n", *v, *v)
|
|
|
|
//case *ublox.RawMessage:
|
|
// //fmt.Printf("%T %v\n\n", *v, *v)
|
|
default:
|
|
return nil, errNotImplemented
|
|
}
|
|
m := u.currentMeas
|
|
return &m, nil
|
|
}
|
|
|
|
func formatTime(Year_y uint16, Month_month byte, Day_d byte, Hour_h byte, Min_min byte, Sec_s byte, Nano_ns int32) string {
|
|
//Nano_ns *= 1e+3
|
|
if Nano_ns < 0 {
|
|
Nano_ns += int32(time.Second)
|
|
if Sec_s > 0 {
|
|
Sec_s--
|
|
} else if Min_min > 0 {
|
|
Sec_s = 59
|
|
Min_min--
|
|
} else if Hour_h > 0 {
|
|
Sec_s = 59
|
|
Min_min = 59
|
|
Hour_h--
|
|
} else if Day_d > 1 {
|
|
Sec_s = 59
|
|
Min_min = 59
|
|
Hour_h = 23
|
|
Day_d--
|
|
} // TODO: more cases for exact behavior! good for now...
|
|
}
|
|
//fmt.Printf("%04d-%02d-%02dT%02d:%02d:%02d.%09dZ00:00\n", Year_y, Month_month, Day_d, Hour_h, Min_min, Sec_s, Nano_ns )
|
|
return fmt.Sprintf("%04d-%02d-%02dT%02d:%02d:%02d.%09dZ", Year_y, Month_month, Day_d, Hour_h, Min_min, Sec_s, Nano_ns)
|
|
}
|
|
|
|
func (u *ubxReceiver) Printloop() {
|
|
|
|
for {
|
|
meas, err := u.NextMeasurement()
|
|
if err != nil {
|
|
continue
|
|
}
|
|
fmt.Println(meas)
|
|
}
|
|
|
|
}
|