AirControlValve/Device/Imu.c

/* Copyright Statement:
 *
 * This software/firmware and related documentation ("AutoChips Software") are
 * protected under relevant copyright laws. The information contained herein is
 * confidential and proprietary to AutoChips Inc. and/or its licensors. Without
 * the prior written permission of AutoChips inc. and/or its licensors, any
 * reproduction, modification, use or disclosure of AutoChips Software, and
 * information contained herein, in whole or in part, shall be strictly
 * prohibited.
 *
 * AutoChips Inc. (C) 2016. All rights reserved.
 *
 * BY OPENING THIS FILE, RECEIVER HEREBY UNEQUIVOCALLY ACKNOWLEDGES AND AGREES
 * THAT THE SOFTWARE/FIRMWARE AND ITS DOCUMENTATIONS ("AUTOCHIPS SOFTWARE")
 * RECEIVED FROM AUTOCHIPS AND/OR ITS REPRESENTATIVES ARE PROVIDED TO RECEIVER
 * ON AN "AS-IS" BASIS ONLY. AUTOCHIPS EXPRESSLY DISCLAIMS ANY AND ALL
 * WARRANTIES, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE OR
 * NONINFRINGEMENT. NEITHER DOES AUTOCHIPS PROVIDE ANY WARRANTY WHATSOEVER WITH
 * RESPECT TO THE SOFTWARE OF ANY THIRD PARTY WHICH MAY BE USED BY,
 * INCORPORATED IN, OR SUPPLIED WITH THE AUTOCHIPS SOFTWARE, AND RECEIVER AGREES
 * TO LOOK ONLY TO SUCH THIRD PARTY FOR ANY WARRANTY CLAIM RELATING THERETO.
 * RECEIVER EXPRESSLY ACKNOWLEDGES THAT IT IS RECEIVER'S SOLE RESPONSIBILITY TO
 * OBTAIN FROM ANY THIRD PARTY ALL PROPER LICENSES CONTAINED IN AUTOCHIPS
 * SOFTWARE. AUTOCHIPS SHALL ALSO NOT BE RESPONSIBLE FOR ANY AUTOCHIPS SOFTWARE
 * RELEASES MADE TO RECEIVER'S SPECIFICATION OR TO CONFORM TO A PARTICULAR
 * STANDARD OR OPEN FORUM. RECEIVER'S SOLE AND EXCLUSIVE REMEDY AND AUTOCHIPS'S
 * ENTIRE AND CUMULATIVE LIABILITY WITH RESPECT TO THE AUTOCHIPS SOFTWARE
 * RELEASED HEREUNDER WILL BE, AT AUTOCHIPS'S OPTION, TO REVISE OR REPLACE THE
 * AUTOCHIPS SOFTWARE AT ISSUE, OR REFUND ANY SOFTWARE LICENSE FEES OR SERVICE
 * CHARGE PAID BY RECEIVER TO AUTOCHIPS FOR SUCH AUTOCHIPS SOFTWARE AT ISSUE.
 */
 
#include "ac780x_uart_reg.h"
#include "ac780x_gpio.h"
#include "Imu.h"
#include "crc16.h"
#include "uart.h"
#include "string.h"
#include <math.h>

#define UARTx   UART2
#define UARTx_IRQn  UART2_IRQn

#define UART_RX_BUFF_LENGTH     16

uint8_t g_imuRxDataBuff[UART_RX_BUFF_LENGTH];
uint16_t g_imuRxDataIndex;

uint8_t *g_imuTxDataPoint;
uint16_t g_imuTxDataLength;
uint16_t g_imuTxDataIndex;

static uint8_t imucmd_buffer[7];

static uint8_t init_finished = 0;
static float roll_angle = 0;
static float pitch_angle = 0;
static float yaw_angle = 0;

static uint8_t imu_dataError_count = 0;


//static int16_t  print_times =0;

//static uint32_t printfBuff[16];
//static uint16_t printfLen = 0;


