/*
 * --------------------------------------------------------------------------------------
 * Company              : 江苏中信博新能源科技股份有限公司上海子公司 2020-2021 版权所有
 * --------------------------------------------------------------------------------------
 * file Name            : angle.c 
 * Description			    : angle-lib
 * --------------------------------------------------------------------------------------
 * Tool Versions        : uVision V5.29.0.0
 * Target Device        : STM32F103|GD32F103
 * --------------------------------------------------------------------------------------
 * Engineer				      : tangtao
 * Revision				      : V0.0
 * Created Date			    : 2021.09.23
 * --------------------------------------------------------------------------------------
 * Engineer				      : 
 * Revision				      : 
 * Modified Date		    : 
 * Additional Comments	: 
 * --------------------------------------------------------------------------------------
 */

#include "LSM6DS3RegDefs.h"
#include "lib_of_tracker.h"
#include <math.h>



/*************************************************
  Function              : CalcAngle()
  Description           : CalcAngle 
  Input                 : Gx, Ay, Az, Gx_offset    
  Output                : angle        
  Return                : angle
  Engineer				      : tangtao
  Revision				      : V0.0
  Modified Date			    : 2021.09.23
  Additional Comments	  : 
*************************************************/
float CalcAngle(float Gx, float Ay, float Az, float Gx_offset)
{
    float acc_angle;
    Ay = Ay * LSM6DS3_ACC_FS;
    Az = Az * -1 * LSM6DS3_ACC_FS;
    Gx = (Gx - Gx_offset) * LSM6DS3_GYRO_FS;
    acc_angle = atan2f(Ay,Az) * 57.3248 + 90;
    
    return KalmanFilter(Gx,acc_angle);
}