diff --git a/calculator/const.go b/calculator/const.go new file mode 100644 index 0000000..ebeffe1 --- /dev/null +++ b/calculator/const.go @@ -0,0 +1,9 @@ +package calculator + +// WGS84 ellipsoid constants +const ( + a = 6378137.0 // semi-major axis in meters + f = 1 / 298.257223563 // flattening + e2 = 2*f - f*f // square of eccentricity + J2000Epoch = 2451545.0 // Julian date of J2000 epoch +) \ No newline at end of file diff --git a/calculator/j2000.go b/calculator/j2000.go new file mode 100644 index 0000000..fac7b8c --- /dev/null +++ b/calculator/j2000.go @@ -0,0 +1,78 @@ +package calculator + +import ( + "math" + "time" +) + +// Function to convert current J2000 position to WGS84 +func J2000ToWGS84(j2000X, j2000Y, j2000Z float64, utc time.Time) (float64, float64, float64) { + julianDate := UTCToJulianDate(utc) + gast := CalculateGAST(julianDate, utc) + + // Calculate rotation matrix from J2000 to ITRS + rotationMatrix := CreateRotationMatrix(gast) + + // Rotate J2000 position to ITRS + itrsX := rotationMatrix[0][0]*j2000X + rotationMatrix[0][1]*j2000Y + rotationMatrix[0][2]*j2000Z + itrsY := rotationMatrix[1][0]*j2000X + rotationMatrix[1][1]*j2000Y + rotationMatrix[1][2]*j2000Z + itrsZ := rotationMatrix[2][0]*j2000X + rotationMatrix[2][1]*j2000Y + rotationMatrix[2][2]*j2000Z + + // Convert ITRS to geodetic coordinates (WGS84) + latitude, longitude, height := ECEFToGeodetic(itrsX, itrsY, itrsZ) + return latitude, longitude, height +} + +// Function to convert UTC to Julian Date +func UTCToJulianDate(t time.Time) float64 { + year, month, day := t.Date() + hour, minute, second := t.Clock() + fracOfDay := float64(hour)/24 + float64(minute)/(24*60) + float64(second)/(24*60*60) + + if month <= 2 { + year -= 1 + month += 12 + } + + A := year / 100 + B := 2 - A + A/4 + julianDate := float64(int(365.25*float64(year))) + float64(int(30.6001*float64(month+1))) + float64(day) + 1720994.5 + fracOfDay + float64(B) + return julianDate +} + +// Function to calculate GAST (Greenwich Apparent Sidereal Time) +func CalculateGAST(julianDate float64, utc time.Time) float64 { + // Calculate Julian centuries since J2000.0 + T := (julianDate - J2000Epoch) / 36525.0 + + // Calculate Greenwich Mean Sidereal Time (GMST) in degrees + GMST := 280.46061837 + 360.98564736629*(julianDate-J2000Epoch) + 0.000387933*T*T - T*T*T/38710000 + GMST = math.Mod(GMST, 360.0) + if GMST < 0 { + GMST += 360.0 + } + + // Calculate the equation of the equinoxes (simplified) + omega := 125.04 - 0.052954*T + L := 280.47 + 0.98565*T + epsilon := 23.4393 - 0.0000004*T + deltaPsi := -0.000319*math.Sin(math.Pi/180.0*omega) - 0.000024*math.Sin(math.Pi/180.0*2*L) + + // Greenwich Apparent Sidereal Time (GAST) + GAST := GMST + deltaPsi*math.Cos(math.Pi/180.0*epsilon) + GAST = math.Mod(GAST, 360.0) + if GAST < 0 { + GAST += 360.0 + } + + return GAST * math.Pi / 180.0 // Convert to radians +} + +// Function to create the rotation matrix for the given GAST +func CreateRotationMatrix(gast float64) [3][3]float64 { + return [3][3]float64{ + {math.Cos(gast), math.Sin(gast), 0}, + {-math.Sin(gast), math.Cos(gast), 0}, + {0, 0, 1}, + } +} diff --git a/calculator/proj.go b/calculator/proj.go deleted file mode 100644 index 69d8dfc..0000000 --- a/calculator/proj.go +++ /dev/null @@ -1,24 +0,0 @@ -package calculator - -import "github.com/twpayne/go-proj/v10" - -func J2000ToLngLat(x, y, z float64) []float64 { - var lng, lat float64 - return []float64{lng, lat} -} - -func WGS84XYZtoLngLat(x, y, z float64) []float64 { - var lng, lat float64 - pj, err := proj.NewCRSToCRS("EPSG:3857", "EPSG:4326", nil) - if err != nil { - panic(err) - } - coor := proj.NewCoord(x, y, z, 0) - coor, err = pj.Forward(coor) - if err != nil { - panic(err) - } - lng = coor.X() - lat = coor.Y() - return []float64{lng, lat} -} diff --git a/calculator/wgs84.go b/calculator/wgs84.go new file mode 100644 index 0000000..1f6cd0c --- /dev/null +++ b/calculator/wgs84.go @@ -0,0 +1,31 @@ +package calculator + +import "math" + +func WGS84XYZtoLatLngH(X, Y, Z float64) (float64, float64, float64) { + return ECEFToGeodetic(X, Y, Z) +} + +// Function to convert ECEF (ITRS) coordinates to geodetic coordinates (latitude, longitude, height) +func ECEFToGeodetic(X, Y, Z float64) (float64, float64, float64) { + b := a * (1 - f) + e2Prime := e2 * (a * a) / (b * b) + p := math.Sqrt(X*X + Y*Y) + theta := math.Atan2(Z*a, p*b) + + // Calculate Longitude + longitude := math.Atan2(Y, X) + + // Calculate Latitude + latitude := math.Atan2(Z+e2Prime*b*math.Pow(math.Sin(theta), 3), p-e2*a*math.Pow(math.Cos(theta), 3)) + + // Calculate Height + N := a / math.Sqrt(1-e2*math.Pow(math.Sin(latitude), 2)) + height := p/math.Cos(latitude) - N + + // Convert radians to degrees for latitude and longitude + latitudeDeg := latitude * 180 / math.Pi + longitudeDeg := longitude * 180 / math.Pi + + return latitudeDeg, longitudeDeg, height +} diff --git a/cmd/parse.go b/cmd/parse.go index 1e31bf4..642e452 100644 --- a/cmd/parse.go +++ b/cmd/parse.go @@ -29,6 +29,7 @@ var parseCmd = &cobra.Command{ logrus.Error(err) } // p.ParseAuxEBox("demo/output/SJY01_PMS_20240516_101236_051622_096_EB.AUX") + e.ParseAuxPlatform("demo/output/Q052100/SJY01_PMS_20240519_121433_Q052100_102.AUX") }, } diff --git a/extract/aux_platform.go b/extract/aux_platform.go index 5a7a108..ffdf32f 100644 --- a/extract/aux_platform.go +++ b/extract/aux_platform.go @@ -9,6 +9,7 @@ import ( "github.com/k0kubun/pp/v3" "github.com/sirupsen/logrus" log "github.com/sirupsen/logrus" + "starwiz.cn/sjy01/preprocessing/calculator" ) const AuxPlatformFrmSize = 512 // 512 bytes @@ -29,9 +30,9 @@ type AuxPlatform struct { QuatOrbJQ1 float64 // [37-40]轨道相对惯性系四元数矢部 的 Q1 值，量纲 1/0x40000000 QuatOrbJQ2 float64 // [41-44]Q2 值 QuatOrbJQ3 float64 // [45-48]Q3 值 - Eular1 float64 // [49-52]本体相对轨道姿态角，量纲：1/1000000 单位：rad - Eular2 float64 // [53-56] - Eular3 float64 // [57-60] + Eular1 float64 // [49-52]本体相对轨道姿态角，[Z]Yaw 测摆角，量纲：1/1000000 单位：rad + Eular2 float64 // [53-56] Pitch [Y] + Eular3 float64 // [57-60] Roll [X] DotEular1 float64 // [61-62]本体相对轨道角速度，量纲：1/100000 单位：rad DotEular2 float64 // [63-64] DotEular3 float64 // [65-66] @@ -353,23 +354,32 @@ func (e *Extractor) ParseAuxPlatform(auxfile string) ([]*AuxPlatform, error) { } aps = append(aps, &ap) - ap.Print() + // ap.Print() // Calculate( // Vector3{ap.J2000Pos_X, ap.J2000Pos_Y, ap.J2000Pos_Y}, // Vector3{ap.J2000Vel_X, ap.J2000Vel_Y, ap.J2000Vel_Z}, // Quaternion{ap.SS1_Q1, ap.SS1_Q2, ap.SS1_Q3, ap.SS1_Q4}, // ) - // pp.Println(calculator.WGS84XYZtoLngLat(ap.W84PosX, ap.W84PosY, ap.W84PosZ)) - // pp.Println(calculator.WGS84XYZtoLngLat(ap.WGS84PosX, ap.WGS84PosY, ap.WGS84PosZ)) + pp.Println(calculator.WGS84XYZtoLatLngH(ap.W84PosX, ap.W84PosY, ap.W84PosZ)) + pp.Println(calculator.WGS84XYZtoLatLngH(ap.WGS84PosX, ap.WGS84PosY, ap.WGS84PosZ)) + pp.Println(calculator.J2000ToWGS84( + ap.J2000PosX, + ap.J2000PosY, + ap.J2000PosZ, + time.Unix( + int64(ap.UTCTimeSec+uint32(ReferenceTime2000)), + int64(ap.Microsecond)*1000).UTC())) + + pp.Println(calculator.J2000ToWGS84( + ap.J2000Pos_X, + ap.J2000Pos_Y, + ap.J2000Pos_Z, + time.Unix( + int64(ap.UTCTimeSec+uint32(ReferenceTime2000)), + int64(ap.Microsecond)*1000).UTC())) break - - if i+AuxPlatformFrmSize > len(data) { - logrus.Info("rest of aux data length is not enough", len(data)-i) - break - } - } return aps, nil }