MTI-1.zip_HMC6343_STM32F777_mti-1_xsens

#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