#
# ISC License
#
# Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
#
# Permission to use, copy, modify, and/or distribute this software for any
# purpose with or without fee is hereby granted, provided that the above
# copyright notice and this permission notice appear in all copies.
#
# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
#
# Copy of the unit test for sunSafe Point adapted to any heading
# Module Name: opNavPoint
# Author: Thibaud Teil
# Creation Date: August 20, 2019
#
import inspect
import os
import numpy as np
import pytest
filename = inspect.getframeinfo(inspect.currentframe()).filename
path = os.path.dirname(os.path.abspath(filename))
# Import all of the modules that we are going to be called in this simulation
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import unitTestSupport # general support file with common unit test functions
from Basilisk.fswAlgorithms import opNavPoint # import the module that is to be tested
from Basilisk.architecture import messaging
from Basilisk.utilities import macros as mc
from Basilisk.utilities import simHelpers
from Basilisk.utilities import RigidBodyKinematics as rbk
# uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed
# @pytest.mark.skipif(conditionstring)
# uncomment this line if this test has an expected failure, adjust message as needed
#@pytest.mark.xfail(conditionstring)
# provide a unique test method name, starting with test_
# The following 'parametrize' function decorator provides the parameters and expected results for each
# of the multiple test runs for this test.
[docs]
@pytest.mark.parametrize("case", [
(1) # target is visible, vectors are not aligned
,(2) # target is not visible, vectors are not aligned
,(3) # target is visible, vectors are aligned
,(4) # target is not visible, search
])
def test_module(show_plots, case):
"""Module Unit Test"""
# each test method requires a single assert method to be called
[testResults, testMessage] = opNavPointTestFunction(show_plots, case)
assert testResults < 1, testMessage
def opNavPointTestFunction(show_plots, case):
testFailCount = 0 # zero unit test result counter
testMessages = [] # create empty array to store test log messages
unitTaskName = "unitTask" # arbitrary name (don't change)
unitProcessName = "TestProcess" # arbitrary name (don't change)
# Create a sim module as an empty container
unitTestSim = SimulationBaseClass.SimBaseClass()
# Create test thread
testProcessRate = mc.sec2nano(0.5) # update process rate update time
testProc = unitTestSim.CreateNewProcess(unitProcessName)
testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate))
# Construct algorithm and associated C++ container
module = opNavPoint.opNavPoint()
module.ModelTag = "opNavPoint"
# Add test module to runtime call list
unitTestSim.AddModelToTask(unitTaskName, module)
# Initialize the test module configuration data
camera_Z = [0.,0.,1.]
module.alignAxis_C = camera_Z
module.minUnitMag = 0.01
module.smallAngle = 0.01*mc.D2R
module.timeOut = 100
# Create input messages
#
planet_B = [1.,1.,0.]
inputOpNavData = messaging.OpNavMsgPayload() # Create a structure for the input message
inputOpNavData.r_BN_C = planet_B
inputOpNavData.valid = 1
if (case == 2): #No valid measurement
inputOpNavData.valid = 0
if (case == 3): #No valid measurement
inputOpNavData.r_BN_C = [0.,0.,-1.]
if (case == 4): #No valid measurement
inputOpNavData.valid = 0
opnavInMsg = messaging.OpNavMsg().write(inputOpNavData)
inputIMUData = messaging.NavAttMsgPayload() # Create a structure for the input message
omega_BN_B = np.array([0.01, 0.50, -0.2])
inputIMUData.omega_BN_B = omega_BN_B
imuInMsg = messaging.NavAttMsg().write(inputIMUData)
omega_RN_B_Search = np.array([0.0, 0.0, 0.1])
if (case ==2 or case==4):
module.omega_RN_B = omega_RN_B_Search
cam = messaging.CameraConfigMsgPayload() # Create a structure for the input message
cam.sigma_CB = [0.,0.,0]
camInMsg = messaging.CameraConfigMsg().write(cam)
# Setup logging on the test module output message so that we get all the writes to it
dataLog = module.attGuidanceOutMsg.recorder()
unitTestSim.AddModelToTask(unitTaskName, dataLog)
# connect messages
module.opnavDataInMsg.subscribeTo(opnavInMsg)
module.imuInMsg.subscribeTo(imuInMsg)
module.cameraConfigInMsg.subscribeTo(camInMsg)
# Need to call the self-init and cross-init methods
unitTestSim.InitializeSimulation()
unitTestSim.ConfigureStopTime(mc.sec2nano(1.)) # seconds to stop simulation
unitTestSim.ExecuteSimulation()
# This pulls the actual data log from the simulation run.
# Note that range(3) will provide [0, 1, 2] Those are the elements you get from the vector (all of them)
#
# check sigma_BR
#
# set the filtered output truth states
eHat = np.cross(-np.array(planet_B), np.array(camera_Z))
eHat = eHat / np.linalg.norm(eHat)
Phi = np.arccos(np.dot(-np.array(planet_B)/np.linalg.norm(-np.array(planet_B)),np.array(camera_Z)))
sigmaTrue = eHat * np.tan(Phi/4.0)
trueVector = [
sigmaTrue.tolist(),
sigmaTrue.tolist(),
sigmaTrue.tolist()
]
if (case == 2 or case == 3 or case == 4):
trueVector = [
[0, 0, 0],
[0, 0, 0],
[0, 0, 0]
]
# compare the module results to the truth values
accuracy = 1e-12
simHelpers.writeTeXSnippet("toleranceValue", str(accuracy), path)
for i in range(0,len(trueVector)):
# check a vector values
if not unitTestSupport.isArrayEqual(dataLog.sigma_BR[i],trueVector[i],3,accuracy):
testFailCount += 1
testMessages.append("FAILED: " + module.ModelTag + " Module failed sigma_BR unit test at t=" +
str(dataLog.times()[i] * mc.NANO2SEC) +
"sec\n")
#
# check omega_BR_B
#
# set the filtered output truth states
trueVector = [
omega_BN_B.tolist(),
omega_BN_B.tolist(),
omega_BN_B.tolist()
]
if (case == 2 or case==4):
trueVector = [
(omega_BN_B - omega_RN_B_Search).tolist(),
(omega_BN_B - omega_RN_B_Search).tolist(),
(omega_BN_B - omega_RN_B_Search).tolist()
]
# compare the module results to the truth values
for i in range(0,len(trueVector)):
# check a vector values
if not unitTestSupport.isArrayEqual(dataLog.omega_BR_B[i],trueVector[i],3,accuracy):
testFailCount += 1
testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_BR_B unit test at t=" +
str(dataLog.times()[i] * mc.NANO2SEC) +
"sec\n")
#
# check omega_RN_B
#
# set the filtered output truth states
trueVector = [
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0]
]
if (case == 2 or case == 4):
trueVector = [
omega_RN_B_Search,
omega_RN_B_Search,
omega_RN_B_Search
]
# compare the module results to the truth values
for i in range(0,len(trueVector)):
# check a vector values
if not unitTestSupport.isArrayEqual(dataLog.omega_RN_B[i],trueVector[i],3,accuracy):
testFailCount += 1
testMessages.append("FAILED: " + module.ModelTag + " Module failed omega_RN_B unit test at t=" +
str(dataLog.times()[i] * mc.NANO2SEC) +
"sec\n")
#
# check domega_RN_B
#
# set the filtered output truth states
trueVector = [
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0]
]
# compare the module results to the truth values
for i in range(0,len(trueVector)):
# check a vector values
if not unitTestSupport.isArrayEqual(dataLog.domega_RN_B[i],trueVector[i],3,accuracy):
testFailCount += 1
testMessages.append("FAILED: " + module.ModelTag + " Module failed domega_RN_B unit test at t=" +
str(dataLog.times()[i] * mc.NANO2SEC) +
"sec\n")
# print out success message if no error were found
snippentName = "passFail" + str(case)
if testFailCount == 0:
colorText = 'ForestGreen'
print("PASSED: " + module.ModelTag)
passedText = r'\textcolor{' + colorText + '}{' + "PASSED" + '}'
else:
colorText = 'Red'
print("FAILED: " + module.ModelTag)
passedText = r'\textcolor{' + colorText + '}{' + "Failed" + '}'
simHelpers.writeTeXSnippet(snippentName, passedText, path)
# each test method requires a single assert method to be called
# this check below just makes sure no sub-test failures were found
return [testFailCount, ''.join(testMessages)]
[docs]
def runReferenceBoresightCheck(sigma_CB, sigma_BN, r_BN_N, alignAxis_C, dropMeasurement=False,
sigma_BN_drop=None):
"""Run ``opNavPoint`` and return the boresight-to-target angles of the reference attitude.
The reference attitude implied by the output is :math:`[RN] = [BR]^T [BN]`, with
:math:`[BR]` built from ``sigma_BR``. In that attitude the camera axis ``alignAxis_C``
should point along the target heading :math:`-\\boldsymbol{r}_{B/N}`.
Args:
sigma_CB (list): camera MRP attitude relative to the body frame
sigma_BN (list): body MRP attitude relative to the inertial frame
r_BN_N (list): target-to-spacecraft position vector in inertial components
alignAxis_C (list): camera axis to point at the target, in camera components
dropMeasurement (bool): if True, the update after the measurement at 0 s receives an invalid
measurement so the stored inertial heading is used instead
sigma_BN_drop (list): body MRP attitude relative to the inertial frame used for the update after
the measurement at 0 s. If None, ``sigma_BN`` is kept. A different attitude makes the
stored heading depend on the inertial frame storage, since a heading stored in camera
components would no longer point at the target after the rotation.
Each angle is evaluated with the body attitude in effect at the corresponding update.
Returns:
numpy.ndarray: angle between the reference camera axis and the target heading for each update [rad]
"""
unitTaskName = "unitTask"
unitProcessName = "TestProcess"
unitTestSim = SimulationBaseClass.SimBaseClass()
testProcessRate = mc.sec2nano(1.0) # [ns]
testProc = unitTestSim.CreateNewProcess(unitProcessName)
testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate))
module = opNavPoint.opNavPoint()
module.ModelTag = "opNavPoint"
module.alignAxis_C = list(alignAxis_C)
module.smallAngle = 1e-6 # [rad]
module.timeOut = 100.0 # [s]
module.omega_RN_B = [0.0, 0.0, 0.0] # [rad/s]
module.opNavAxisSpinRate = 0.0 # [rad/s]
unitTestSim.AddModelToTask(unitTaskName, module)
dcm_BN = rbk.MRP2C(sigma_BN)
dcm_CB = rbk.MRP2C(sigma_CB)
r_BN_N = np.array(r_BN_N)
opNavData = messaging.OpNavMsgPayload()
opNavData.valid = 1
opNavData.r_BN_C = (dcm_CB @ dcm_BN @ r_BN_N).tolist()
opNavInMsg = messaging.OpNavMsg().write(opNavData)
imuData = messaging.NavAttMsgPayload()
imuData.sigma_BN = list(sigma_BN)
imuInMsg = messaging.NavAttMsg().write(imuData)
camData = messaging.CameraConfigMsgPayload()
camData.sigma_CB = list(sigma_CB)
camInMsg = messaging.CameraConfigMsg().write(camData)
module.opnavDataInMsg.subscribeTo(opNavInMsg)
module.imuInMsg.subscribeTo(imuInMsg)
module.cameraConfigInMsg.subscribeTo(camInMsg)
dataLog = module.attGuidanceOutMsg.recorder()
unitTestSim.AddModelToTask(unitTaskName, dataLog)
if sigma_BN_drop is None:
sigma_BN_drop = sigma_BN
dcm_BN_drop = rbk.MRP2C(sigma_BN_drop)
unitTestSim.InitializeSimulation()
unitTestSim.ConfigureStopTime(0)
unitTestSim.ExecuteSimulation()
imuData.sigma_BN = list(sigma_BN_drop)
imuInMsg.write(imuData)
if dropMeasurement:
opNavData.valid = 0
opNavData.r_BN_C = [0.0, 0.0, 0.0] # [m]
else:
opNavData.r_BN_C = (dcm_CB @ dcm_BN_drop @ r_BN_N).tolist() # [m]
opNavInMsg.write(opNavData, testProcessRate)
unitTestSim.ConfigureStopTime(testProcessRate)
unitTestSim.ExecuteSimulation()
heading_N = -r_BN_N / np.linalg.norm(r_BN_N)
angles = []
for sigma_BR, dcm_BN_update in zip(dataLog.sigma_BR, [dcm_BN, dcm_BN_drop]):
dcm_RN = rbk.MRP2C(sigma_BR).T @ dcm_BN_update
cameraAxis_N = (dcm_CB @ dcm_RN).T @ np.array(alignAxis_C)
cameraAxis_N = cameraAxis_N / np.linalg.norm(cameraAxis_N)
angles.append(np.arctan2(np.linalg.norm(np.cross(cameraAxis_N, heading_N)),
np.dot(cameraAxis_N, heading_N)))
return np.array(angles)
[docs]
@pytest.mark.parametrize("sigma_CB", [
[0.1, 0.0, 0.0],
[0.0, 0.2, 0.1],
[0.3, -0.2, 0.1],
[-0.5, 0.4, 0.6],
])
@pytest.mark.parametrize("sigma_BN", [
[0.0, 0.0, 0.0],
[0.2, -0.1, 0.3],
[-0.35, 0.25, 0.05],
])
@pytest.mark.parametrize("dropMeasurement", [False, True])
def test_nonZeroCameraMounting(sigma_CB, sigma_BN, dropMeasurement):
"""Verify the reference attitude points the camera axis at the target for a non-zero ``sigma_CB``.
The principal rotation axis is computed in camera components and must be mapped to body
components before it is written to ``sigma_BR``. The check is performed both with a fresh
measurement and with the stored inertial heading after a dropped measurement. In the dropped
measurement case the body attitude is rotated to ``sigma_BN_drop`` between the measurement and
the dropped update, so a heading that is not stored in inertial components, or that is not
mapped back with the current ``[CN]``, would no longer point the camera axis at the target.
"""
r_BN_N = [7000e3, -2000e3, 1000e3] # [m]
alignAxis_C = [0.0, 0.0, 1.0]
# rotated attitude for the dropped update, different from every sigma_BN case
sigma_BN_drop = [0.1, 0.4, -0.2] if dropMeasurement else None
angles = runReferenceBoresightCheck(sigma_CB, sigma_BN, r_BN_N, alignAxis_C, dropMeasurement,
sigma_BN_drop)
np.testing.assert_allclose(angles, 0.0, atol=1e-10, # [rad]
err_msg="reference camera axis does not point at the target")
[docs]
@pytest.mark.parametrize("alignAxis_C", [
[0.0, 0.0, 1.0],
[1.0, 0.0, 0.0],
])
@pytest.mark.parametrize("sigma_CB", [
[0.0, 0.0, 0.0],
[0.3, -0.2, 0.1],
])
def test_antiparallelHeading(alignAxis_C, sigma_CB):
"""Verify the 180 degree fallback axis for a target directly behind the camera axis.
The fallback eigen axis is computed in camera components at reset and must be mapped to body
components like the regular axis. The ``[1, 0, 0]`` camera axis exercises the secondary cross
product used when ``alignAxis_C`` is parallel to the first camera axis.
"""
sigma_BN = [0.2, -0.1, 0.3]
dcm_NB = rbk.MRP2C(sigma_BN).T
dcm_BC = rbk.MRP2C(sigma_CB).T
# place the target along -alignAxis_C so that the heading -r_BN is opposite the camera axis
r_BN_N = 7000e3 * (dcm_NB @ dcm_BC @ np.array(alignAxis_C)) # [m]
angles = runReferenceBoresightCheck(sigma_CB, sigma_BN, r_BN_N, alignAxis_C)
# acos() near 180 degrees amplifies roundoff in the heading, so a looser tolerance is used here
np.testing.assert_allclose(angles, 0.0, atol=1e-7, # [rad]
err_msg="reference camera axis does not point at the target")
[docs]
@pytest.mark.parametrize("firstMeasurementTime", [0.0, 3.0]) # [s]
def test_savedHeadingTimeout(firstMeasurementTime):
"""Verify the saved heading is reused until ``timeOut`` and a new image is used as soon as it arrives.
Valid images are separated by invalid ones. The saved heading must keep producing the tracking error of
the last image for less than ``timeOut`` seconds after it, also when the image arrives at 0 s, and the
module must switch to the search rate once ``timeOut`` is reached. A valid image that arrives exactly
``timeOut`` seconds after the previous one must be used on that update instead of being ignored.
"""
timeStep = 1.0 # [s]
timeOut = 2.0 # [s]
omega_RN_B_Search = [0.0, 0.0, 0.1] # [rad/s]
omega_BN_B = [0.01, 0.50, -0.2] # [rad/s]
r_BN_C = [1.0, 1.0, 0.0] # [m]
unitTestSim = SimulationBaseClass.SimBaseClass()
testProc = unitTestSim.CreateNewProcess("TestProcess")
testProc.addTask(unitTestSim.CreateNewTask("unitTask", mc.sec2nano(timeStep)))
module = opNavPoint.opNavPoint()
module.ModelTag = "opNavPoint"
module.alignAxis_C = [0.0, 0.0, 1.0]
module.smallAngle = 0.01 * mc.D2R # [rad]
module.timeOut = timeOut
module.omega_RN_B = omega_RN_B_Search
unitTestSim.AddModelToTask("unitTask", module)
opNavData = messaging.OpNavMsgPayload()
opNavData.r_BN_C = r_BN_C
opNavInMsg = messaging.OpNavMsg().write(opNavData)
imuData = messaging.NavAttMsgPayload()
imuData.omega_BN_B = omega_BN_B
imuInMsg = messaging.NavAttMsg().write(imuData)
camInMsg = messaging.CameraConfigMsg().write(messaging.CameraConfigMsgPayload())
module.opnavDataInMsg.subscribeTo(opNavInMsg)
module.imuInMsg.subscribeTo(imuInMsg)
module.cameraConfigInMsg.subscribeTo(camInMsg)
dataLog = module.attGuidanceOutMsg.recorder()
unitTestSim.AddModelToTask("unitTask", dataLog)
unitTestSim.InitializeSimulation()
# valid images, defined by integer update index to avoid floating point time comparisons: the first one
# times out, the second one is followed by a third exactly timeOut later
firstStep = round(firstMeasurementTime / timeStep)
timeOutSteps = round(timeOut / timeStep)
validSteps = [firstStep,
firstStep + 2 * timeOutSteps,
firstStep + 3 * timeOutSteps]
steps = range(validSteps[-1] + 1)
for step in steps:
opNavData.valid = 1 if step in validSteps else 0
opNavInMsg.write(opNavData, step * mc.sec2nano(timeStep))
unitTestSim.ConfigureStopTime(step * mc.sec2nano(timeStep))
unitTestSim.ExecuteSimulation()
heading_C = -np.array(r_BN_C) / np.linalg.norm(r_BN_C)
eHat = np.cross(heading_C, [0.0, 0.0, 1.0])
eHat = eHat / np.linalg.norm(eHat)
sigmaTrack = eHat * np.tan(np.arccos(heading_C[2]) / 4.0)
for step, sigma_BR, omega_RN_B in zip(steps, dataLog.sigma_BR, dataLog.omega_RN_B):
t = step * timeStep # [s]
lastValidStep = max((v for v in validSteps if v <= step), default=None)
tracking = lastValidStep is not None and step - lastValidStep < timeOutSteps
sigmaTrue = sigmaTrack if tracking else np.zeros(3)
omegaTrue = np.zeros(3) if tracking else np.array(omega_RN_B_Search)
np.testing.assert_allclose(sigma_BR, sigmaTrue, atol=1e-12,
err_msg=f"sigma_BR mismatch at t = {t} s")
np.testing.assert_allclose(omega_RN_B, omegaTrue, atol=1e-12, # [rad/s]
err_msg=f"omega_RN_B mismatch at t = {t} s")
[docs]
def test_defaultTimeOut():
"""Verify valid images are used when ``timeOut`` is left at its default value of zero.
Valid and invalid images alternate, starting with a valid image at 0 s and including consecutive
valid and consecutive invalid images. With ``timeOut = 0`` each valid image is used once, on the
update where it arrives, and the saved heading is discarded on the next update without a valid
image, so it is never reused. Every valid update must therefore produce the tracking error of the
image with a zero ``omega_RN_B``, and every invalid update must produce a zero ``sigma_BR`` with the
search rate.
"""
timeStep = 1.0 # [s]
omega_RN_B_Search = [0.0, 0.0, 0.1] # [rad/s]
omega_BN_B = [0.01, 0.50, -0.2] # [rad/s]
r_BN_C = [1.0, 1.0, 0.0] # [m]
# image validity flag for each update, starting at 0 s
validFlags = [1, 0, 1, 1, 0, 0, 1, 0]
unitTestSim = SimulationBaseClass.SimBaseClass()
testProc = unitTestSim.CreateNewProcess("TestProcess")
testProc.addTask(unitTestSim.CreateNewTask("unitTask", mc.sec2nano(timeStep)))
module = opNavPoint.opNavPoint()
module.ModelTag = "opNavPoint"
module.alignAxis_C = [0.0, 0.0, 1.0]
module.smallAngle = 0.01 * mc.D2R # [rad]
module.omega_RN_B = omega_RN_B_Search
unitTestSim.AddModelToTask("unitTask", module)
opNavData = messaging.OpNavMsgPayload()
opNavData.r_BN_C = r_BN_C
opNavInMsg = messaging.OpNavMsg().write(opNavData)
imuData = messaging.NavAttMsgPayload()
imuData.omega_BN_B = omega_BN_B
imuInMsg = messaging.NavAttMsg().write(imuData)
camInMsg = messaging.CameraConfigMsg().write(messaging.CameraConfigMsgPayload())
module.opnavDataInMsg.subscribeTo(opNavInMsg)
module.imuInMsg.subscribeTo(imuInMsg)
module.cameraConfigInMsg.subscribeTo(camInMsg)
dataLog = module.attGuidanceOutMsg.recorder()
unitTestSim.AddModelToTask("unitTask", dataLog)
unitTestSim.InitializeSimulation()
for step, valid in enumerate(validFlags):
opNavData.valid = valid
opNavInMsg.write(opNavData, step * mc.sec2nano(timeStep))
unitTestSim.ConfigureStopTime(step * mc.sec2nano(timeStep))
unitTestSim.ExecuteSimulation()
heading_C = -np.array(r_BN_C) / np.linalg.norm(r_BN_C)
eHat = np.cross(heading_C, [0.0, 0.0, 1.0])
eHat = eHat / np.linalg.norm(eHat)
sigmaTrack = eHat * np.tan(np.arccos(heading_C[2]) / 4.0)
assert len(dataLog.sigma_BR) == len(validFlags)
for step, (valid, sigma_BR, omega_RN_B) in enumerate(zip(validFlags, dataLog.sigma_BR,
dataLog.omega_RN_B)):
t = step * timeStep # [s]
sigmaTrue = sigmaTrack if valid else np.zeros(3)
omegaTrue = np.zeros(3) if valid else np.array(omega_RN_B_Search)
np.testing.assert_allclose(sigma_BR, sigmaTrue, atol=1e-12,
err_msg=f"sigma_BR mismatch at t = {t} s")
np.testing.assert_allclose(omega_RN_B, omegaTrue, atol=1e-12, # [rad/s]
err_msg=f"omega_RN_B mismatch at t = {t} s")
[docs]
@pytest.mark.parametrize("timeOut", [0.0, 2.0]) # [s]
def test_retainedMeasurement(timeOut):
"""Consume each valid message once, retain its inertial heading, and expire it.
Write valid images at 0 s and 4 s, leaving the payload untouched between writes.
Rotate the body after the first image and require the reference camera axis to
keep pointing at the saved target until timeout. After expiry, search must
continue until the second image supplies a new target. With zero timeout,
only the updates receiving new images may track. Reset must allow a new
measurement at time zero again, and a delayed first read must still expire
relative to the message write time.
"""
module = opNavPoint.opNavPoint()
module.alignAxis_C = [0.0, 0.0, 1.0]
module.smallAngle = 1e-6 # [rad]
module.timeOut = timeOut
module.omega_RN_B = [0.0, 0.0, 0.1] # [rad/s]
r_BN_N = np.array([7000e3, -2000e3, 1000e3]) # [m]
opNavData = messaging.OpNavMsgPayload()
opNavData.valid = 1
opNavInMsg = messaging.OpNavMsg()
imuData = messaging.NavAttMsgPayload()
imuInMsg = messaging.NavAttMsg()
camData = messaging.CameraConfigMsgPayload()
camData.sigma_CB = [0.3, -0.2, 0.1]
camInMsg = messaging.CameraConfigMsg().write(camData)
dcm_CB = rbk.MRP2C(camData.sigma_CB)
module.opnavDataInMsg.subscribeTo(opNavInMsg)
module.imuInMsg.subscribeTo(imuInMsg)
module.cameraConfigInMsg.subscribeTo(camInMsg)
module.SelfInit()
module.Reset(0)
for step in range(8):
callTime = mc.sec2nano(float(step)) # [ns]
imuData.sigma_BN = [0.0, 0.0, 0.0] if step == 0 else [0.1, 0.4, -0.2]
imuInMsg.write(imuData, callTime)
dcm_BN = rbk.MRP2C(imuData.sigma_BN)
if step == 4:
r_BN_N = np.array([-2000e3, 3000e3, 7000e3]) # [m]
if step in (0, 4):
opNavData.r_BN_C = (dcm_CB @ dcm_BN @ r_BN_N).tolist()
opNavInMsg.write(opNavData, callTime)
module.UpdateState(callTime)
output = module.attGuidanceOutMsg.read()
tracking = step in (0, 4) or step % 4 < timeOut
if tracking:
dcm_RN = rbk.MRP2C(output.sigma_BR).T @ dcm_BN
cameraAxis_N = (dcm_CB @ dcm_RN).T @ np.array(module.alignAxis_C)
np.testing.assert_allclose(cameraAxis_N, -r_BN_N / np.linalg.norm(r_BN_N), rtol=0, atol=1e-12)
else:
np.testing.assert_allclose(output.sigma_BR, np.zeros(3), atol=1e-12)
omegaTrue = np.zeros(3) if tracking else np.array(module.omega_RN_B)
np.testing.assert_allclose(output.omega_RN_B, omegaTrue, atol=1e-12) # [rad/s]
module.Reset(0)
opNavInMsg.write(opNavData, 0)
imuInMsg.write(imuData, 0)
module.UpdateState(mc.sec2nano(0.5)) # [ns]
np.testing.assert_allclose(module.attGuidanceOutMsg.read().omega_RN_B, np.zeros(3), atol=1e-12) # [rad/s]
module.UpdateState(mc.sec2nano(2.0)) # [ns]
np.testing.assert_allclose(module.attGuidanceOutMsg.read().omega_RN_B, module.omega_RN_B, atol=1e-12) # [rad/s]
#
# This statement below ensures that the unitTestScript can be run as a
# stand-along python script
#
if __name__ == "__main__":
opNavPointTestFunction(False, 1)