From 100787f0475fc64dbe9e2d4bfdb4f5b17bec0463 Mon Sep 17 00:00:00 2001 From: Timo Volkmann Date: Fri, 18 Dec 2020 12:30:26 +0100 Subject: [PATCH] added fields to sensordata --- core/datamodel.go | 351 +++++++++++++++++++++++++--------------------- core/pipelinex.go | 3 +- 2 files changed, 195 insertions(+), 159 deletions(-) diff --git a/core/datamodel.go b/core/datamodel.go index 7ab1d98..aab6103 100644 --- a/core/datamodel.go +++ b/core/datamodel.go @@ -1,224 +1,259 @@ package core import ( - "errors" - "git.timovolkmann.de/gyrogpsc/ublox" - "github.com/google/uuid" - "github.com/sirupsen/logrus" - "github.com/tidwall/gjson" - "math" - "time" + "errors" + "git.timovolkmann.de/gyrogpsc/ublox" + "github.com/google/uuid" + "github.com/sirupsen/logrus" + "github.com/tidwall/gjson" + "math" + "time" ) type Tracking struct { - TrackingMetadata - Data []SensorData + TrackingMetadata + Data []SensorData } type TrackingMetadata struct { - UUID uuid.UUID - TimeCreated time.Time - Collectors []CollectorType - Size int + UUID uuid.UUID + TimeCreated time.Time + Collectors []CollectorType + Size int } func newTracking() Tracking { - return Tracking{ - TrackingMetadata: TrackingMetadata{ - UUID: uuid.New(), - }, - Data: []SensorData{}, - } + return Tracking{ + TrackingMetadata: TrackingMetadata{ + UUID: uuid.New(), + }, + Data: []SensorData{}, + } } func (s *Tracking) isEmpty() bool { - if len(s.Data) != s.Size { - logrus.Errorln("data inconsistent...", len(s.Data), s.Size) - } - return len(s.Data) == 0 + if len(s.Data) != s.Size { + logrus.Errorln("data inconsistent...", len(s.Data), s.Size) + } + return len(s.Data) == 0 } type SourceId string const ( - SOURCE_TCP SourceId = "SOURCE_TCP" - SOURCE_SERIAL SourceId = "SOURCE_SERIAL" + SOURCE_TCP SourceId = "SOURCE_TCP" + SOURCE_SERIAL SourceId = "SOURCE_SERIAL" ) var timeex int64 type SensorData struct { - //MsgClass string - //FixType string - itow uint32 - source SourceId - latency int - Servertime time.Time - Timestamp time.Time - Position [3]float64 `json:",omitempty"` - PosAcc [2]float64 `json:",omitempty"`//[H,V] - Orientation [3]float64 `json:",omitempty"` - Speed float64 `json:",omitempty"` - PosHeading float64 `json:",omitempty"` // Course / Heading of Motion - HeadingAcc float64 `json:",omitempty"` - Gyroscope [3]float64 `json:",omitempty"` - LinearAcc [3]float64 `json:",omitempty"` + //MsgClass string + //FixType string + itow uint32 + source SourceId + latency int + Servertime time.Time + Timestamp time.Time + Position [3]float64 //`json:",omitempty"` + HAcc float64 //`json:",omitempty"`//[H,V] + VAcc float64 //`json:",omitempty"`//[H,V] + Orientation [3]float64 //`json:",omitempty"` + Speed float64 //`json:",omitempty"` + HeadDevice float64 //`json:",omitempty"` // Course / Heading of Motion + HeadMotion float64 //`json:",omitempty"` // Course / Heading of Motion + HeadingAcc float64 //`json:",omitempty"` + Gyroscope [3]float64 //`json:",omitempty"` + LinearAcc [3]float64 //`json:",omitempty"` } func (s *SensorData) Source() SourceId { - return s.source + return s.source } func (s *SensorData) SetSource(si SourceId) { - s.source = si + s.source = si } func (s SensorData) isSameEpoch(n SensorData) bool { - if n.itow == 0 { - return false - } - return s.itow == n.itow + if n.itow == 0 { + return false + } + return s.itow == n.itow } // Consolidates two sensordata elements if they are in the same epoch func (s SensorData) ConsolidateEpochsOnly(n SensorData) SensorData { - s.checkSources(&n) - if s.isSameEpoch(n) { - null := SensorData{} + s.checkSources(&n) + if s.isSameEpoch(n) { + null := SensorData{} - if n.Timestamp == null.Timestamp { - n.Timestamp = s.Timestamp - } - if n.Position == null.Position { - n.Position = s.Position - } - if n.Orientation == null.Orientation { - n.Orientation = s.Orientation - } - } - return n + if n.Timestamp == null.Timestamp { + n.Timestamp = s.Timestamp + } + if n.Position == null.Position { + n.Position = s.Position + } + if n.Orientation == null.Orientation { + n.Orientation = s.Orientation + } + } + return n } // Consolidates two sensordata elements but ignores timestamps func (s SensorData) ConsolidateExTime(n SensorData) SensorData { - s.checkSources(&n) - null := SensorData{} + s.checkSources(&n) + null := SensorData{} - if n.Position == null.Position { - n.Position = s.Position - } - if n.Orientation == null.Orientation { - n.Orientation = s.Orientation - } - return n + if n.Position == null.Position { + n.Position = s.Position + } + if n.Orientation == null.Orientation { + n.Orientation = s.Orientation + } + return n } func (s *SensorData) checkSources(n *SensorData) { - if (s.source != n.source && *s != SensorData{}) { - logrus.Println(s) - logrus.Println(n) - logrus.Fatalln("Do not consolidate SensorData from different Sources") - } + if (s.source != n.source && *s != SensorData{}) { + logrus.Println(s) + logrus.Println(n) + logrus.Fatalln("Do not consolidate SensorData from different Sources") + } } var ( - errNotImplemented = errors.New("message not implemented") - errRawMessage = errors.New("raw message") + errNotImplemented = errors.New("message not implemented") + errRawMessage = errors.New("raw message") ) func ConvertUbxSensorData(msg interface{}) (*SensorData, error) { - sd := &SensorData{ - //Servertime: time.Now().UTC(), - source: SOURCE_SERIAL, - } - switch v := msg.(type) { - case *ublox.NavPvt: - //logrus.Println("NAV-PVT") - 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) - sd.Position[0] = float64(v.Lat_dege7) / 1e+7 - sd.Position[1] = float64(v.Lon_dege7) / 1e+7 - sd.Position[2] = float64(v.HMSL_mm) / 1e+3 // mm in m - case *ublox.HnrPvt: - //logrus.Println("HNR-PVT") - 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) - sd.Position[0] = float64(v.Lat_dege7) / 1e+7 - sd.Position[1] = float64(v.Lon_dege7) / 1e+7 - sd.Position[2] = float64(v.HMSL_mm) / 1e+3 // mm in m - case *ublox.NavAtt: - //logrus.Println("NAV-ATT") - sd.itow = v.ITOW_ms - sd.Orientation[0] = float64(v.Pitch_deg) * 1e-5 - sd.Orientation[1] = float64(v.Roll_deg) * 1e-5 - sd.Orientation[2] = float64(v.Heading_deg) * 1e-5 - case *ublox.RawMessage: - //class := make([]byte, 2) - //binary.LittleEndian.PutUint16(class, v.ClassID()) - //logrus.Printf("%#v, %#v", class[0],class[1]) - return nil, nil - default: - return nil, errNotImplemented - } - return sd, nil + sd := &SensorData{ + //Servertime: time.Now().UTC(), + source: SOURCE_SERIAL, + } + switch v := msg.(type) { + case *ublox.NavPvt: + //logrus.Println("NAV-PVT") + 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) + sd.Position[0] = float64(v.Lat_dege7) / 1e+7 + sd.Position[1] = float64(v.Lon_dege7) / 1e+7 + sd.Position[2] = float64(v.HMSL_mm) / 1e+3 // mm in m + sd.HAcc = float64(v.HAcc_mm) / 1000 + sd.VAcc = float64(v.VAcc_mm) / 1000 + sd.HeadMotion = float64(v.HeadMot_dege5) / 1e+5 + sd.HeadDevice = float64(v.HeadVeh_dege5) / 1e+5 + sd.HeadingAcc = float64(v.HeadAcc_dege5) / 1e+5 + sd.Speed = float64(v.GSpeed_mm_s) / 3600 + case *ublox.HnrPvt: + //logrus.Println("HNR-PVT") + 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) + sd.Position[0] = float64(v.Lat_dege7) / 1e+7 + sd.Position[1] = float64(v.Lon_dege7) / 1e+7 + sd.Position[2] = float64(v.HMSL_mm) / 1e+3 // mm in m + sd.HAcc = float64(v.HAcc) / 1000 + sd.VAcc = float64(v.VAcc) / 1000 + sd.HeadMotion = float64(v.HeadMot_dege5) / 1e+5 + sd.HeadDevice = float64(v.HeadVeh_dege5) / 1e+5 + sd.HeadingAcc = float64(v.HeadAcc_dege5) / 1e+5 + sd.Speed = float64(v.GSpeed_mm_s) / 3600 + case *ublox.NavAtt: + //logrus.Println("NAV-ATT") + sd.itow = v.ITOW_ms + sd.Orientation[0] = float64(v.Pitch_deg) * 1e-5 + sd.Orientation[1] = float64(v.Roll_deg) * 1e-5 + sd.Orientation[2] = float64(v.Heading_deg) * 1e-5 + case *ublox.RawMessage: + //class := make([]byte, 2) + //binary.LittleEndian.PutUint16(class, v.ClassID()) + //logrus.Printf("%#v, %#v", class[0],class[1]) + return nil, nil + default: + return nil, errNotImplemented + } + return sd, nil } func ConvertByteSensorData(jsonData []byte) (*SensorData, error) { - if gjson.Get(string(jsonData), "os").String() == "hyperimu" { - return convertAndroidHyperImu(jsonData) - } - return convertIPhoneSensorLog(jsonData) + if gjson.Get(string(jsonData), "os").String() == "hyperimu" { + return convertAndroidHyperImu(jsonData) + } + return convertIPhoneSensorLog(jsonData) } func convertIPhoneSensorLog(jsonData []byte) (*SensorData, error) { - timestamp := gjson.Get(string(jsonData), "locationTimestamp_since1970").Float() - lat := gjson.Get(string(jsonData), "locationLatitude").Float() - lon := gjson.Get(string(jsonData), "locationLongitude").Float() - alt := gjson.Get(string(jsonData), "locationAltitude").Float() - pitch := gjson.Get(string(jsonData), "motionPitch").Float() * 180 / math.Pi - roll := gjson.Get(string(jsonData), "motionRoll").Float() * 180 / math.Pi - yaw := gjson.Get(string(jsonData), "motionYaw").Float() * 180 / math.Pi - var ts time.Time - if timestamp != 0 { - ts = time.Unix(0, int64(timestamp*float64(time.Second))).UTC() - timeex = time.Now().UnixNano() - ts.UnixNano() - } else if timeex != 0 { - ts = time.Now().Add(time.Duration(timeex)).UTC() - } - //if ts == time.Date() - sd := &SensorData{ - //Servertime: time.Now().UTC(), - source: SOURCE_TCP, - Timestamp: ts, - Position: [3]float64{lat, lon, alt}, - Orientation: [3]float64{pitch, roll, yaw}, - } - if (*sd == SensorData{}) { - return nil, errors.New("iphone sensorlog: convert empty") - } - return sd, nil + timestamp := gjson.Get(string(jsonData), "locationTimestamp_since1970").Float() + lat := gjson.Get(string(jsonData), "locationLatitude").Float() + lon := gjson.Get(string(jsonData), "locationLongitude").Float() + alt := gjson.Get(string(jsonData), "locationAltitude").Float() + pitch := gjson.Get(string(jsonData), "motionPitch").Float() * 180 / math.Pi + roll := gjson.Get(string(jsonData), "motionRoll").Float() * 180 / math.Pi + yaw := gjson.Get(string(jsonData), "motionYaw").Float() * 180 / math.Pi + hAcc := gjson.Get(string(jsonData), "locationHorizontalAccuracy").Float() + vAcc := gjson.Get(string(jsonData), "locationVerticalAccuracy").Float() + headingAcc := gjson.Get(string(jsonData), "locationHeadingAccuracy").Float() + headMotion := gjson.Get(string(jsonData), "locationCourse").Float() + headDevice := gjson.Get(string(jsonData), "locationTrueHeading").Float() + speed := gjson.Get(string(jsonData), "locationSpeed").Float() + + var ts time.Time + if timestamp != 0 { + ts = time.Unix(0, int64(timestamp*float64(time.Second))).UTC() + timeex = time.Now().UnixNano() - ts.UnixNano() + } else if timeex != 0 { + ts = time.Now().Add(time.Duration(timeex)).UTC() + } + //if ts == time.Date() + sd := &SensorData{ + //Servertime: time.Now().UTC(), + source: SOURCE_TCP, + Timestamp: ts, + Position: [3]float64{lat, lon, alt}, + Orientation: [3]float64{pitch, roll, yaw}, + HAcc: hAcc, + VAcc: vAcc, + HeadingAcc: headingAcc, + HeadMotion: headMotion, + HeadDevice: headDevice, + Speed: speed, + + } + if (*sd == SensorData{}) { + return nil, errors.New("iphone sensorlog: convert empty") + } + return sd, nil } func convertAndroidHyperImu(jsonData []byte) (*SensorData, error) { - timestamp := gjson.Get(string(jsonData), "Timestamp").Int() - lat := gjson.Get(string(jsonData), "GPS.0").Float() - lon := gjson.Get(string(jsonData), "GPS.1").Float() - alt := gjson.Get(string(jsonData), "GPS.2").Float() - pitch := gjson.Get(string(jsonData), "orientation.0").Float() - roll := gjson.Get(string(jsonData), "orientation.1").Float() - yaw := gjson.Get(string(jsonData), "orientation.2").Float() + timestamp := gjson.Get(string(jsonData), "Timestamp").Int() + lat := gjson.Get(string(jsonData), "GPS.0").Float() + lon := gjson.Get(string(jsonData), "GPS.1").Float() + alt := gjson.Get(string(jsonData), "GPS.2").Float() + pitch := gjson.Get(string(jsonData), "orientation.0").Float() + roll := gjson.Get(string(jsonData), "orientation.1").Float() + yaw := gjson.Get(string(jsonData), "orientation.2").Float() + //hAcc := gjson.Get(string(jsonData), "locationHorizontalAccuracy").Float() + //vAcc := gjson.Get(string(jsonData), "locationVerticalAccuracy").Float() + //headingAcc := gjson.Get(string(jsonData), "locationHeadingAccuracy").Float() + //headMotion := gjson.Get(string(jsonData), "locationCourse").Float() + //headDevice := gjson.Get(string(jsonData), "locationTrueHeading").Float() + //speed := gjson.Get(string(jsonData), "locationSpeed").Float() - sd := &SensorData{ - //Servertime: time.Now().UTC(), - source: SOURCE_TCP, - Timestamp: time.Unix(0, timestamp*int64(time.Millisecond)).UTC(), - Position: [3]float64{lat, lon, alt}, - Orientation: [3]float64{pitch, roll, yaw}, - } - if (*sd == SensorData{}) { - return nil, errors.New("android hyperimu: convert empty") - } - return sd, nil + + sd := &SensorData{ + //Servertime: time.Now().UTC(), + source: SOURCE_TCP, + Timestamp: time.Unix(0, timestamp*int64(time.Millisecond)).UTC(), + Position: [3]float64{lat, lon, alt}, + Orientation: [3]float64{pitch, roll, yaw}, + } + if (*sd == SensorData{}) { + return nil, errors.New("android hyperimu: convert empty") + } + return sd, nil } diff --git a/core/pipelinex.go b/core/pipelinex.go index cb96f2c..115acd0 100644 --- a/core/pipelinex.go +++ b/core/pipelinex.go @@ -5,6 +5,7 @@ import ( ext "github.com/reugn/go-streams/extension" "github.com/reugn/go-streams/flow" "github.com/sirupsen/logrus" + "github.com/tidwall/pretty" "time" ) @@ -63,7 +64,7 @@ func storeFunc(s Tracker) flow.MapFunc { } jdata, err := json.Marshal(data) - //logrus.Println(string(pretty.Pretty(jdata))) + logrus.Traceln(string(pretty.Pretty(jdata))) if err != nil { logrus.Fatalln(err) }