/***********************************************************************************************************************
* DISCLAIMER
* This software is supplied by Renesas Electronics Corporation and is only intended for use with Renesas products.
* No other uses are authorized. This software is owned by Renesas Electronics Corporation and is protected under all
* applicable laws, including copyright laws. 
* THIS SOFTWARE IS PROVIDED "AS IS" AND RENESAS MAKES NO WARRANTIESREGARDING THIS SOFTWARE, WHETHER EXPRESS, IMPLIED
* OR STATUTORY, INCLUDING BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NON-INFRINGEMENT.  ALL SUCH WARRANTIES ARE EXPRESSLY DISCLAIMED.TO THE MAXIMUM EXTENT PERMITTED NOT PROHIBITED BY
* LAW, NEITHER RENESAS ELECTRONICS CORPORATION NOR ANY OF ITS AFFILIATED COMPANIES SHALL BE LIABLE FOR ANY DIRECT,
* INDIRECT, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES FOR ANY REASON RELATED TO THIS SOFTWARE, EVEN IF RENESAS OR
* ITS AFFILIATES HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES.
* Renesas reserves the right, without notice, to make changes to this software and to discontinue the availability 
* of this software. By using this software, you agree to the additional terms and conditions found by accessing the 
* following link:
* http://www.renesas.com/disclaimer
*
* Copyright (C) 2015, 2016 Renesas Electronics Corporation. All rights reserved.
***********************************************************************************************************************/

/***********************************************************************************************************************
* File Name    : r_cg_main.c
* Version      : Code Generator for RX231 V1.00.03.04 [05 Aug 2016]
* Device(s)    : R5F52315AxFM
* Tool-Chain   : CCRX
* Description  : This file implements main function.
* Creation Date: 2017-02-04
***********************************************************************************************************************/

/***********************************************************************************************************************
Pragma directive
***********************************************************************************************************************/
/* Start user code for pragma. Do not edit comment generated here */
/* End user code. Do not edit comment generated here */

/***********************************************************************************************************************
Includes
***********************************************************************************************************************/
#include "r_cg_macrodriver.h"
#include "r_cg_cgc.h"
#include "r_cg_cmt.h"
#include "r_cg_sci.h"
#include "r_cg_riic.h"
/* Start user code for include. Do not edit comment generated here */
#include "math.h"
/* End user code. Do not edit comment generated here */
#include "r_cg_userdefine.h"

/***********************************************************************************************************************
Global variables and functions
***********************************************************************************************************************/
/* Start user code for global. Do not edit comment generated here */
#define MPUSS 0				//	MPU slave select J17 jumper setting : 0 = GND / 1 = VCC
#if MPUSS
	uint8_t mpuSlaveAdr = 0x69;
#else
	uint8_t mpuSlaveAdr = 0x68;
#endif

int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
float dt;
float accel_angle_x, accel_angle_y, accel_angle_z;
float gyro_angle_x, gyro_angle_y, gyro_angle_z;
float filtered_angle_x, filtered_angle_y, filtered_angle_z;
float baseAcX, baseAcY, baseAcZ;  //accelerometer average value storage variables
float baseGyX, baseGyY, baseGyZ;  //gyroscope average value storage variables
float gyro_x, gyro_y, gyro_z; //angular velocity storage global variables : Angle rotated per unit time

static uint8_t data[5] = {0xFF,0,3,0,3};

void calcFilteredYPR();
void calcGyroYPR();
void calcAccelYPR();
void calibAccelGyro();
void readAccelGyro();
/* End user code. Do not edit comment generated here */

