ivan-igorevich/dc-observatio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ПУУД с постоянным азимутом

Browse Source
This commit is contained in:
emelianov
2026-06-09 12:31:53 +03:00
parent e1e96b1ff0
commit faf88eaf94
8 changed files with 184 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AbstractPuudCalculator.kt
+42 -57
View File
@@ -25,7 +25,6 @@ abstract class AbstractPuudCalculator(
protected val astro = AstronomerJ2000(earthType)
protected fun validate(id: SurveyId) {
require(id.nlv in 1..7) { "Номер линии визирования должен быть в диапазоне 1..7" }
require(id.duration >= 0.0) { "Длительность режима не может быть отрицательной" }
require(config.focus > 0.0) { "Фокусное расстояние должно быть положительным" }
}
@@ -36,27 +35,6 @@ abstract class AbstractPuudCalculator(
protected fun pointAt(t: Double): OrbitalPoint =
stepper.calculate(t) ?: throw AngularMotionCalculationException("Ошибка выхода на заданное время: $t")
protected fun lineAngle(nlv: Int): Double =
when (nlv) {
1 -> config.psk1Angle + config.lv1Angle
2 -> config.psk1Angle + config.lv2Angle
3 -> config.psk1Angle + config.lv3Angle
4 -> config.psk3Angle + config.lv4Angle
5 -> config.psk2Angle + config.lv5Angle
6 -> config.psk2Angle + config.lv6Angle
7 -> config.psk2Angle + config.lv7Angle
else -> 0.0
}
protected fun oepNlv(oep: Int): Int =
when (oep) {
1 -> 3
2 -> 1
3 -> 7
4 -> 5
else -> 4
}
protected fun sdiAt(sdi: List<Double>, elapsed: Double): Double =
if (sdi.isEmpty()) {
-1.0
@@ -129,10 +107,9 @@ abstract class AbstractPuudCalculator(
}
private fun equationValue(id: SurveyId, t: Double): Double =
orientOnPoint(id.nlv, t, id.b, id.l, id.h).tang - id.uprAngle
orientOnPoint(t, id.b, id.l, id.h).tang - id.uprAngle
protected fun orientOnPoint(
nlv: Int,
t: Double,
b: Double,
l: Double,
@@ -141,12 +118,11 @@ abstract class AbstractPuudCalculator(
): Orientation {
val ka = pointAt(t)
val targetGsk = astro.earth.blh2xyz(b, l, h)
val w = pointInOrbitalFrame(ka, targetGsk)
val compensated = rotationZ(-lineAngle(nlv)) * w
val orient = orientationFromOrbitalVector(compensated)
val targetInOrbit = pointInOrbitalFrame(ka, targetGsk)
val orient = orientationFromOrbitalVector(targetInOrbit)
if (routeDirectionGsk != null && routeDirectionGsk.module() > 1.0e-8) {
orient.risk = riskFromGroundDirection(ka, nlv, orient, routeDirectionGsk)
orient.risk = riskFromGroundDirection(ka, orient, routeDirectionGsk)
}
return orient
}
@@ -161,7 +137,6 @@ abstract class AbstractPuudCalculator(
*/
private fun riskFromGroundDirection(
kaGsk: OrbitalPoint,
nlv: Int,
orientWithoutRisk: Orientation,
routeDirectionGsk: Vector3D,
): Double {
@@ -169,9 +144,8 @@ abstract class AbstractPuudCalculator(
val routeAbs = astro.grinvToASK(routeDirectionGsk, kaGsk.t)
val routeOrbit = orbBookToOrbMatrix() * absToOrbBookMatrix(kaAbs.r, kaAbs.v) * routeAbs
val routeConnected = orientationMatrix(orientWithoutRisk).transpose() * routeOrbit
val routeClv = rotationZ(-lineAngle(nlv)) * routeConnected
val wx = routeClv.x
val wz = routeClv.z
val wx = routeConnected.x
val wz = routeConnected.z
return if (sqrt(wx * wx + wz * wz) < 1.0e-10) {
0.0
} else {
@@ -192,14 +166,13 @@ abstract class AbstractPuudCalculator(
return Orientation(tang, kren, 0.0)
}
protected fun pointOnEarth(point: OrbitalPoint, nlv: Int, orientation: Orientation): BLHPoint? {
protected fun pointOnEarth(point: OrbitalPoint, orientation: Orientation): BLHPoint? {
val ask = astro.grinvToASK(point)
val orbitToAbs = absToOrbBookMatrix(ask.r, ask.v).transpose() * orbBookToOrbMatrix().transpose()
val g = Matrix3D().also { it.makeOzMatrix(astro.si2000(point.t)) }.transpose()
val orbitToGsk = g * orbitToAbs
val connectedToOrbit = orientationMatrix(orientation)
val dConnected = lineOfSightVectorInConnected(lineAngle(nlv))
val direction = (orbitToGsk * connectedToOrbit * dConnected).normSafe()
val direction = (orbitToGsk * connectedToOrbit * lineOfSightVectorInConnected()).normSafe()
return earthIntersection(point.r, direction)
}
@@ -225,23 +198,20 @@ abstract class AbstractPuudCalculator(
return quaternionFromMatrix(absToConnected.transpose()).normalized()
}
protected fun wdForLines(point: OrbitalPoint, orientation: Orientation, omega: Vector3D): List<Vector3D> =
(1..7).map { nlv -> wd(point, nlv, orientation, omega) }
protected fun sdiForWd(wd: List<Vector3D>, sickle: Boolean = false): List<Double> =
wd.map { (if (sickle) it.z else it.x) * config.focus }
protected fun sdiForWd(wd: Vector3D, sickle: Boolean = false): Double =
(if (sickle) wd.z else wd.x) * config.focus
/**
* Аналог AbstractAISTPUUD::wd. Возвращает W/D в связанной СК для заданной линии визирования.
* Аналог AbstractAISTPUUD::wd. Возвращает W/D в связанной СК для единственной центральной линии визирования.
*
* W — относительная скорость движения по земной поверхности точки пересечения ЛВ с Землей.
* W — относительная скорость движения по земной поверхности точки пересечения ЦЛВ с Землей.
* Это не скорость КА: из скорости спутника вычитается переносное вращение Земли, а затем
* добавляется радиальная составляющая изменения наклонной дальности, чтобы точка оставалась
* на поверхности эллипсоида. Такая схема соответствует формулам из OrbitalMotion::AbstractAISTPUUD::wd.
*/
protected fun wd(point: OrbitalPoint, nlv: Int, orientation: Orientation, omegaConnected: Vector3D): Vector3D {
protected fun wd(point: OrbitalPoint, orientation: Orientation, omegaConnected: Vector3D): Vector3D {
val ask = astro.grinvToASK(point)
val lineConnected = lineOfSightVectorInConnected(lineAngle(nlv))
val lineConnected = lineOfSightVectorInConnected()
val connectedToOrbit = orientationMatrix(orientation)
val absToOrbBook = absToOrbBookMatrix(ask.r, ask.v)
val orbitBookToAbs = absToOrbBook.transpose()
@@ -283,25 +253,41 @@ abstract class AbstractPuudCalculator(
wGround = absToOrbBook * wGround
wGround = orbBookToOrbMatrix() * wGround
wGround = connectedToOrbit.transpose() * wGround
wGround = rotationZ(-lineAngle(nlv)) * wGround
return wGround / range
}
protected fun routeNormalInGreenwich(b: Double, l: Double, h: Double, azimuth: Double): Vector3D {
val first = astro.earth.blh2xyz(b, l, h)
val secondBlh = endPointByAzimuth(b, l, azimuth, 0.1 * PI / 180.0)
val second = astro.earth.blh2xyz(secondBlh.lat, secondBlh.long, h)
return first.rem(second).normSafe()
// Центральная линия маршрута по ТЗ задается сечением ОЗЭ плоскостью,
// построенной в начальной точке по нормали к ОЗЭ и касательной к ОЗЭ
// с заданным азимутом. Нормаль такой плоскости — вектор normal x tangent.
val normal = ellipsoidNormalInGreenwich(b, l)
val tangent = routeDirectionInGreenwich(b, l, azimuth)
return normal.rem(tangent).normSafe()
}
protected fun routeDirectionInGreenwich(b: Double, l: Double, azimuth: Double): Vector3D {
val north = northDirectionInGreenwich(b, l)
val east = eastDirectionInGreenwich(l)
return (north * cos(azimuth) + east * sin(azimuth)).normSafe()
}
protected fun routeDirectionInOrbitalFrame(id: SurveyId, t: Double): Vector3D {
val p1 = astro.earth.blh2xyz(id.b, id.l, id.h)
val p2 = astro.earth.blh2xyz(endPointByAzimuth(id.b, id.l, id.azimuth, 0.1 * PI / 180.0))
val ka = pointAt(t)
return pointInOrbitalFrame(ka, p2) - pointInOrbitalFrame(ka, p1)
val kaAbs = astro.grinvToASK(ka)
val routeAbs = astro.grinvToASK(routeDirectionInGreenwich(id.b, id.l, id.azimuth), t)
return orbBookToOrbMatrix() * absToOrbBookMatrix(kaAbs.r, kaAbs.v) * routeAbs
}
private fun ellipsoidNormalInGreenwich(b: Double, l: Double): Vector3D =
Vector3D(cos(b) * cos(l), cos(b) * sin(l), sin(b)).normSafe()
private fun eastDirectionInGreenwich(l: Double): Vector3D =
Vector3D(-sin(l), cos(l), 0.0).normSafe()
private fun northDirectionInGreenwich(b: Double, l: Double): Vector3D =
Vector3D(-sin(b) * cos(l), -sin(b) * sin(l), cos(b)).normSafe()
protected fun endPointByAzimuth(b: Double, l: Double, azimuth: Double, centralAngle: Double): BLHPoint {
val sinB = sin(b)
val cosB = cos(b)
@@ -314,7 +300,6 @@ abstract class AbstractPuudCalculator(
protected fun buildPoint(
t: Double,
nlv: Int,
orientation: Orientation,
previous: AngularMotionPoint? = null,
sickle: Boolean = false,
@@ -323,8 +308,8 @@ abstract class AbstractPuudCalculator(
val q = quaternionFor(orbital, orientation)
val omega = previous?.let { omegaFromTwoQuat(it.quaternion, q, t - it.t) } ?: Vector3D()
val eps = previous?.let { if (abs(t - it.t) > EPS) (omega - it.omega) / (t - it.t) else Vector3D() } ?: Vector3D()
val ground = pointOnEarth(orbital, nlv, orientation)
val wd = wdForLines(orbital, orientation, omega)
val ground = pointOnEarth(orbital, orientation)
val wd = wd(orbital, orientation, omega)
return AngularMotionPoint(
t = t,
orbitalPoint = orbital,
@@ -338,7 +323,7 @@ abstract class AbstractPuudCalculator(
)
}
protected fun calculateTauPoints(points: List<AngularMotionPoint>, nlv: Int): List<AngularMotionPoint> {
protected fun calculateTauPoints(points: List<AngularMotionPoint>): List<AngularMotionPoint> {
if (points.size < 3 || config.tau <= 0.0) return emptyList()
val step = calculationStep()
val baseTime = points.first().t
@@ -358,7 +343,7 @@ abstract class AbstractPuudCalculator(
lagrange(x, xs, kren),
lagrange(x, xs, risk),
)
val point = buildPoint(t, nlv, orientation, previous)
val point = buildPoint(t, orientation, previous)
result += point
previous = point
d -= step
libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AngularMotionTypes.kt
+2 -17
View File
@@ -5,7 +5,6 @@ import ballistics.types.OrbitalPoint
import ballistics.types.Orientation
import ballistics.utils.math.Quaternion3D
import ballistics.utils.math.Vector3D
import kotlin.math.PI
/**
* Параметры блока состояния КА, перенесенные из sBLS_KA проекта OrbitalMotion.
@@ -22,18 +21,6 @@ data class AngularMotionConfig(
val discreteTimes: Double = 0.125,
/** Длительность интервала успокоения, с. */
val tau: Double = 10.0,
/** Углы отклонения программных СК. */
val psk1Angle: Double = 19.5 * PI / 180.0,
val psk3Angle: Double = 0.0,
val psk2Angle: Double = -19.5 * PI / 180.0,
/** Углы отклонения линий визирования. */
val lv1Angle: Double = 0.01875,
val lv2Angle: Double = 0.0,
val lv3Angle: Double = -0.01875,
val lv4Angle: Double = 0.0,
val lv5Angle: Double = 0.01875,
val lv6Angle: Double = 0.0,
val lv7Angle: Double = -0.01875,
/** Расстояние от центра ОЭП до центра фокальной плоскости телескопа, мм. */
val dxOep: Double = 37.5,
/** Длина линейки ОЭП, мм. */
@@ -53,8 +40,6 @@ enum class AngularMotionMode {
data class SurveyId(
/** Признаки включения ОЭП: 1..4. */
val oep: List<Boolean> = listOf(false, false, false, false),
/** Номер линии визирования: 1..7. */
val nlv: Int = 4,
/** Примерное интегральное время начала наблюдения, с. */
val t: Double = 0.0,
/** Координаты точки прицеливания: широта/долгота в радианах, высота в метрах. */
@@ -89,8 +74,8 @@ data class AngularMotionPoint(
val omega: Vector3D = Vector3D(),
val eps: Vector3D = Vector3D(),
val quaternion: Quaternion3D = Quaternion3D(1.0, 0.0, 0.0, 0.0),
val wd: List<Vector3D> = List(7) { Vector3D() },
val sdi: List<Double> = List(7) { 0.0 },
val wd: Vector3D = Vector3D(),
val sdi: Double = 0.0,
)
/** Полный результат расчета режима углового движения. */
libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/AzimuthPUUD.kt
+48 -60
View File
@@ -1,7 +1,6 @@
package space.nstart.pcp.angularmotion
import ballistics.orbitalPoints.timeStepper.AbstractStepper
import ballistics.types.BLHPoint
import ballistics.types.EarthType
import ballistics.types.Orientation
import ballistics.utils.math.Matrix3D
@@ -32,41 +31,35 @@ open class AzimuthPUUD(
throw AngularMotionCalculationException("Некорректное значение СДИ")
}
var tn = calcTn(id)
var workId = id
var duration = id.duration
var oeps = buildOeps(id, tn)
// Логика специальных ЛВ 2/6 из AzimuthAISTPUUD: расширяем длительность на
// время прохода между линейками ОЭП и сдвигаем t_on/t_off парных ОЭП.
if (id.nlv == 2 || id.nlv == 6) {
val rvo = roundDownByDay(config.dxOep / id.sdi.first(), config.discreteTimes).coerceAtLeast(0.0)
duration += rvo * 2.0
tn -= rvo
oeps = shiftedPairOeps(id, tn, rvo)
workId = id.copy(t = tn)
}
val points = calculateAzimuth(workId, tn, duration)
val tn = calcTn(id)
val points = calculateAzimuth(id, tn, id.duration)
return AngularMotionResult(
mode = AngularMotionMode.AZIMUTH,
startTime = tn,
points = points,
tauPoints = calculateTauPoints(points, id.nlv),
oeps = oeps,
tauPoints = calculateTauPoints(points),
oeps = buildOeps(id, tn),
)
}
protected open fun calculateAzimuth(id: SurveyId, tn: Double, duration: Double): List<AngularMotionPoint> {
val step = calculationStep()
val routeNormalGsk = routeNormalInGreenwich(id.b, id.l, id.h, id.azimuth)
val routeDirectionGsk = routeDirectionInGreenwich(id.b, id.l, id.azimuth)
val firstTargetGsk = astro.earth.blh2xyz(id.b, id.l, id.h)
var currentSdi = sdiAt(id.sdi, 0.0)
val initialSdi = sdiAt(id.sdi, 0.0)
val firstPoint = pointAt(tn)
val firstAbs = astro.grinvToASK(firstPoint)
val targetAbs = astro.grinvToASK(firstTargetGsk, tn)
val initialLiv = initialVisirQuaternion(firstAbs.r, targetAbs, routeNormalGsk, tn, currentSdi < 0.0)
val initialLiv = initialVisirQuaternion(
rAbs = firstAbs.r,
targetAbs = targetAbs,
routeNormalGsk = routeNormalGsk,
routeDirectionGsk = routeDirectionGsk,
time = tn,
reverse = initialSdi < 0.0,
)
var liv = initialLiv
val result = mutableListOf<AngularMotionPoint>()
@@ -75,10 +68,9 @@ open class AzimuthPUUD(
var previous: AngularMotionPoint? = null
while (elapsed <= duration + EPS) {
currentSdi = sdiAt(id.sdi, elapsed)
val orbital = pointAt(t)
val orientation = orientationFromVisirQuaternion(orbital, id.nlv, liv)
val point = buildIntegratedPoint(t, id.nlv, orientation, liv, previous, sickle)
val orientation = orientationFromVisirQuaternion(orbital, liv)
val point = buildIntegratedPoint(t, orientation, liv, previous, sickle)
result += point
previous = point
@@ -87,7 +79,8 @@ open class AzimuthPUUD(
val p = pointAt(time)
val ask = astro.grinvToASK(p)
val di = slantRangeFromQuaternion(q, ask.r)
ownCornerSpeed(time, q, di, ask.r, ask.v, currentSdi)
val sdiForTime = sdiAt(id.sdi, time - tn)
ownCornerSpeed(time, q, di, ask.r, ask.v, sdiForTime)
}
}
elapsed += step
@@ -120,7 +113,6 @@ open class AzimuthPUUD(
protected fun buildIntegratedPoint(
t: Double,
nlv: Int,
orientation: Orientation,
liv: Quaternion3D,
previous: AngularMotionPoint?,
@@ -129,8 +121,8 @@ open class AzimuthPUUD(
val orbital = pointAt(t)
val omega = previous?.let { omegaFromTwoQuat(it.quaternion, liv, t - it.t) } ?: Vector3D()
val eps = previous?.let { if (abs(t - it.t) > EPS) (omega - it.omega) / (t - it.t) else Vector3D() } ?: Vector3D()
val ground = pointOnEarth(orbital, nlv, orientation)
val wd = wdForLines(orbital, orientation, omega)
val ground = pointOnEarth(orbital, orientation)
val wd = wd(orbital, orientation, omega)
return AngularMotionPoint(
t = t,
orbitalPoint = orbital,
@@ -148,25 +140,36 @@ open class AzimuthPUUD(
rAbs: Vector3D,
targetAbs: Vector3D,
routeNormalGsk: Vector3D,
routeDirectionGsk: Vector3D,
time: Double,
reverse: Boolean,
): Quaternion3D {
var normalAbs = astro.grinvToASK(routeNormalGsk, time).normSafe()
if (reverse) normalAbs = normalAbs * -1.0
val di = targetAbs - rAbs
val eY = normalAbs.rem(targetAbs.normSafe()).normSafe()
val eZ = eY.rem(di.normSafe()).normSafe()
var m = Matrix3D(
di.normSafe(),
eZ.rem(di.normSafe()).normSafe(),
eZ,
)
if (sickle) {
m = rotationX(PI) * Matrix3D(
di.normSafe(),
eZ,
di.normSafe().rem(eZ).normSafe(),
val line = (targetAbs - rAbs).normSafe()
val routeNormalAbs = astro.grinvToASK(routeNormalGsk, time).normSafe()
val routeDirectionAbs = astro.grinvToASK(routeDirectionGsk, time).normSafe()
// Ось движения изображения в визирной СК должна быть связана с касательной
// центральной линии маршрута, а не с хордой двух земных радиус-векторов.
// Проекция азимутальной касательной в фокальную плоскость соответствует
// критериям Wz/D = 0 и Wx/D = const: поперечная составляющая обнуляется,
// а продольная ось направлена по маршруту.
var routeAxis = (routeDirectionAbs - line * (routeDirectionAbs * line)).normSafe()
if (routeAxis.module() < EPS) {
routeAxis = routeNormalAbs.rem(line).normSafe()
}
if (reverse) {
routeAxis = routeAxis * -1.0
}
val crossAxis = line.rem(routeAxis).normSafe()
val m = if (sickle) {
rotationX(PI) * Matrix3D(
line,
crossAxis,
line.rem(crossAxis).normSafe(),
)
} else {
Matrix3D(line, routeAxis, crossAxis)
}
return quaternionFromMatrix(m).inverse().normalized()
}
@@ -182,29 +185,14 @@ open class AzimuthPUUD(
return lineAbs * root
}
protected fun orientationFromVisirQuaternion(orbital: ballistics.types.OrbitalPoint, nlv: Int, liv: Quaternion3D): Orientation {
protected fun orientationFromVisirQuaternion(orbital: ballistics.types.OrbitalPoint, liv: Quaternion3D): Orientation {
val ask = astro.grinvToASK(orbital)
val ob = orbBookToOrbMatrix()
val absOb = absToOrbBookMatrix(ask.r, ask.v)
val absToOrbit = ob * absOb
val oepToConnected = rotationZ(-lineAngle(nlv)).transpose()
val lConToVisir = Quaternion3D(0.0, sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0)
val connectedToAbs = (liv * lConToVisir.inverse() * quaternionFromMatrix(oepToConnected).inverse()).matrix().transpose()
val connectedToAbs = (liv * lConToVisir.inverse()).matrix().transpose()
val orbitToConnected = connectedToAbs * absToOrbit.transpose()
return anglesFromOrbToCon(orbitToConnected)
}
private fun shiftedPairOeps(id: SurveyId, tn: Double, rvo: Double): List<OepResult> {
val base = MutableList(4) { index ->
if (id.oep.getOrNull(index) == true) OepResult(true, tn, tn + id.duration) else OepResult(false)
}
if (id.nlv == 2) {
base[0] = OepResult(id.oep.getOrNull(0) == true, tn + rvo * 2.0, tn + rvo * 2.0 + id.duration)
base[1] = OepResult(id.oep.getOrNull(1) == true, tn, tn + id.duration)
} else if (id.nlv == 6) {
base[2] = OepResult(id.oep.getOrNull(2) == true, tn + rvo * 2.0, tn + rvo * 2.0 + id.duration)
base[3] = OepResult(id.oep.getOrNull(3) == true, tn, tn + id.duration)
}
return base
}
}
libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/ConstOrientPUUD.kt
+10 -12
View File
@@ -11,7 +11,7 @@ import ballistics.utils.math.Vector3D
*
* Базовая логика повторяет исходный класс: уточняется время начала по упреждающему
* углу, затем с дискретом stepPuud рассчитываются углы ориентации, точка пересечения
* ЛВ с Землей, кватернион, угловая скорость, W/D и СДИ по всем семи ЛВ.
* единственной ЦЛВ с Землей, кватернион, угловая скорость, W/D и СДИ.
*/
class ConstOrientPUUD(
stepper: AbstractStepper,
@@ -27,7 +27,7 @@ class ConstOrientPUUD(
mode = AngularMotionMode.CONST_ORIENT,
startTime = tn,
points = points,
tauPoints = calculateTauPoints(points, id.nlv),
tauPoints = calculateTauPoints(points),
oeps = buildOeps(id, tn),
)
}
@@ -44,21 +44,20 @@ class ConstOrientPUUD(
// пересекаем эту фиксированную ЦЛВ с Землей. Важно, чтобы первая точка маршрута
// тоже была получена через такое пересечение: иначе первый шаг сшивает заданную
// исходную ЦЛМ с расчетной трассой постоянной ЦЛВ и дает искусственный скачок рысканья.
val fixedClvOrientation = orientOnPoint(id.nlv, tn, id.b, id.l, id.h)
val fixedClvOrientation = orientOnPoint(tn, id.b, id.l, id.h)
while (elapsed <= duration + 2.0 * step + EPS) {
val currentGround = pointOnFixedClv(t, id.nlv, fixedClvOrientation)
val currentGround = pointOnFixedClv(t, fixedClvOrientation)
?: BLHPoint(id.b, id.l, id.h)
val routeDirectionGsk = fixedClvGroundDirection(t, step, id.nlv, fixedClvOrientation, currentGround)
val routeDirectionGsk = fixedClvGroundDirection(t, step, fixedClvOrientation, currentGround)
val orientation = orientOnPoint(
id.nlv,
t,
currentGround.lat,
currentGround.long,
currentGround.h,
routeDirectionGsk,
)
val point = buildPoint(t, id.nlv, orientation, previous)
val point = buildPoint(t, orientation, previous)
result += point
previous = point
@@ -69,18 +68,17 @@ class ConstOrientPUUD(
return if (result.size > 2) result.dropLast(2) else result
}
private fun pointOnFixedClv(t: Double, nlv: Int, fixedClvOrientation: Orientation): BLHPoint? =
pointOnEarth(pointAt(t), nlv, fixedClvOrientation)
private fun pointOnFixedClv(t: Double, fixedClvOrientation: Orientation): BLHPoint? =
pointOnEarth(pointAt(t), fixedClvOrientation)
private fun fixedClvGroundDirection(
t: Double,
step: Double,
nlv: Int,
fixedClvOrientation: Orientation,
currentGround: BLHPoint,
): Vector3D {
val previousGround = pointOnFixedClv(t - step, nlv, fixedClvOrientation)
val nextGround = pointOnFixedClv(t + step, nlv, fixedClvOrientation)
val previousGround = pointOnFixedClv(t - step, fixedClvOrientation)
val nextGround = pointOnFixedClv(t + step, fixedClvOrientation)
return when {
previousGround != null && nextGround != null -> groundDirection(previousGround, nextGround)
libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/InternalMath.kt
+4 -5
View File
@@ -143,12 +143,11 @@ internal fun conToOpticMatrix(): Matrix3D = Matrix3D(
Vector3D(0.0, 0.0, -1.0),
)
internal fun lineOfSightVectorInConnected(lineAngle: Double): Vector3D {
// Центральная линия визирования направлена из центра КА к Земле.
internal fun lineOfSightVectorInConnected(): Vector3D {
// У спутника используется единственная центральная линия визирования.
// В программной/связанной СК при нулевых углах +OY направлена от Земли,
// поэтому ЦЛВ соответствует вектору (0, -1, 0). Отклонения боковых ЛВ
// задаются поворотом вокруг OZ связанной СК, как в OrbitalMotion::conToCLV.
return Vector3D(sin(lineAngle), -cos(lineAngle), 0.0).normSafe()
// поэтому ЦЛВ, направленная из центра КА к Земле, задается вектором (0, -1, 0).
return Vector3D(0.0, -1.0, 0.0)
}
internal fun quaternionFromMatrix(m: Matrix3D): Quaternion3D = Quaternion3D().also { it.fromMatrixStanley(m) }.normalized()
libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/SmoothSDIPUUD.kt
+1 -1
View File
@@ -29,7 +29,7 @@ class SmoothSDIPUUD(
mode = AngularMotionMode.SMOOTH_SDI,
startTime = tn,
points = points,
tauPoints = calculateTauPoints(points, id.nlv),
tauPoints = calculateTauPoints(points),
oeps = buildOeps(id, tn),
)
}
libs/angular-motion-lib/src/test/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorSmokeTest.kt
+75 -4
View File
@@ -10,6 +10,8 @@ import ballistics.utils.fromDateTime
import ballistics.utils.math.Vector3D
import ballistics.utils.toDateTime
import java.time.LocalDateTime
import java.time.format.DateTimeFormatter
import java.util.Locale
import kotlin.math.PI
import kotlin.math.cos
import kotlin.math.sin
@@ -18,11 +20,12 @@ import kotlin.test.assertEquals
import kotlin.test.assertTrue
class AngularMotionCalculatorSmokeTest {
private val outputTimeFormatter = DateTimeFormatter.ofPattern("yyyy-MM-dd'T'HH:mm:ss.SSS")
@Test
fun `const orientation returns points`() {
val result = ConstOrientPUUD(CircularStepper()).calculate(
SurveyId(
nlv = 4,
t = 1000.0,
b = 0.1,
l = 0.2,
@@ -44,7 +47,6 @@ class AngularMotionCalculatorSmokeTest {
val result = calculator.calculate(
SurveyId(
nlv = 4,
t = 1000.0,
b = 0.1,
l = 0.2,
@@ -83,7 +85,7 @@ class AngularMotionCalculatorSmokeTest {
@Test
fun chek(){
fun chekConstOrient(){
val ballistics = Ballistics()
val t = LocalDateTime.of(2023, 4, 12, 17, 41, 18, 0)
val ic = InitialConditions(
@@ -119,7 +121,6 @@ class AngularMotionCalculatorSmokeTest {
val calculator = ConstOrientPUUD(stepper, EarthType.PZ90d02)
val id = SurveyId(
oep = listOf(false, false, false, true),
nlv = 5,
t = fromDateTime(LocalDateTime.of(2023, 4, 16, 3, 48, 22, 0)),
b = 49.25824 * PI / 180,
l = 153.65914 * PI / 180,
@@ -140,4 +141,74 @@ class AngularMotionCalculatorSmokeTest {
println("${toDateTime( v.t)} ${v.orientation.tang * 180 / PI} ${v.orientation.kren * 180 / PI } ${v.orientation.risk * 180 / PI}")
}
}
@Test
fun chekAzimuth(){
val ballistics = Ballistics()
val t = LocalDateTime.of(2023, 4, 12, 17, 41, 18, 0)
val ic = InitialConditions(
OrbitalPoint(
fromDateTime(t),
1,
Vector3D(
-3101926.630,
6018678.8,
0.0,
),
Vector3D(
1287.651,
663.634,
7612.951,
),
),
0.005,
160.0
)
val result = ballistics.calculateOrbPoints(
ic,
ic.point.t,
ic.point.t + 86400 * 10
)
assertEquals(result, BallisticsError.OK)
if (result != BallisticsError.OK)
return
val stepper = ballistics.getStepper()
assert(stepper != null)
if (stepper == null)
return
val calculator = AzimuthPUUD(stepper, EarthType.PZ90d02)
val id = SurveyId(
oep = listOf(false, false, false, true),
t = fromDateTime(LocalDateTime.of(2023, 4, 16, 3, 48, 22, 0)),
b = 49.25824 * PI / 180,
l = 153.65914 * PI / 180,
h = 0.0,
duration = 69.0,
sdi = listOf(30.0),
azimuth = 25.567 * PI / 180,
uprAngle = 6.0 * PI / 180,
pointInCenter = false
)
val rc = calculator.calculate(id)
println(rc.mode)
println(rc.points.size)
println(rc.startTime)
for (v in rc.points) {
println(
String.format(
Locale.US,
"%s %.3f %.3f %.3f , %s %s",
toDateTime(v.t).format(outputTimeFormatter),
v.orientation.tang * 180 / PI,
v.orientation.kren * 180 / PI,
v.orientation.risk * 180 / PI,
v.groundPoint?.let { String.format(Locale.US, "%.3f", it.lat * 180 / PI) },
v.groundPoint?.let { String.format(Locale.US, "%.3f", it.long * 180 / PI) },
)
)
}
}
}