package extract import ( "encoding/binary" "fmt" "os" "github.com/sirupsen/logrus" ) const AuxPlatformFrmSize = 512 // 512 bytes // 中心机辅助数据 512 字节 (每 16 行原始图像数据为一组) type AuxPlatform struct { // 结构中标注的字节序号从 1 开始计数 UTCTimeSec uint32 // [1-4] 卫星 UTC 时间戳（秒 Waveway uint8 // [5] 波道 Microsecond uint32 // [6-8]卫星秒小数（微秒） QuatAttstarQ0 float64 // [9-12]定姿四元数(J2000) 的 Q0 值，量纲 1/0x40000000 QuatAttstarQ1 float64 // [13-16]Q1 值 QuatAttstarQ2 float64 // [17-20]Q2 值 QuatAttstarQ3 float64 // [21-24]Q3 值 QuatOrbitQ1 float64 // [25-28]本体相对轨道四元数矢部 的 Q1 值，量纲 1/0x40000000 QuatOrbitQ2 float64 // [29-32]Q2 值 QuatOrbitQ3 float64 // [33-36]Q3 值Q3 值 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] DotEular1 float64 // [61-62]本体相对轨道角速度，量纲：1/100000 单位：rad DotEular2 float64 // [63-64] DotEular3 float64 // [65-66] ModTime uint32 // [67-70]模式运行时间 秒 DTime int16 // [71-72]姿控调用周期，量纲：1/1000 单位：秒 AutoState [3]uint8 // [73-75]姿轨控部件使用标志 ProTrack [16]uint8 // [76-91]姿轨控算法执行标记 QeQ1 float64 // [92-95]偏差四元数 double 量纲：1/0x40000000 /bit QeQ2 float64 // [96-99] QeQ3 float64 // [100-103] We1 float64 // [104-105]偏差角速度 量纲：1/0x100000 单位：rad We2 float64 // [106-107] We3 float64 // [108-109] WTFX float64 // [110-111]X飞轮估计摩擦力矩 量纲：1/0x400000 WTFY float64 // [112-113] WTFZ float64 // [114-115] FbdriftX float64 // [116-117]三轴陀螺X轴角速度漂移估计 1/0x4000000 rad/s FbdriftY float64 // [118-119] FbdriftZ float64 // [120-121] KalbX float64 // [122-123]三轴角速度卡尔曼漂移X 量纲：1/0x4000000 单位：rad/s KalbY float64 // [124-125] KalbZ float64 // [126-127] HTDX float64 // [128-129]X轴估计环境干扰力矩 量纲：1/0x200000 HTDY float64 // [130-131] HTDZ float64 // [132-133] CommandWheelX float64 // [134-135]X轴计算飞轮控制力矩 量纲：1/0x40000 CommandWheelY float64 // [136-137] CommandWheelZ float64 // [138-139] QuatG1 float64 // [140-143]期望四元数矢部1 量纲：1/0x40000000 /bit QuatG2 float64 // [144-147] QuatG3 float64 // [148-151] WG1 float64 // [152-153]期望角速度1 量纲：1/0x40000 WG2 float64 // [154-155] WG3 float64 // [156-157] J2000PosX float64 // [158-161]计算当前J2000位置X 1/0x100 单位：m J2000PosY float64 // [162-165] J2000PosZ float64 // [166-169] J2000VelX float64 // [170-173]计算当前J2000速度X 量纲：1/0x100 单位：m/s J2000VelY float64 // [174-177] J2000VelZ float64 // [178-181] W84PosX float64 // [182-185]计算当前WGS 84位置X 1/0x100 单位：m W84PosY float64 // [186-189] W84PosZ float64 // [190-193] W84VelX float64 // [194-197]计算当前WGS 84速度X 量纲：1/0x100 单位：m/s W84VelY float64 // [198-201] W84VelZ float64 // [202-205] AngleDraft float64 // [206-209]偏流角 量纲：1/10000000 单位：rad DataTransLong float64 // [210-211]数传点经度 量纲：1/1000 DataTransLat float64 // [212-213]数传点纬度 量纲：1/1000 DataTransH float64 // [214-215]数传点地程高 量纲：1 StarPrio1 byte // [216] 星敏1定姿方式标志1 0x00：星敏1定姿；0xA0:星敏1滤波;0x01星敏2定姿；0xA1:星敏2滤波;0xff:未使用星敏定姿 StarPrio2 byte // [217] StarPrio3 byte // [218] WheelspeedcalX float64 // [219-222]X飞轮期望转速 量纲：1/10000 单位：rpm WheelspeedcalY float64 // [223-226] WheelspeedcalZ float64 // [227-230] Reserved0 [2]byte // 预留字节 Reserved1 [4]byte // 预留字节 Reserved2 [2]byte // 预留字节 Reserved3 [2]byte // 预留字节 // 241 - 311 暂不解析 WGS84PosX float64 // [312-315]计算当前WGS 84位置X 单位：0.01米 WGS84PosY float64 // [316-319] WGS84PosZ float64 // [320-323] WGS84VelX float64 // [324-327]计算当前WGS 84速度X 单位：0.01米/秒 WGS84VelY float64 // [328-331] WGS84VelZ float64 // [332-335] J2000Pos_X float64 // [336-339]计算当前J2000位置X 单位：0.01米 J2000Pos_Y float64 // [340-343] J2000Pos_Z float64 // [344-347] J2000Vel_X float64 // [348-351]计算当前J2000速度X 单位：0.01米/秒 J2000Vel_Y float64 // [352-355] J2000Vel_Z float64 // [356-359] // 字节[360-377] 三轴光纤陀螺参数 SS1_UTCTime uint32 // [378-381] 星敏1 UTC时间 SS1_UTCTimeFrac float32 // [382-384] 星敏1 UTC秒小数 单位为40.96 us，高字节在前 SS1_Q1 float64 // [385-388] 星敏1四元数q1 当量：1/2147483647 （星敏坐标系相对于J2000惯性坐标系） SS1_Q2 float64 // [389-392] 星敏1四元数q2 SS1_Q3 float64 // [393-396] 星敏1四元数q3 SS1_Q4 float64 // [397-400] 星敏1四元数q4 SS1_ExposureTime uint8 // [401] 星敏1曝光时间 无符号数，单位ms SS1_ImgFrmNo uint32 // [411-413] 星敏1图像帧号 SS2_UTCTime uint32 // [424-427] 星敏2 UTC时间 SS2_UTCTimeFrac float32 // [428-430] 星敏2 UTC秒小数 单位为40.96 us，高字节在前 SS2_Q1 float64 // [431-434] 星敏2四元数q1 当量：1/2147483647 （星敏坐标系相对于J2000惯性坐标系） SS2_Q2 float64 // [435-438] 星敏2四元数q2 SS2_Q3 float64 // [439-442] 星敏2四元数q3 SS2_Q4 float64 // [443-446] 星敏2四元数q4 SS2_ExposureTime uint8 // [447] 星敏2曝光时间 无符号数，单位ms SS2_ImgFrmNo uint32 // [457-459] 星敏2图像帧号 CheckSum byte // [512] 校验和 } func (ap *AuxPlatform) Parse(data []byte) error { if len(data) < 512 { return ErrAuxPlatformDataLen } // 按需解析数据 ap.UTCTimeSec = binary.BigEndian.Uint32(data[0:4]) ap.Microsecond = uint32(data[5])<<16 | uint32(data[6])<<8 | uint32(data[7]) ap.DataTransLong = float64(int16(binary.BigEndian.Uint16(data[209:211]))) / 1000 ap.DataTransLat = float64(int16(binary.BigEndian.Uint16(data[211:213]))) / 1000 ap.DataTransH = float64(binary.BigEndian.Uint16(data[213:215])) ap.SS1_UTCTime = binary.BigEndian.Uint32(data[377:381]) ap.SS1_UTCTimeFrac = float32(binary.BigEndian.Uint16(data[381:384])) * 40.96 ap.SS1_ImgFrmNo = uint32(data[410])<<16 | uint32(data[411])<<8 | uint32(data[412]) ap.SS2_UTCTime = binary.BigEndian.Uint32(data[423:427]) ap.SS2_UTCTimeFrac = float32(binary.BigEndian.Uint16(data[427:430])) * 40.96 ap.SS1_ImgFrmNo = uint32(data[456])<<16 | uint32(data[457])<<8 | uint32(data[458]) return nil } func (ap AuxPlatform) Print() { fmt.Println("--- aux frm ----") fmt.Printf("UTCTimeSec: %d\n", ap.UTCTimeSec) fmt.Printf("Microsecond: %d\n", ap.Microsecond) fmt.Printf("DataTransLong: %f\n", ap.DataTransLong) fmt.Printf("DataTransLat: %f\n", ap.DataTransLat) fmt.Printf("DataTransH: %f\n", ap.DataTransH) fmt.Println("--- ss1 ---") fmt.Printf("SS1_UTCTime: %d\n", ap.SS1_UTCTime) fmt.Printf("SS1_UTCTimeFrac: %f\n", ap.SS1_UTCTimeFrac) fmt.Printf("SS1_ImgFrmNo: %d\n", ap.SS1_ImgFrmNo) fmt.Println("--- ss2 ---") fmt.Printf("SS2_UTCTime: %d\n", ap.SS2_UTCTime) fmt.Printf("SS2_UTCTimeFrac: %f\n", ap.SS2_UTCTimeFrac) fmt.Printf("SS2_ImgFrmNo: %d\n", ap.SS2_ImgFrmNo) } // Extractor 辅助数据提取器 // auxfile 辅助数据文件路径 func (e *Extractor) ParseAuxPlatform(auxfile string) ([]*AuxPlatform, error) { data, err := os.ReadFile(auxfile) if err != nil { logrus.Println("read aux data from", auxfile, "error:", err.Error()) return nil, err } var aps []*AuxPlatform for i := 0; i < len(data); i += AuxPlatformFrmSize { ap := AuxPlatform{} if err := ap.Parse(data[i : i+AuxPlatformFrmSize]); err != nil { return nil, err } ap.Print() aps = append(aps, &ap) if i+AuxPlatformFrmSize > len(data) { logrus.Info("rest of aux data length is not enough", len(data)-i) break } } return aps, nil }