gyrogpsc/core/sensordata.go

package core

import (
    "errors"
    "git.timovolkmann.de/gyrogpsc/ublox"
    "github.com/sirupsen/logrus"
    "github.com/tidwall/gjson"
    "math"
    "time"
)

type sourceId string

const (
    SOURCE_TCP    sourceId = "SOURCE_TCP"
    SOURCE_SERIAL sourceId = "SOURCE_SERIAL"
)

type sensorData struct {
    itow        uint32
    source      sourceId
    ServerTime  time.Time
    Timestamp   int64
    Position    [3]float64
    Orientation [3]float64
}

type recordPair struct {
    RecordTime time.Time
    data       map[sourceId]sensorData
}

type rawRecord struct {
    RecordTime time.Time
    sensorData
}

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")
)

func ConvertUbxToSensorData(msg interface{}) (*sensorData, error) {
    sd := &sensorData{
        ServerTime: time.Now(),
        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).UnixNano()
        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).UnixNano()
        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
}

func ConvertSensorDataPhone(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
    sd := &sensorData{
        ServerTime:  time.Now(),
        source:      SOURCE_TCP,
        Timestamp:   int64(timestamp * float64(time.Second)),
        Position:    [3]float64{lat, lon, alt},
        Orientation: [3]float64{pitch, roll, yaw},
        //Timestamp:   time.Unix(0, prep.Timestamp * int64(time.Millisecond)),
    }
    //logrus.Println(string(pretty.Pretty(jsonData)))
    //logrus.Println(sd)
    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()

    sd := &sensorData{
        ServerTime:  time.Now(),
        source:      SOURCE_TCP,
        Timestamp:   timestamp * int64(time.Millisecond),
        Position:    [3]float64{lat, lon, alt},
        Orientation: [3]float64{pitch, roll, yaw},
        //Timestamp:   time.Unix(0, prep.Timestamp * int64(time.Millisecond)),
    }
    return sd, nil
}