109 lines
2.6 KiB
Go
109 lines
2.6 KiB
Go
package core
|
|
|
|
import (
|
|
"errors"
|
|
"git.timovolkmann.de/gyrogpsc/ublox"
|
|
"log"
|
|
"time"
|
|
|
|
"github.com/m7shapan/njson"
|
|
)
|
|
|
|
/*{
|
|
"smartphone": {
|
|
// hier daten von hyperimu
|
|
},
|
|
"serial": {
|
|
// hier Daten von M8U:
|
|
"timestamp": 37539672354
|
|
"position": [0, 0, 0],
|
|
"orientation": [0, 0, 0]
|
|
}
|
|
}
|
|
*/
|
|
|
|
type sourceId string
|
|
|
|
const (
|
|
SOURCE_TCP sourceId = "SOURCE_TCP"
|
|
SOURCE_SERIAL sourceId = "SOURCE_SERIAL"
|
|
)
|
|
|
|
type Sensordata struct {
|
|
itow uint32
|
|
SourceId sourceId
|
|
Timestamp int64
|
|
Position [3]float64
|
|
Orientation [3]float64
|
|
}
|
|
|
|
func (s *Sensordata) isSameEpoch(n *Sensordata) bool {
|
|
if n.itow == 0 {
|
|
return false
|
|
}
|
|
return s.itow == n.itow
|
|
}
|
|
|
|
func (s *Sensordata) Consolidate(n *Sensordata) *Sensordata {
|
|
if s.SourceId != n.SourceId {
|
|
log.Fatalln("Do not consolidate Sensordata from different Sources")
|
|
}
|
|
if s.isSameEpoch(n) {
|
|
// TODO: implement epoch consolidation logic
|
|
}
|
|
return n
|
|
}
|
|
|
|
var (
|
|
errNotImplemented = errors.New("message not implemented")
|
|
)
|
|
|
|
func ConvertUbxToSensorData(msg interface{}) (*Sensordata, error) {
|
|
sd := &Sensordata{}
|
|
switch v := msg.(type) {
|
|
case *ublox.NavPvt:
|
|
sd.itow = v.ITOW_ms
|
|
sd.Timestamp = time.Date(int(v.Year_y), time.Month(v.Month_month), int(v.Day_d), int(v.Hour_h), int(v.Min_min), int(v.Sec_s), int(v.Nano_ns), time.UTC).UnixNano()
|
|
sd.Position[0] = float64(v.Lat_dege7)
|
|
sd.Position[1] = float64(v.Lon_dege7)
|
|
sd.Position[2] = float64(v.Height_mm)
|
|
case *ublox.HnrPvt:
|
|
sd.itow = v.ITOW_ms
|
|
sd.Timestamp = time.Date(int(v.Year_y), time.Month(v.Month_month), int(v.Day_d), int(v.Hour_h), int(v.Min_min), int(v.Sec_s), int(v.Nano_ns), time.UTC).UnixNano()
|
|
sd.Position[0] = float64(v.Lat_dege7)
|
|
sd.Position[1] = float64(v.Lon_dege7)
|
|
sd.Position[2] = float64(v.Height_mm)
|
|
case *ublox.NavAtt:
|
|
sd.itow = v.ITOW_ms
|
|
sd.Orientation[0] = float64(v.Pitch_deg)
|
|
sd.Orientation[1] = float64(v.Roll_deg)
|
|
sd.Orientation[2] = float64(v.Heading_deg)
|
|
default:
|
|
return nil, errNotImplemented
|
|
}
|
|
return sd, nil
|
|
}
|
|
|
|
func ConvertSensorDataPhone(jsonData []byte) (*Sensordata, error) {
|
|
return convertAndroidHyperImu(jsonData)
|
|
}
|
|
|
|
func convertAndroidHyperImu(jsonData []byte) (*Sensordata, error) {
|
|
prep := struct {
|
|
Timestamp int64 `njson:"Timestamp"`
|
|
Position [3]float64 `njson:"GPS"`
|
|
Orientation [3]float64 `njson:"orientation"`
|
|
}{}
|
|
err := njson.Unmarshal(jsonData, &prep)
|
|
if err != nil {
|
|
return nil, err
|
|
}
|
|
sd := &Sensordata{
|
|
Timestamp: prep.Timestamp * int64(time.Millisecond),
|
|
//Timestamp: time.Unix(0, prep.Timestamp * int64(time.Millisecond)),
|
|
Position: prep.Position,
|
|
Orientation: prep.Orientation,
|
|
}
|
|
return sd, nil
|
|
}
|