GPSUpdate.h

Go to the documentation of this file.

/******************************************************************************
* Copyright (c) ATTX INC 2025. All Rights Reserved.
*
* This software and associated documentation (the "Software") are the 
* proprietary and confidential information of ATTX, INC. 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, INC.
******************************************************************************/
/*
Inertial Navigation EKF: GPS Measurement Update Header File
 
Author: James Tabony
*/
 
#ifndef APPS_GNC_INERTIAL_NAV_GPS_UPDATE_APP_H
#define APPS_GNC_INERTIAL_NAV_GPS_UPDATE_APP_H
 
#include "core/mathmacros.h"
 
#include "flight/App.h"
#include "flight/FlightExecutive.h"
 
#include "telemetry/tlm_InertialNavigationGPSMeasurementUpdate.h"
#include "command/cmd_InertialNavigationGPSMeasurementUpdate.h"
 
#include "gncutils/EKF/GPSMeasurements.hpp"
#include "gncutils/EKF/EkfMeasurementUpdate.hpp"
 
#include "apps/gnc/InertialNavigationEKF/InertialNavigationDefinition.hpp"
namespace warpos{

    class GPSUpdate : public App {
    public:
        START_PARAMS
            SIGNAL(time_diff_stale_bound,       clockwerk::Time,                clockwerk::Time(1, 0))
            SIGNAL(GPS_meas_noise,  clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(pos_pI_tI__pI,               clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(pos_GPS_B__B,                clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
        END_PARAMS
 
        START_INPUTS
            // ***** STATE INPUT DEFINITIONS ***** //

            SIGNAL(pos_B_pI__pI,                clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(vel_B_pI__pI,                clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(quat_B_pI,                   clockwerk::Quaternion,          clockwerk::Quaternion({1.0, 0.0, 0.0, 0.0}))
            SIGNAL(gyroBias_B_pI__pI,           clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(accelBias_B_pI__pI,          clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(state_cov,                   Matrix16,                       Matrix16())
            SIGNAL(state_input_time_stamp,      clockwerk::Time,                clockwerk::Time(0, 0))
 
            // ***** GPS MEASUREMENT INPUT DEFINITIONS ***** //
 
            
            SIGNAL(GPS_meas_vec__ECEF,           clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(GPS_meas_time_stamp,         clockwerk::Time,                clockwerk::Time(0, 0))
            SIGNAL(GPS_meas_validity_flag,      bool,                           false)
 
            // ***** ADDITIONAL INPUT DEFINITIONS ***** //

            SIGNAL(quat_pI_REF,                 clockwerk::Quaternion,          clockwerk::Quaternion({1.0, 0.0, 0.0, 0.0}))
        END_INPUTS
 
        START_OUTPUTS
            SIGNAL(pos_B_pI__pI,                clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(vel_B_pI__pI,                clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(quat_B_pI,                   clockwerk::Quaternion,          clockwerk::Quaternion({1.0, 0.0, 0.0, 0.0}))
            SIGNAL(gyroBias_B_pI__pI,           clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(accelBias_B_pI__pI,          clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(state_cov,                  Matrix16,                        Matrix16())
            SIGNAL(state_output_time_stamp,      clockwerk::Time,                clockwerk::Time(0, 0))
            SIGNAL(pre_residual,          clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(post_residual,          clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
 
        END_OUTPUTS
 
        GPSUpdate(FlightExecutive &executive);
 
        virtual ~GPSUpdate() {};

        int16 activate() override;

        int16 deactivate() override;

        int16 command(uint16 apid, uint8* buffer, uint16 size) override;

        inertial_nav::MagMeasVector getResidual() {return _residual;}
 
    protected:
        int16 start() override;
        int16 execute() override;

        GPSMeasurements<inertial_nav::STATE_SIZE> _measurements;
        EkfMeasurementUpdate<inertial_nav::STATE_SIZE, GPS_MEASUREMENT_VECTOR_ELEMENTS,
                             GPS_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS> _measUpdate;

        std::array<floating_point, 3> _dummy3; 
        std::array<floating_point, 4> _dummy4; 
        inertial_nav::MagObsVector _dummyObs; 

        inertial_nav::StateVector _state_input_array, _state_output_array;
        inertial_nav::GpsObsVector _state_observer_array;
        inertial_nav::GpsMeasVector _measurement_array, _residual;

        clockwerk::CartesianVector<3> _gps_meas_vec_B_pI__pI;

        clockwerk::Quaternion _unit_quat;

        tlm_gnc_gps_update_state _tlm_a_posteriori_state;

        clockwerk::Time _repeat_check_time;
        CartesianVector3 _repeat_check_meas;
    };
}
 
 
#endif