static int TransmitData(uint8_t *pdata, uint16_t length);
/**
* UartEventCallback
*
* @param[in] device: UART_Type pointer
* @param[in] wpara: UART lsr0 register
* @param[in] lpara: UART lsr1 register
* @return    none
*
* @brief   uart receive handle
*/
static void UartEventCallback(void *device, uint32_t wpara, uint32_t lpara)
{
    int16_t tmp = 0;
		uint8_t sum = 0;
		uint8_t i =0;
    UART_Type *uart_Device = (UART_Type *)device;
    /*rx interrupt*/
    if ((uart_Device->IER & UART_IER_ERXNE_Msk) && (wpara & UART_LSR0_DR_Msk))
    {
        //data = uart_Device->RBR;  
        g_imuRxDataBuff[g_imuRxDataIndex++] = uart_Device->RBR;
        
        if (g_imuRxDataIndex >= 11)
        {
            g_imuRxDataIndex = 0;
					
						for( i=0; i<10; i++){
						   sum += g_imuRxDataBuff[i];
						}
			
						if(1 == init_finished){
							
							if((sum == g_imuRxDataBuff[10]) && (0x55 == g_imuRxDataBuff[0]) && (0x53 == g_imuRxDataBuff[1])){
													
								tmp = g_imuRxDataBuff[3];
								tmp = (tmp<<8)|g_imuRxDataBuff[2];
								roll_angle = 1.0*tmp/32768*180;

								
								tmp = g_imuRxDataBuff[5];
								tmp = (tmp<<8)|g_imuRxDataBuff[4];
								pitch_angle = 1.0*tmp/32768*180;

							
								tmp = g_imuRxDataBuff[7];
								tmp = (tmp<<8)|g_imuRxDataBuff[6];
								yaw_angle = 1.0*tmp/32768*180;
								
								
								imu_dataError_count = 0;	
								
								
							}else{	
								
								imu_dataError_count++;
								if(imu_dataError_count >= 5){
									 imu_dataError_count = 3;
									
									roll_angle = -300;
									pitch_angle = -300;
									yaw_angle = -300;
									
								}
							
							}
							
								
						}
						
        }
    }
		
		
    /*tx interrupt*/
    if ((uart_Device->IER & UART_IER_ETXE_Msk) && (wpara & UART_LSR0_THRE_Msk)) 
    {
        uart_Device->RBR = g_imuTxDataPoint[g_imuTxDataIndex++];

        if (g_imuTxDataIndex >= g_imuTxDataLength)
        {
            UART_SetTXEInterrupt(device, DISABLE);  ///<发送最后一个字节时关闭发送空中断
        }
    }
}
/**
* uart_Initialize
* 
* @param[in] none
* @return none
*
* @brief uart 初始化
*/
static void uart_Initialize(void)
{
    UART_ConfigType  uartConfig = {0};
    
    //set pin mux
		//GPIO_SetFunc(GPIOB, GPIO_PIN9, GPIO_FUN3);
		//GPIO_SetFunc(GPIOB, GPIO_PIN10, GPIO_FUN3);
    
    
    uartConfig.baudrate = 9600;
    uartConfig.dataBits = UART_WORD_LEN_8BIT;
    uartConfig.stopBits = UART_STOP_1BIT;
    uartConfig.parity = UART_PARI_NO;
    uartConfig.fifoByteEn = DISABLE; ///<must enable fifoByte when use DMA
    uartConfig.dmaEn = UART_DMA_TXRX_NONE;
    uartConfig.callBack = UartEventCallback;   ///<uart2 interrupt callback
    UART_Init(UARTx, &uartConfig);
    
    UART_SetRXNEInterrupt(UARTx, ENABLE);
    ///Enable UARTx interrupt
    NVIC_SetPriority(UARTx_IRQn, 3);
    NVIC_ClearPendingIRQ(UARTx_IRQn);
    NVIC_EnableIRQ(UARTx_IRQn);
    
}
/**
* TransmitData
* 
* @param[in] pdata :发送数据指针
* @param[in] length :发送数据长度
* @return none
*
* @brief uart 发送函数，通过中断发送 
*/
int TransmitData(uint8_t *pdata, uint16_t length) 
{
    if (g_imuTxDataIndex < g_imuTxDataLength)  
    {
        return -1;      ///<有数据正在发送
    }
    g_imuTxDataPoint = pdata;
    g_imuTxDataLength = length;
    g_imuTxDataIndex = 0;
    UART_SetTXEInterrupt(UARTx, ENABLE);
    return 0;
}


void Imu_Init(void)
{
		uint16_t crc;
		uart_Initialize();
		
		mdelay(30);
	
		//设置参数
		imucmd_buffer[0] = 0xFF;
		imucmd_buffer[1] = 0xAA;	
	
		//step1 解锁 
		imucmd_buffer[2] = 0x69;
		imucmd_buffer[3] = 0x88;
		imucmd_buffer[4] = 0xB5;
		crc = crc16(imucmd_buffer, 5);
		imucmd_buffer[5] = (uint8_t)((crc>>8) & 0x00ff);
		imucmd_buffer[6] = (uint8_t)(crc & 0x00ff);
		TransmitData(imucmd_buffer, 7);
		mdelay(100);
		
		//step2  设置输出内容 ，仅输出角度
		imucmd_buffer[2] = 0x02;
		imucmd_buffer[3] = 0x08;
		imucmd_buffer[4] = 0x00;
	
		crc = crc16(imucmd_buffer, 5);
		imucmd_buffer[5] = (uint8_t)((crc>>8) & 0x00ff);
		imucmd_buffer[6] = (uint8_t)(crc & 0x00ff);
		TransmitData(imucmd_buffer, 7);
		mdelay(100);
		
		//step3  设置单次回传 
		imucmd_buffer[2] = 0x03;
		imucmd_buffer[3] = 0x0C;
		imucmd_buffer[4] = 0x00;
	
		crc = crc16(imucmd_buffer, 5);
		imucmd_buffer[5] = (uint8_t)((crc>>8) & 0x00ff);
		imucmd_buffer[6] = (uint8_t)(crc & 0x00ff);
		TransmitData(imucmd_buffer, 7);
		
		init_finished = 1;
		
}

uint8_t Imu_MR(void)
{
		uint16_t crc;
		imucmd_buffer[0] = 0xFF;
		imucmd_buffer[1] = 0xAA;
		
		imucmd_buffer[2] = 0x03;
		imucmd_buffer[3] = 0x0C;
		imucmd_buffer[4] = 0x00;
	
		crc = crc16(imucmd_buffer, 5);
		imucmd_buffer[5] = (uint8_t)((crc>>8) & 0x00ff);
		imucmd_buffer[6] = (uint8_t)(crc & 0x00ff);
	
		g_imuRxDataIndex = 0;
		TransmitData(imucmd_buffer, 7);
	
		return 0;
}


uint8_t Imu_GetAngle(float* roll, float* pitch, float* yaw)
{
	UART_SetRXNEInterrupt(UARTx, DISABLE);
	
	*roll = roll_angle;
	*pitch = pitch_angle;
	*yaw = yaw_angle;
	
	UART_SetRXNEInterrupt(UARTx, ENABLE);
	
		return 0;
}

uint8_t Imu_Print(void)
{

}

void Imu_DeInit(void)
{

}