added fields to sensordata
This commit is contained in:
parent
5a3a9feca8
commit
100787f047
@ -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
|
||||
}
|
||||
|
||||
@ -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)
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user