CalcRangeRangeRate.hpp

/******************************************************************************
* Copyright (c) ATTX LLC 2024. All Rights Reserved.
*
* This software and associated documentation (the "Software") are the 
* proprietary and confidential information of ATTX, LLC. The Software is 
* furnished under a license agreement between ATTX and the user organization 
* and may be used or copied only in accordance with the terms of the agreement.
* Refer to 'license/attx_license.adoc' for standard license terms.
*
* EXPORT CONTROL NOTICE: THIS SOFTWARE MAY INCLUDE CONTENT CONTROLLED UNDER THE
* INTERNATIONAL TRAFFIC IN ARMS REGULATIONS (ITAR) OR THE EXPORT ADMINISTRATION 
* REGULATIONS (EAR99). No part of the Software may be used, reproduced, or 
* transmitted in any form or by any means, for any purpose, without the express 
* written permission of ATTX, LLC.
******************************************************************************/
/****************************************************
* @brief Function to calculate range and range rate 
of a target with respect to an observer.
****************************************************/
 
#ifndef CALC_RANGE_RANGE_RATE_HPP
#define CALC_RANGE_RANGE_RATE_HPP
 
#include <cmath>
 
#include "utils/Measurements.hpp"
#include "core/safemath.hpp"
 
#define RANGE_RANGERATE_TARGET_STATES 6
#define RANGE_RANGERATE_OBSERVER_STATES 6
#define RANGE_RANGERATE_MEASUREMENTS 2
 
namespace clockwerk {
 
    template <typename T>
    class CalcRangeRangeRate : public clockwerk::Measurements<T, RANGE_RANGERATE_TARGET_STATES, RANGE_RANGERATE_MEASUREMENTS, RANGE_RANGERATE_OBSERVER_STATES> {
        public:
            /// @brief   Function to calculate rate of change in 6-element orbit position with J2, J3
            /// @param   time    The reference time
            /// @param   tar_state   The target reference state [x, y, z, vx, vy, vz] of spacecraft
            /// @param   obs_state   The observer reference state [x, y, z, vx, vy, vz] of ground station
            /// @param   out_measurements   Implicit return of [range, range rate]
            int calculateMeasurements(T time, const std::array<T, RANGE_RANGERATE_TARGET_STATES> &tar_state, const std::array<T, RANGE_RANGERATE_OBSERVER_STATES> &obs_state,
                            std::array<T, RANGE_RANGERATE_MEASUREMENTS> &out_measurements);
        private:
 
    };
 
    template <typename T>
    int CalcRangeRangeRate<T>::calculateMeasurements(T time, const std::array<T, RANGE_RANGERATE_TARGET_STATES> &tar_state, const std::array<T, RANGE_RANGERATE_OBSERVER_STATES> &obs_state,
                            std::array<T, RANGE_RANGERATE_MEASUREMENTS> &out_measurements) {
 
        // Calculate range
        out_measurements[0] = std::sqrt( (tar_state[0]-obs_state[0])*(tar_state[0]-obs_state[0]) + (tar_state[1]-obs_state[1])*(tar_state[1]-obs_state[1]) + (tar_state[2]-obs_state[2])*(tar_state[2]-obs_state[2]));
        // Calculate range rate
        int err = clockwerk::safeDivide((tar_state[0]-obs_state[0])*(tar_state[3]-obs_state[3]) + (tar_state[1]-obs_state[1])*(tar_state[4]-obs_state[4]) + (tar_state[2]-obs_state[2])*(tar_state[5]-obs_state[5]),
        out_measurements[0], out_measurements[1]);
 
        return err;
 
    };
 
}
 
#endif