266 lines
8.1 KiB
Go
266 lines
8.1 KiB
Go
package core
|
|
|
|
import (
|
|
"errors"
|
|
"git.timovolkmann.de/gyrogpsc/ublox"
|
|
"github.com/google/uuid"
|
|
"github.com/sirupsen/logrus"
|
|
"github.com/tidwall/gjson"
|
|
"math"
|
|
"time"
|
|
)
|
|
|
|
// holds all sensor data and metadata for a specific recording
|
|
type Tracking struct {
|
|
TrackingMetadata
|
|
Data []SensorData
|
|
}
|
|
|
|
// holds all metadata for a specific recording
|
|
type TrackingMetadata struct {
|
|
UUID uuid.UUID
|
|
TimeCreated time.Time
|
|
Collectors []CollectorType
|
|
Size int
|
|
}
|
|
|
|
func newTracking() Tracking {
|
|
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
|
|
}
|
|
|
|
// enumerate sources
|
|
type SourceId string
|
|
|
|
const (
|
|
SOURCE_TCP SourceId = "SOURCE_TCP"
|
|
SOURCE_SERIAL SourceId = "SOURCE_SERIAL"
|
|
)
|
|
|
|
// internal unified representation for smartphone and ublox sensor data
|
|
type SensorData struct {
|
|
//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
|
|
}
|
|
|
|
func (s *SensorData) SetSource(si SourceId) {
|
|
s.source = si
|
|
}
|
|
|
|
func (s SensorData) isSameEpoch(n SensorData) bool {
|
|
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{}
|
|
|
|
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{}
|
|
|
|
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")
|
|
}
|
|
}
|
|
|
|
var (
|
|
errNotImplemented = errors.New("message not implemented")
|
|
errRawMessage = errors.New("raw message")
|
|
)
|
|
|
|
var lastTimeOffsetIphone int64
|
|
var lastTimeOffsetUblox int64
|
|
|
|
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
|
|
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) * 1e-3
|
|
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) * 1e-3 // mm in m/s
|
|
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:
|
|
return nil, nil
|
|
default:
|
|
return nil, errNotImplemented
|
|
}
|
|
|
|
if !sd.Timestamp.IsZero() && sd.Timestamp.Nanosecond() != 0 {
|
|
lastTimeOffsetUblox = time.Now().UnixNano() - sd.Timestamp.UnixNano()
|
|
} else {
|
|
sd.Timestamp = time.Now().UTC().Add(time.Duration(lastTimeOffsetUblox))
|
|
}
|
|
|
|
return sd, nil
|
|
}
|
|
|
|
func ConvertByteSensorData(jsonData []byte) (*SensorData, error) {
|
|
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
|
|
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()
|
|
lastTimeOffsetIphone = time.Now().UnixNano() - ts.UnixNano()
|
|
} else {
|
|
ts = time.Now().UTC().Add(time.Duration(lastTimeOffsetIphone))
|
|
}
|
|
//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()
|
|
//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
|
|
}
|