gyrogpsc/core/sensordata.go

package core

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

type sourceId string

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

type Sensordata struct {
	itow        uint32
	SourceId    sourceId
	Timestamp   int64
	Position    [3]float64
	Orientation [3]float64
}

func (s Sensordata) isSameEpoch(n Sensordata) bool {
	if n.itow == 0 {
		return false
	}
	return s.itow == n.itow
}

func (s Sensordata) Consolidate(n Sensordata) Sensordata {
	if (s.SourceId != n.SourceId && s != Sensordata{}) {
		log.Println(s)
		log.Println(n)
		log.Fatalln("Do not consolidate Sensordata from different Sources")
	}
	if s.isSameEpoch(n) {
		null := Sensordata{}
		//if s.Timestamp == null.Timestamp { s.Timestamp = n.Timestamp }
		//if s.Position == null.Position { s.Position = n.Position }
		//if s.Orientation == null.Orientation { s.Orientation = n.Orientation }
		if n.Timestamp != null.Timestamp && s.Timestamp != n.Timestamp {
			s.Timestamp = n.Timestamp
		}
		if n.Position != null.Position && s.Position != n.Position {
			s.Position = n.Position
		}
		if n.Orientation != null.Orientation && s.Orientation != n.Orientation {
			s.Orientation = n.Orientation
		}

		return s
	}
	return n
}

func (s Sensordata) Consolidate2(n Sensordata) Sensordata {
	null := Sensordata{}
	//if s.Timestamp == null.Timestamp { s.Timestamp = n.Timestamp }
	//if s.Position == null.Position { s.Position = n.Position }
	//if s.Orientation == null.Orientation { s.Orientation = n.Orientation }
	//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
}

var (
	errNotImplemented = errors.New("message not implemented")
	errRawMessage     = errors.New("raw message")
)

func ConvertUbxToSensorData(msg interface{}) (*Sensordata, error) {
	sd := &Sensordata{
		SourceId: SOURCE_SERIAL,
	}
	switch v := msg.(type) {
	case *ublox.NavPvt:
		//log.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:
		//log.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:
		//log.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())
		//log.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{
		SourceId:  SOURCE_TCP,
		Timestamp: int64(timestamp * float64(time.Second)),
		//Timestamp:   time.Unix(0, prep.Timestamp * int64(time.Millisecond)),
		Position:    [3]float64{lat, lon, alt},
		Orientation: [3]float64{pitch, roll, yaw},
	}
	//log.Println(string(pretty.Pretty(jsonData)))
	//log.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{
		SourceId:  SOURCE_TCP,
		Timestamp: timestamp * int64(time.Millisecond),
		//Timestamp:   time.Unix(0, prep.Timestamp * int64(time.Millisecond)),
		Position:    [3]float64{lat, lon, alt},
		Orientation: [3]float64{pitch, roll, yaw},
	}
	return sd, nil
}