PdAttitudeControl.h

/******************************************************************************
* 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.
******************************************************************************/
/*
LED Blinker header file

Author: Alex Reynolds
*/ 
#ifndef APPS_PD_ATTITUDE_CONTROL_H
#define APPS_PD_ATTITUDE_CONTROL_H

#include "flight/App.h"
#include "flight/FlightExecutive.h"
#include "core/CartesianVector.hpp"
#include "dynamics/Quaternion.h"
#include "dynamics/DCM.h"
#include "core/mathmacros.h"

#include "telemetry/tlm_PdAttitudeControl.h"
#include "command/cmd_PdAttitudeControl.h"

namespace warpos {
    
    /**
     * @brief Simple PD attitude controller
     * 
     * This app implements a simple PD attitude controller using a quaternion and
     * body-referenced angular velocity, where the P term is based on the vector
     * component of the error quaternion between desired and actual, and the 
     * D term is based on the difference between the angular velocities. 
     * 
     * CONTROL = P*quat_err + K*ang_vel_err
     * 
     * The controller implements a deadband which is based on the magnitude of the
     * rotation quaternion between desired and actual. 
     * 
     * As of 10/22/2025, P and K are three element vectors
     * 
     *
     * As of 10/22/2025, P and K are three element vectors
     *
     */
    class PdAttitudeControl : public App {
    public:
        START_PARAMS
            /** The proportional weight in the PD controller */
            SIGNAL(P,                       CartesianVector3,               CartesianVector3({0.0, 0.0, 0.0}))
            /** The angular acceleration (derivative) weight in the PD controller */
            SIGNAL(K,                       CartesianVector3,               CartesianVector3({0.0, 0.0, 0.0}))
            /** Orientation error dead band. If the attitude error is less than this
             *  value, then no command will be output. (radians) */
            SIGNAL(angle_deadband_rad,      floating_point,                 0.0)
        END_PARAMS

        START_INPUTS
            /** The commanded attitude. This is the desired attitude quaternion for the system */
            SIGNAL(cmd_quat,                clockwerk::Quaternion,          clockwerk::Quaternion({1.0, 0.0, 0.0, 0.0}))
            /** The actual attitude. This is the current attitude quaternion for the system */
            SIGNAL(act_quat,                clockwerk::Quaternion,          clockwerk::Quaternion({1.0, 0.0, 0.0, 0.0}))
            /** The commanded angular velocity, represented in the commanded attitude frame. (radians/second) */
            SIGNAL(cmd_omega__cmd,          CartesianVector3,               CartesianVector3({0.0, 0.0, 0.0}))
            /** The actual angular velocity, represented in the current body frame. (radians/second) */
            SIGNAL(act_omega__act,          CartesianVector3,               CartesianVector3({0.0, 0.0, 0.0}))
        END_INPUTS

        START_OUTPUTS
            /** The commanded torque, as represented in the body frame */
            SIGNAL(cmd_torque__body,        CartesianVector3,               CartesianVector3({0.0, 0.0, 0.0}))
            /** The current error quaternion for the system: diff actual/command */
            SIGNAL(err_quat,                clockwerk::Quaternion,          clockwerk::Quaternion({1.0, 0.0, 0.0, 0.0}))
            /** The magnitude of the error angle between control and actual attitude (radians) */
            SIGNAL(err_angle,               floating_point,                 0.0)
            /** The current error in angular velocity in the system: actual - cmd (rad/s) */
            SIGNAL(err_omega,               CartesianVector3,               CartesianVector3({0.0, 0.0, 0.0}))
        END_OUTPUTS

        PdAttitudeControl(FlightExecutive &executive);

        virtual ~PdAttitudeControl() {};
    
        /// @brief Activate the app. The app will step when active.
        /// @return Flag indicating success/failure
        int16 activate() override;

        /// @brief Deactivate the app. The app will not step when deactivated
        /// @return Flag indicating success/failure
        int16 deactivate() override;

        /// @brief Process commands issued to the app
        /// @param apid The APID of the command sent
        /// @param buffer Pointer to the location of the buffer holding the command
        /// @param size The size of the command being sent
        /// @return Flag indicating success/failure
        int16 command(uint16 apid, uint8* buffer, uint16 size) override;
    protected:
        int16 start() override;
        int16 execute() override;

        /// Internal variable for the attiude angle error (radians)
        floating_point _angle;

        /// Internal constants for gains
        CartesianVector3 _K = CartesianVector3({0.0, 0.0, 0.0});
        CartesianVector3 _PP = CartesianVector3({0.0, 0.0, 0.0});
        floating_point _att_deadband_rad = 0.0;

        /// Temporary vector for quaternion vector component
        CartesianVector3 _err_quat_vec;

        /// Temporary vectors to hold proportional and derivative command terms:
        CartesianVector3 _prop_cmd;
        CartesianVector3 _deriv_cmd;

        /// Quaternion representing the distance between the actual and commanded quaternion
        clockwerk::Quaternion _quat_act_cmd;

        /// Internal variables to hold telemetry buffer for packing tlm
        tlm_gnc_pd_att_ctrl_data _tlm_data;
    };
}

#endif