#include "mti.h"
#include "spi.h"
#include <string.h>
#include "usart.h"
#include "arm_math.h"
MTI_DAT MtiData;
MTI_D_DAT MtiDData;
MTI_D_DAT MtiDData_bak;
uint8_t mti_send_buffer[256] = {0};
uint8_t mti_recv_buffer[256] = {0};
void get_MTI_data(P_MTI_DAT *p_mti_data)
{
*p_mti_data = &MtiData;
}
void uDelay(uint32_t n)
{
uint32_t ticks = 0;
uint32_t told = 0;
uint32_t tnow = 0;
uint32_t tcnt = 0;
uint32_t reload = 0;
reload = SysTick->LOAD;
ticks = n * (SystemCoreClock / 1000000);
tcnt = 0;
told = SysTick->VAL;
while (1)
{
tnow = SysTick->VAL;
if (tnow != told)
{
if (tnow < told)
{
tcnt += told - tnow;
}
else
{
tcnt += reload - tnow + told;
}
told = tnow;
if (tcnt >= ticks)
{
break;
}
}
}
}
void mDelay(uint32_t Delay)
{
HAL_Delay(Delay);
}
void MtiReadProtocol(uint8_t opcode, uint16_t write_size)
{
uint16_t send_size = 0;
uint8_t head_buff[3] = {0xFF, 0xFF, 0xFF};
memset(mti_send_buffer, 0x0, 256);
mti_send_buffer[send_size++] = opcode;
memcpy(mti_send_buffer + send_size, head_buff, 3);
send_size += 3;
send_size += write_size;
switch(opcode)
{
case PipeStatus:
comm_set_cmd(MTIPIP_TX_CMD);
break;
case NotificationPipe:
comm_set_cmd(MTINOT_TX_CMD);
break;
case MeasurementPipe:
comm_set_cmd(MTIMEA_TX_CMD);
break;
default:
break;
}
MTI_CS_L();
HAL_SPI_TransmitReceive_DMA(&hspi2, mti_send_buffer, mti_recv_buffer, send_size);
}
uint8_t SpiSendRecv(uint8_t data)
{
uint8_t recv_dat = 0;
HAL_SPI_TransmitReceive(&hspi2, &data, &recv_dat, 1, 0xFF);
return recv_dat;
}
static void SpiReadMsg(uint8_t opcode, uint8_t* buff, uint8_t len)
{
int i;
MTI_CS_L();
SpiSendRecv(opcode);
SpiSendRecv(0xFF);
SpiSendRecv(0xFF);
SpiSendRecv(0xFF);
for(i=0;i<len;i++)
{
buff[i] = SpiSendRecv(0x00);
}
MTI_CS_H();
}
static void SpiWriteMsg(uint8_t opcode, uint8_t* buff, uint8_t len)
{
int i;
MTI_CS_L();
SpiSendRecv(opcode);
SpiSendRecv(0xFF);
SpiSendRecv(0xFF);
SpiSendRecv(0xFF);
for(i=0;i<len;i++)
{
SpiSendRecv(buff[i]);
}
MTI_CS_H();
}
void MtiProtocol(uint8_t opcode, uint8_t* buff, uint8_t len)
{
switch(opcode)
{
case ProtocolInfo:
SpiReadMsg(opcode, buff, len);
break;
case ConfigureProtocol:
SpiWriteMsg(opcode, buff, len);
break;
case ControlPipe:
SpiWriteMsg(opcode, buff, len);
break;
case PipeStatus:
SpiReadMsg(opcode, buff, len);
break;
case NotificationPipe:
SpiReadMsg(opcode, buff, len);
break;
case MeasurementPipe:
SpiReadMsg(opcode, buff, len);
break;
default:
break;
}
}
void MTI_exit_init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = GPIO_PIN_8;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
HAL_NVIC_ClearPendingIRQ((IRQn_Type)(EXTI9_5_IRQn));
HAL_NVIC_SetPriority(EXTI9_5_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
}
void MTI_reset(void)
{
uint8_t buff[256];
uint16_t buff_pos = 0;
uint16_t len = 0;
MTI_RST_H();
uDelay(50);
MTI_RST_L();
mDelay(10);
memset(buff, 0x0, 256);
while(MTI_DRDY());
MtiProtocol(PipeStatus, buff, PIPESTATUS_LEN);
len = (buff[1] << 8)+ buff[0];
uDelay(5);
if(len)
{
memset(buff, 0x0, 256);
MtiProtocol(NotificationPipe, buff, len);
uDelay(5);
}
mDelay(1);
memset(buff, 0x0, 256);
buff[buff_pos++] = XMID_WakeupAck;
buff[buff_pos++] = 0x00;
buff[buff_pos++] = 0x00 - (0xFF + buff[0] + buff[1]);
MtiProtocol(ControlPipe, buff, buff_pos);
uDelay(5);
}
void MTI_deviceID(void)
{
uint8_t buff[256];
uint16_t buff_pos = 0;
uint16_t len = 0;
memset(buff, 0x0, 256);
buff[buff_pos++] = XMID_ReqDid;
buff[buff_pos++] = 0x00;
buff[buff_pos++] = 0x00 - (0xFF + buff[0] + buff[1]);
MtiProtocol(ControlPipe, buff, buff_pos);
memset(buff, 0x0, 256);
while(MTI_DRDY());
MtiProtocol(PipeStatus, buff, PIPESTATUS_LEN);
uDelay(5);
len = (buff[1] << 8) + buff[0];
if(len)
{
memset(buff, 0x0, 256);
MtiProtocol(NotificationPipe, buff, len);
uDelay(5);
}
}
void MTI_setparam(void)
{
uint8_t i;
uint8_t buff[256];
uint16_t buff_pos = 0;
uint16_t len = 0;
memset(buff, 0x0, 256);
buff[buff_pos++] = XMID_SetOutputConfig;
buff[buff_pos++] = 0x10;
buff[buff_pos++] = (uint8_t)((XDI_EulerAngles >> 8) & 0xff);
buff[buff_pos++] = (uint8_t)(XDI_EulerAngles & 0xff);
buff[buff_pos++] = 0x00;
buff[buff_pos++] = 100;
buff[buff_pos++] = (uint8_t)((XDI_Acceleration >> 8) & 0xff);
buff[buff_pos++] = (uint8_t)(XDI_Acceleration & 0xff);
buff[buff_pos++] = 0x00;
buff[buff_pos++] = 100;
buff[buff_pos++] = (uint8_t)((XDI_RateOfTurn >> 8) & 0xff);
buff[buff_pos++] = (uint8_t)(XDI_RateOfTurn & 0xff);
buff[buff_pos++] = 0x00;
buff[buff_pos++] = 100;
buff[buff_pos++] = (uint8_t)((XDI_MagneticField >> 8) & 0xff);
buff[buff_pos++] = (uint8_t)(XDI_MagneticField & 0xff);
buff[buff_pos++] = 0x00;
buff[buff_pos++] = 100;
buff[buff_pos] = 0x00;
buff[buff_pos] -= 0xFF;
for(i = 0; i < 18; i++)
{
buff[buff_pos] -= buff[i];
}
MtiProtocol(ControlPipe, buff, buff_pos + 1);
memset(buff, 0x0, 256);
while(MTI_DRDY());
MtiProtocol(PipeStatus, buff, PIPESTATUS_LEN);
uDelay(5);
len = (buff[1]<<8)+buff[0];
if(len)
{
memset(buff, 0x0, 256);
MtiProtocol(NotificationPipe, buff, len);
uDelay(5);
}
}
void MTI_sart_measure(void)
{
uint8_t buff[256];
uint16_t buff_pos = 0;
uint16_t len = 0;
memset(buff, 0x0, 256);
buff[buff_pos++] = XMID_GotoMeasurement;
buff[buff_pos++] = 0x00;
buff[buff_pos++] = 0x00-(0xFF + buff[0] + buff[1]);
MtiProtocol(ControlPipe, buff, buff_pos);
memset(buff, 0x0, 256);
while(MTI_DRDY());
MtiProtocol(PipeStatus, buff, PIPESTATUS_LEN);
uDelay(5);
len = (buff[1] << 8) + buff[0];
if(len)
{
memset(buff, 0x0, 256);
MtiProtocol(NotificationPipe, buff, len);
uDelay(5);
}
}
void MTI_Init(void)
{
MTI_reset();
MTI_deviceID();
MTI_setparam();
MTI_sart_measure();
MTI_exit_init();
}
void MTI_proc_dat(uint8_t *dst_buffer, uint8_t *src_buffer, uint8_t dat_pos)
{
dst_buffer[0] = src_buffer[dat_pos + 6];
dst_buffer[1] = src_buffer[dat_pos + 5];
dst_buffer[2] = src_buffer[dat_pos + 4];
dst_buffer[3] = src_buffer[dat_pos + 3];
dst_buffer[4] = src_buffer[dat_pos + 10];
dst_buffer[5] = src_buffer[dat_pos + 9];
dst_buffer[6] = src_buffer[dat_pos + 8];
dst_buffer[7] = src_buffer[dat_pos + 7];
dst_buffer[8] = src_buffer[dat_pos + 14];
dst_buffer[9] = src_buffer[dat_pos + 13];
dst_buffer[10] = src_buffer[dat_pos + 12];
dst_buffer[11] = src_buffer[dat_pos + 11];
}
void MTI_proc(uint8_t *p, uint8_t len)
{
uint8_t i;
uint8_t packlen;
uint16_t packid;
P_MTI_DAT p_mti_data;
get_MTI_data(&p_mti_data);
for(i = 0; i < len;)
{
packid = (p[i] << 8) + p[i + 1];
packlen = p[i + 2];
switch(packid)
{
case XDI_EulerAngles:
MTI_proc_dat(p_mti_data->Euler_u.byte, p, i);
p_mti_data->Euler_u.Euler_s.pitch = (-1) * p_mti_data->Euler_u.Euler_s.pitch;
p_mti_data->Euler_u.Euler_s.yaw = (-1) * p_mti_data->Euler_u.Euler_s.yaw;
if((p_mti_data->Euler_u.Euler_s.yaw >= (float)((-1) * 180)) && (p_mti_data->Euler_u.Euler_s.yaw < 0.0f))
{
p_mti_data->Euler_u.Euler_s.yaw += 360.0f;
}
MtiDData.x_u.x = p_mti_data->Euler_u.Euler_s.pitch - MtiDData_bak.x_u.x;
MtiDData.y_u.y = p_mti_data->Euler_u.Euler_s.roll - MtiDDa
