MagUpdate.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: Magnetometer Measurement Update Header File
 
Author: James Tabony
*/
 
#ifndef APPS_GNC_INERTIAL_NAV_MAG_UPDATE_APP_H
#define APPS_GNC_INERTIAL_NAV_MAG_UPDATE_APP_H
 
#include "core/mathmacros.h"
 
#include "flight/App.h"
#include "flight/FlightExecutive.h"
 
#include "telemetry/tlm_InertialNavigationMagnetometerMeasurementUpdate.h"
#include "command/cmd_InertialNavigationMagnetometerMeasurementUpdate.h"
 
#include "gncutils/EKF/PointingVectorMeasurements.hpp"
#include "gncutils/EKF/EkfMeasurementUpdate.hpp"
 
#include "apps/gnc/InertialNavigationEKF/InertialNavigationDefinition.hpp"
namespace warpos{

    class MagUpdate : public App {
    public:
        START_PARAMS
            SIGNAL(time_diff_stale_bound,       clockwerk::Time,                clockwerk::Time(5, 0))
            SIGNAL(mag_meas_noise,  clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(quat_MAG_B,                  clockwerk::Quaternion,          clockwerk::Quaternion({1.0, 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))
 
            // ***** MAG MEASUREMENT INPUT DEFINITIONS ***** //
 
            
            SIGNAL(mag_meas_vec__MAG,           clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            SIGNAL(MAG_meas_time_stamp,         clockwerk::Time,                clockwerk::Time(0, 0))
            SIGNAL(MAG_meas_validity_flag,      bool,                           false)
 
            // ***** ADDITIONAL INPUT DEFINITIONS ***** //

            SIGNAL(expected_mag_vec__REF,       clockwerk::CartesianVector<3>,  clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}))
            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
 
        MagUpdate(FlightExecutive &executive);
 
        virtual ~MagUpdate() {};

        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;

        PointingVectorMeasurements<inertial_nav::STATE_SIZE> _measurements;
        EkfMeasurementUpdate<inertial_nav::STATE_SIZE, POINTING_MEASUREMENT_MEASUREMENT_VECTOR_ELEMENTS,
                             POINTING_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::MagObsVector _state_observer_array;
        inertial_nav::MagMeasVector _measurement_array, _residual;

        clockwerk::CartesianVector<3> _expected_mag_vec__pI, _mag_meas_vec__B;

        clockwerk::Quaternion _unit_quat;

        tlm_gnc_mag_update_state _tlm_a_posteriori_state;

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