motor-controller/app/sensor_task.c

#include "app.h"
#include "cmsis_os2.h"
#include "soft_i2c.h"
static soft_i2c_t si2c;

#define ACC_UPDATE		0x01
#define GYRO_UPDATE		0x02
#define ANGLE_UPDATE	0x04
#define MAG_UPDATE		0x08
#define READ_UPDATE		0x80
static volatile char s_cDataUpdate = 0, s_cCmd = 0xff;
static void CmdProcess(void);
static void AutoScanSensor(void);
static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum);
static void Delayms(uint16_t ucMs);

static int32_t IICreadBytes(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen) {
	soft_i2c_start_frame(&si2c);
	soft_i2c_send_frame(&si2c, ucAddr);
	if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
	soft_i2c_send_frame(&si2c, ucReg);
	if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
	soft_i2c_start_frame(&si2c);
	soft_i2c_send_frame(&si2c, ucAddr+1);
	if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
	for(uint32_t i = 0; i < uiLen; i++)
	{
		if(i+1 == uiLen)*p_ucVal++ = soft_i2c_read_frame(&si2c, 0);  //last byte no ask
		else *p_ucVal++ = soft_i2c_read_frame(&si2c, 1);  //  ask
	}
	soft_i2c_stop_frame(&si2c);
	return 1;
}

static int32_t IICwriteBytes (uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen) {
	soft_i2c_start_frame(&si2c);
	soft_i2c_send_frame(&si2c, ucAddr);
	if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
	soft_i2c_send_frame(&si2c, ucReg);
	if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
	for(uint32_t i = 0; i < uiLen; i++)
	{
		soft_i2c_send_frame(&si2c, *p_ucVal++); 
		if(soft_i2c_wait_ack_frame(&si2c) != 0)return 0;
	}
	soft_i2c_stop_frame(&si2c);
	return 1;
}

void sensor_task(void)
{
	float fAcc[3], fGyro[3], fAngle[3];
	int i;
	int ret;
	ret = soft_i2c_init(&si2c, 0);
	printf("[sensor] soft_i2c_init = %d\r\n", ret);
	WitInit(WIT_PROTOCOL_I2C, 0x50);
	WitI2cFuncRegister(IICwriteBytes, IICreadBytes);
	WitRegisterCallBack(CopeSensorData);
	WitDelayMsRegister(Delayms);
	AutoScanSensor();
	while (1)
	{
		WitReadReg(AX, 12);
		Delayms(5);
		CmdProcess();
		if(s_cDataUpdate)
		{
			for(i = 0; i < 3; i++)
			{
				fAcc[i] = sReg[AX+i] * 0.0054931640625f;
				fGyro[i] = sReg[GX+i] * 0.0054931640625f;
				fAngle[i] = sReg[Roll+i] * 0.0054931640625f;
			}
			if(s_cDataUpdate & ACC_UPDATE)
			{
				printf("raw:AX=%d AY=%d AZ=%d GX=%d GY=%d GZ=%d\r\n",
					sReg[AX], sReg[AY], sReg[AZ],
					sReg[GX], sReg[GY], sReg[GZ]);
				printf("acc:%.3f %.3f %.3f\r\n", fAcc[0], fAcc[1], fAcc[2]);
				s_cDataUpdate &= ~ACC_UPDATE;
			}
			if(s_cDataUpdate & GYRO_UPDATE)
			{
				printf("gyro:%.3f %.3f %.3f\r\n", fGyro[0], fGyro[1], fGyro[2]);
				s_cDataUpdate &= ~GYRO_UPDATE;
			}
			if(s_cDataUpdate & ANGLE_UPDATE)
			{
				printf("angle:%.3f %.3f %.3f\r\n", fAngle[0], fAngle[1], fAngle[2]);
				s_cDataUpdate &= ~ANGLE_UPDATE;
			}
			if(s_cDataUpdate & MAG_UPDATE)
			{
				printf("mag:%d %d %d\r\n", sReg[HX], sReg[HY], sReg[HZ]);
				s_cDataUpdate &= ~MAG_UPDATE;
			}
		}
	}
}


void CopeCmdData(unsigned char ucData)
{
	static unsigned char s_ucData[50], s_ucRxCnt = 0;
	
	s_ucData[s_ucRxCnt++] = ucData;
	if(s_ucRxCnt<3)return;										//Less than three data returned
	if(s_ucRxCnt >= 50) s_ucRxCnt = 0;
	if(s_ucRxCnt >= 3)
	{
		if((s_ucData[1] == '\r') && (s_ucData[2] == '\n'))
		{
			s_cCmd = s_ucData[0];
			memset(s_ucData,0,50);//
			s_ucRxCnt = 0;
		}
		else 
		{
			s_ucData[0] = s_ucData[1];
			s_ucData[1] = s_ucData[2];
			s_ucRxCnt = 2;
			
		}
	}

}

static void CmdProcess(void)
{
	switch(s_cCmd)
	{
		case 'a':	
				if(WitStartAccCali() != WIT_HAL_OK) 
					printf("\r\nSet AccCali Error\r\n");
			break;
		case 'm':	
				if(WitStartMagCali() != WIT_HAL_OK) 
					printf("\r\nStart MagCali Error\r\n");
			break;
		case 'e':	
				if(WitStopMagCali() != WIT_HAL_OK) 
					printf("\r\nEnd MagCali Error\r\n");
			break;
		case 'u':	
				if(WitSetBandwidth(BANDWIDTH_5HZ) != WIT_HAL_OK) 
					printf("\r\nSet Bandwidth Error\r\n");
			break;
		case 'U':	
				if(WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK)
					printf("\r\nSet Bandwidth Error\r\n");
			break;
		case 'B':	
				if(WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK) 
					printf("\r\nSet Baud Error\r\n");
			break;
		case 'b':	
				if(WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK) 
					printf("\r\nSet Baud Error\r\n");
			break;
		default : return ;
	}
	s_cCmd = 0xff;
}

static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum)
{
	int i;
    for(i = 0; i < uiRegNum; i++)
    {
        switch(uiReg)
        {
//            case AX:
//            case AY:
            case AZ:
				s_cDataUpdate |= ACC_UPDATE;
            break;
//            case GX:
//            case GY:
            case GZ:
				s_cDataUpdate |= GYRO_UPDATE;
            break;
//            case HX:
//            case HY:
            case HZ:
				s_cDataUpdate |= MAG_UPDATE;
            break;
//            case Roll:
//            case Pitch:
            case Yaw:
				s_cDataUpdate |= ANGLE_UPDATE;
            break;
            default:
				s_cDataUpdate |= READ_UPDATE;
			break;
        }
		uiReg++;
    }
}

static void Delayms(uint16_t ucMs)
{
	osDelay(pdMS_TO_TICKS(ucMs));
}

static void AutoScanSensor(void)
{
	int i, iRetry;

	for(i = 0; i < 0x7F; i++)
	{
		WitInit(WIT_PROTOCOL_I2C, i);
		iRetry = 2;
		do
		{
			s_cDataUpdate = 0;
			WitReadReg(AX, 3);
			Delayms(5);
			if(s_cDataUpdate != 0)
			{
				printf("find %02X addr sensor\r\n", i);
				return ;
			}
			iRetry--;
		}while(iRetry);
	}
	printf("can not find sensor\r\n");
	printf("please check your connection\r\n");
}