static void R_MAIN_UserInit(void);
/***********************************************************************************************************************
* Function Name: main
* Description  : This function implements main function.
* Arguments    : None
* Return Value : None
***********************************************************************************************************************/
void main(void)
{
    R_MAIN_UserInit();
    /* Start user code. Do not edit comment generated here */
    while (1U)
    {
		readAccelGyro();
    	dt = cmt_checkTime();
    	dt = dt / 1000;
    	R_CMT0_Start();

		calcAccelYPR(); // accelerometer processing
		calcGyroYPR(); // gyroscope processing
		calcFilteredYPR();

		if(sciFlg){
			data[2] = 3;
			data[4] = 3;

			data[1] = (uint8_t)fabs(filtered_angle_x);  //double to integer
			if (filtered_angle_x > 0) data[2] -= 2 ;  //check sign
			data[3] = (uint8_t)fabs(filtered_angle_y);
			if (filtered_angle_y < 0) data[4] -= 2 ;

			R_SCI1_AsyncTransmit(data,5);
			sciFlg = 0;
		}
    }
    /* End user code. Do not edit comment generated here */
}
/***********************************************************************************************************************
* Function Name: R_MAIN_UserInit
* Description  : This function adds user code before implementing main function.
* Arguments    : None
* Return Value : None
***********************************************************************************************************************/
void R_MAIN_UserInit(void)
{
    /* Start user code. Do not edit comment generated here */
    uint16_t protect_dummy = (uint16_t)(SYSTEM.PRCR.WORD & 0x000FU);
    uint8_t start[2] = {0x6B,0x00};
    /* Disable protect bit */
    SYSTEM.PRCR.WORD = 0xA50FU;

    SYSTEM.VBATTCR.BYTE = 0x81U;
	
    /* Restore the previous state of the protect register */
    SYSTEM.PRCR.WORD = (uint16_t)(0xA500U | protect_dummy);
    R_CMT0_Start();
    R_SCI1_Start();
    R_RIIC0_Start();
    R_SCI1_Serial_Receive((uint8_t *)&g_rx_char, 1);

	R_RIIC0_Master_Send(mpuSlaveAdr,start,2);
	cmt_delayms(10);
	calibAccelGyro();

	R_CMT0_Start();
    /* End user code. Do not edit comment generated here */
}

/* Start user code for adding. Do not edit comment generated here */
void readAccelGyro(){
	uint8_t rx_buf[14] = {0,};
	R_RIIC0_Master_Receive(mpuSlaveAdr,0x3B,rx_buf,14);
	cmt_delayms(10);
	AcX = rx_buf[0] << 8 | rx_buf[1];
	AcY = rx_buf[2] << 8 | rx_buf[3];
	AcZ = rx_buf[4] << 8 | rx_buf[5];
	Tmp = rx_buf[6] << 8 | rx_buf[7];
	GyX = rx_buf[8] << 8 | rx_buf[9];
	GyY = rx_buf[10] << 8 | rx_buf[11];
	GyZ = rx_buf[12] << 8 | rx_buf[13];
}

void calibAccelGyro(){
	baseAcX = 547;
	baseAcY = -146;
	baseAcZ = 16650;
	baseGyX = 4608;
	baseGyY = 63;
	baseGyZ = 51;
}

void calcAccelYPR(){
  float accel_x, accel_y, accel_z; //accelerometer sensor final correction value
  float accel_xz, accel_yz;
  const float RADIANS_TO_DEGREES = 180/3.14159;

  accel_x = AcX - baseAcX; // accelerometer (straight line) Current value for X axis - Average value of accelerometer
  accel_y = AcY - baseAcY;
  accel_z = AcZ + (16384 - baseAcZ);

  //Angle + X-axis tilted angle wanted
  accel_yz = sqrt(pow(accel_y, 2) + pow(accel_z, 2));
  accel_xz = sqrt(pow(accel_x, 2) + pow(accel_z, 2));
  accel_angle_y = atan(-accel_x / accel_yz)*RADIANS_TO_DEGREES;
  accel_angle_x = atan(accel_y / accel_xz)*RADIANS_TO_DEGREES;

  accel_angle_z = 0;
}

void calcGyroYPR(){
  const float GYROXYZ_TO_DEGREES_PER_SED = 131;
                                  //The 131 value is 131 when the gyro turns 1 degree for 1 second.
  gyro_x = (GyX - baseGyX) / GYROXYZ_TO_DEGREES_PER_SED;
  gyro_y = (GyY - baseGyY) / GYROXYZ_TO_DEGREES_PER_SED;
  gyro_z = (GyZ - baseGyZ) / GYROXYZ_TO_DEGREES_PER_SED;

  gyro_angle_x += gyro_x * dt;
  gyro_angle_y += gyro_y * dt;
  gyro_angle_z += gyro_z * dt;
}

void calcFilteredYPR(){
  const float ALPHA = 0.96;
  float tmp_angle_x, tmp_angle_y, tmp_angle_z;  //Prev filter angle

  tmp_angle_x = filtered_angle_x + gyro_x * dt; //Gyroangle = angular velocity x Sensor input cycle
                                                //angular velocity = Angle rotated per unit time -> rotation angle / unit time
  tmp_angle_y = filtered_angle_y + gyro_y * dt;
  tmp_angle_z = filtered_angle_z + gyro_z * dt;

  filtered_angle_x = ALPHA * tmp_angle_x + (1.0 - ALPHA) * accel_angle_x;
  filtered_angle_y = ALPHA * tmp_angle_y + (1.0 - ALPHA) * accel_angle_y;
  filtered_angle_z = tmp_angle_z; //Curve + Z axis is shown using only gyro sensor (acceleration sensor is not)
  //So when you look above (calcAccelYPR), accel_angle_z is zero.
}
/* End user code. Do not edit comment generated here */
