tvolkmann/gyrogpsc
1
0
Fork 0
Code Issues Pull Requests Projects 1 Releases 1 Wiki Activity

added fields to sensordata

Browse Source
This commit is contained in:
Timo Volkmann 2020-12-18 12:30:26 +01:00
parent 5a3a9feca8
commit 100787f047
2 changed files with 195 additions and 159 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

351
core/datamodel.go
View File

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

3
core/pipelinex.go
View File

@ -5,6 +5,7 @@ import (
ext "github.com/reugn/go-streams/extension" ext "github.com/reugn/go-streams/extension"
"github.com/reugn/go-streams/flow" "github.com/reugn/go-streams/flow"
"github.com/sirupsen/logrus" "github.com/sirupsen/logrus"
"github.com/tidwall/pretty"
"time" "time"
) )
@ -63,7 +64,7 @@ func storeFunc(s Tracker) flow.MapFunc {
} }
jdata, err := json.Marshal(data) jdata, err := json.Marshal(data)
//logrus.Println(string(pretty.Pretty(jdata))) logrus.Traceln(string(pretty.Pretty(jdata)))
if err != nil { if err != nil {
logrus.Fatalln(err) logrus.Fatalln(err)
} }