int main(void)
{
    BOARD_Init();
    SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
    SERIAL_Init();
    SPI_Init(SPI_SS1_CLKRATE, 0, 0);
    TIMERS_Init();
    printf("program begins...\n");
    uint16_t i, j;
    /* timer test: sucess
    SET_OUTPUT_PIN(D, 10);
    TIMERS_Init();
    while (1) {
        WRITE_HIGH(D, 10);
        InitTimer(1, 100);
        while (IsTimerActive(1));
        WRITE_LOW(D, 10);
        InitTimer(1, 100);
        while (IsTimerActive(1)) ;
    }
    //*/

    /* SPI read test 1: need to get 0x70 to ensure SPI reading is good
    printf("IMU: SPI read test 1\n");
    IMU_Init();
    uint8_t id;
    while (1) {
        id = MPU6500_readOneByte( MPU6500_RA_WHO_AM_I );
        printf( "id should be 0x70: 0x%x\n", id );
        InitTimer(1, 1000);
        while (IsTimerActive(1)) ;
    }
    //*/

    /* SPI read test 2: need to get 0x70 to ensure SPI reading is good
    printf("IMU: SPI read test 2\n");
    IMU_Init();
    uint8_t id;
    while (1) {
        MPU6500_readBytes(SPI_SS2_ID, MPU6500_RA_WHO_AM_I, 1, &id);
        printf( "id should be 0x70: 0x%x\n", id );
        InitTimer(1, 1000);
        while (IsTimerActive(1)) ;
    }
    //*/

    /* inv_mpu test: porting invense IMU library
    printf("inv_mpu test: read test\n");
    uint8_t buf[3] = {0};
    uint8_t flag;
    while (1) {
        flag = mpu_read_reg(MPU6500_RA_WHO_AM_I, buf);
        printf("flag: %d\n", flag);
        printf( "id should be 0x70: 0x%x\n", buf[0]);
        InitTimer(2, 1000);
        while (IsTimerActive(2)) {
            //printf("timer is still active\n");
        }
    }
    //*/

    /* porting inv_mpu test: init and read gyro
    printf("inv_mpu test: read test\n");
    uint16_t buf[3] = {0};
    uint8_t flag = mpu_init(0);
    mpu_reset_fifo();
    mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);
    mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);
    printf("flag: %d\n", flag);

    uint16_t gyro[3], accel[3];

   // flag = mpu_run_6500_self_test(gyro, accel, 0);
    printf("flag=%d, gyro: % 5d, % 5d, % 5d", flag, gyro[0], gyro[1], gyro[2]);
    printf(", accel: % 5d, % 5d, % 5d\n", accel[0], accel[1], accel[2]);
    while (1) {
        flag = mpu_get_gyro_reg(buf, 0);
        printf("flag=%d, gyro: % 5d, % 5d, % 5d", flag, buf[0], buf[1], buf[2]);
        mpu_get_accel_reg(buf, 0);
        printf(", accel: % 5d, % 5d, % 5d\n", buf[0], buf[1], buf[2]);
        InitTimer(2, 1000);
        while (IsTimerActive(2));
    }
    //*/

    /* test: reading multiple times from the same register: the internal pointer +1 when it read once
    int vals;
    uint16_t gyro[3], accel[3];
    vals = mympu_open(200);
    printf("MPU Init: %d\n", vals);
    struct s_mympu mympu;
    uint8_t flag;
    while(1) {
        mpu_get_accel_reg(accel, 0);
        printf("all  accel: % 5d, % 5d, % 5d\n", accel[0], accel[1], accel[2]);
        uint8_t tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_XOUT_H);
        accel[0] = tmp << 8;
        tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_XOUT_L);
        accel[0] |= tmp;
        tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_YOUT_H);
        accel[1] = tmp << 8;
        tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_YOUT_L);
        accel[1] |= tmp;
        tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_ZOUT_H);
        accel[2] = tmp << 8;
        tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_ZOUT_L);
        accel[2] |= tmp;
        printf("byte accel: % 5d, % 5d, % 5d\n", accel[0], accel[1], accel[2]);

        InitTimer(2, 100);
        while (IsTimerActive(2));
    }
    //*/

    /* test: reading multiple times from the same register: the internal pointer +1 when it read once
    int vals;
    uint8_t buf[20] = {0};
    uint16_t gyro[3] = {0}, accel[3] = {0};
    vals = mympu_open(200);
    printf("MPU Init: %d\n", vals);
    //IMU_Init();
    uint8_t id = MPU6500_readOneByte( MPU6500_RA_WHO_AM_I );
    printf( "id should be 0x70: 0x%x\n", id);
    struct s_mympu mympu = {0};
    int flag;
    //MPU6500_writeOneByte( MPU6500_RA_USER_CTRL, 0x40 | 0x10 );
    while(1) {
        flag = mympu_update(&mympu);
        //printf("yaw=[%d, %d], pitch=[%d,%d], roll=[%d,%d]\n", mympu.ypr[0], mympu.gyro[0], mympu.ypr[1], mympu.gyro[1], mympu.ypr[2], mympu.gyro[2]);
        //printf("flag=%d, yaw=% 11f, pitch=% 11f, roll=% 11f\n", flag, mympu.ypr[0], mympu.ypr[1], mympu.ypr[2]);
        printf("yaw=[%.3f, %.3f], pitch=[%.3f,%.3f], roll=[%.3f,%.3f]\n",
        mympu.ypr[0], mympu.gyro[0], mympu.ypr[1], mympu.gyro[1], mympu.ypr[2], mympu.gyro[2]);
        //flag = dmp_read_fifo(gyro, accel, buf, NULL,&vals,&vals);

        //buf[0] = MPU6500_readOneByte(MPU6500_RA_FIFO_COUNTH);
        //buf[1] = MPU6500_readOneByte(MPU6500_RA_FIFO_COUNTL);
        //uint16_t count = (buf[0] << 8) | buf[1];
        //printf("fifo count: %d\n", count);

        //flag = mpu_read_fifo(gyro, accel, &vals, &vals, &vals);
        //flag = mpu_read_fifo_stream(10, buf, &vals);
        //read_fifo(buf, gyro, accel);
        //printf("flag=%d, gyro: % 5d, % 5d, % 5d", flag, gyro[0], gyro[1], gyro[2]);
       // printf(", accel: % 5d, % 5d, % 5", accel[0], accel[1], accel[2]);
        //printf(", quat: %d, %d, %d, %d\n", buf[0], buf[1], buf[2], buf[3]);

        InitTimer(2, 50);
        while (IsTimerActive(2));
    }
    //*/

    /* test: calibration by adding offset
    int vals = mympu_open(200);
    printf("MPU Init: %d\n", vals);
    uint8_t id = MPU6500_readOneByte( MPU6500_RA_WHO_AM_I );
    printf( "id should be 0x70: 0x%x\n", id);

    struct s_mympu mympu = {0};
    int flag;
    printf("waiting for initial drift\n");
    InitTimer(2, 20000); // wait for 20s to pass initial drift
    while (IsTimerActive(2));
    printf("get average offset\n");
    float offsetX = 0, offsetY = 0, offsetZ = 0;
    float offsetPrev[3] = {0};
    float alpha = 0;
    uint8_t k;
    for (k=1; k <= 200; k++) {
        flag = mympu_update(&mympu);
        //printf("yaw=% 3.3f, pitch=% 3.3f, roll=% 3.3f\n",
        //mympu.ypr[0], mympu.ypr[1], mympu.ypr[2]);
        alpha = ((float) k - 1.) / k;
        offsetX = alpha*offsetPrev[0] + (1. - alpha) * mympu.ypr[0];
        offsetY = alpha*offsetPrev[1] + (1. - alpha) * mympu.ypr[1];
        offsetZ = alpha*offsetPrev[2] + (1. - alpha) * mympu.ypr[2];
        offsetPrev[0] = offsetX;
        offsetPrev[1] = offsetY;
        offsetPrev[2] = offsetZ;
        InitTimer(2, 50);
        while (IsTimerActive(2));
    }
    printf("average: x offset= %3.3f, y offset= %3.3f, z offset= %3.3f\n",
        offsetX, offsetY, offsetZ);
    //offsetX = abs(offsetX);
    offsetY = abs(offsetY);
    //offsetZ = abs(offsetZ);

    while(1) {
        flag = mympu_update(&mympu);
        //printf("yaw=% 3.0f, pitch=% 3.0f, roll=% 3.0f\n",
        //printf("%4.1f, %4.1f, %4.1f\n", mympu.ypr[0] - offsetX, mympu.ypr[1] - offsetY, mympu.ypr[2] - offsetZ);

        printf("yaw=[%.3f, %.3f], pitch=[%.3f,%.3f], roll=[%.3f,%.3f]\n", mympu.ypr[0] - offsetX, mympu.gyro[0],        mympu.ypr[1] - offsetY, mympu.gyro[1],        mympu.ypr[2] - offsetZ, mympu.gyro[2]);

        InitTimer(2, 50);
        while (IsTimerActive(2));
    }
    //*/


    //* test: two SPI devices, OLED and IMU
    int vals = mympu_open(10);
    OLED_Init();
    OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_1, 0x2710);
    ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2);
    printf("MPU Init: %d\n", vals);
    uint8_t id = MPU6500_readOneByte( MPU6500_RA_WHO_AM_I );
    printf( "id should be 0x70: 0x%x\n", id);

    UG_SetForecolor(0x0F);
    UG_SetBackcolor(0x00);
    UG_FontSelect(FONT_6X8);
    char str[50] = "";
    while(1) {
        //mympu_update(&mympu);
        sprintf(str, "%.3f, %.3f, %.3f\n", mympu.ypr[0], mympu.ypr[1], mympu.ypr[2]);
        printf("%s\n",str);
        flag_writingScreen = 1;
        UG_PutString(0, 0, str);
        flag_writingScreen = 0;
        InitTimer(1, 200);
        while (IsTimerActive(1));
    }
    //*/

    /*
    flag = mpu_run_6500_self_test(gyro, accel, 0);
    printf("\nflag=%d, gyro: % 5d, % 5d, % 5d", flag, gyro[0], gyro[1], gyro[2]);
    printf(", accel: % 5d, % 5d, % 5d\n", accel[0], accel[1], accel[2]);

    MPU6500_writeOneByte(MPU6500_RA_XA_OFFS_H, (accel[0] & 0x7F80) >> 8);
    MPU6500_writeOneByte(MPU6500_RA_XA_OFFS_L_TC, accel[0] & 0x7F);
    MPU6500_writeOneByte(MPU6500_RA_YA_OFFS_H, (accel[1] & 0x7F80) >> 8);
    MPU6500_writeOneByte(MPU6500_RA_YA_OFFS_L_TC, accel[1] & 0x7F);
    MPU6500_writeOneByte(MPU6500_RA_ZA_OFFS_H, (accel[2] & 0x7F80) >> 8);
    MPU6500_writeOneByte(MPU6500_RA_ZA_OFFS_L_TC, accel[2] & 0x7F);

    MPU6500_writeOneByte(MPU6500_RA_XG_OFFS_USRH, gyro[0] >> 8);
    MPU6500_writeOneByte(MPU6500_RA_XG_OFFS_USRL, gyro[0] & 0xFF);
    MPU6500_writeOneByte(MPU6500_RA_YG_OFFS_USRH, gyro[1] >> 8);
    MPU6500_writeOneByte(MPU6500_RA_YG_OFFS_USRL, gyro[1] & 0xFF);
    MPU6500_writeOneByte(MPU6500_RA_ZG_OFFS_USRH, gyro[2] >> 8);
    MPU6500_writeOneByte(MPU6500_RA_ZG_OFFS_USRL, gyro[2] & 0xFF);
    //*/
    printf("program ended...\n");
    return 0;
}
Exemplo n.º 2
0
void CScopeWindow::UpdateChart (void)
{
	UG_AREA Area;
	UG_WindowGetArea (&m_Window, &Area);
	Area.xs += MARGIN;
	Area.xe -= MARGIN;
	Area.ys += MARGIN+20+10;	// consider title height and margin below
	Area.ye -= MARGIN;

	UG_FillFrame (Area.xs, Area.ys, Area.xe, Area.ye, C_BLACK);

	if (m_pTimeLine == 0)
	{
		return;
	}

	// display headline

	double fZoomFactor = m_pTimeLine->GetZoomFactor ();
	CString Zoom;
	if (fZoomFactor >= 1.0)
	{
		Zoom.Format ("%.0f", fZoomFactor);
	}
	else
	{
		Zoom.Format ("1/%.0f", 1.0 / fZoomFactor);
	}

	double fSampleRate = m_pTimeLine->GetSampleRate ();
	CString SampleRate;
	if (fSampleRate < 1000000.0)
	{
		SampleRate.Format ("%.0fKHz", fSampleRate / 1000.0);
	}
	else
	{
		SampleRate.Format ("%.3fMHz", fSampleRate / 1000000.0);
	}

	CString String;
	String.Format ("SC: %.0fus     OF: %.1fus     TO: %.0fus     ZO: x%s     FQ: %s",
		       m_pTimeLine->GetScaleDivision () * 1000000.0,
		       m_pTimeLine->GetWindowLeft () * 1000000.0,
		       m_pTimeLine->GetRuntime () * 1000000.0,
		       (const char *) Zoom,
		       (const char *) SampleRate);

	UG_SetBackcolor (C_BLACK);
	UG_SetForecolor (C_WHITE);
	UG_FontSelect (&FONT_6X8);
	UG_PutString (Area.xs+5, Area.ys+1, (char *) (const char *) String);

	Area.ys += 12;
	UG_DrawLine (Area.xs, Area.ys, Area.xe, Area.ys, C_WHITE);

	// draw scale

	DrawScale (Area);

	// count active channels

	unsigned nChannelCount = 0;
	for (unsigned nChannel = 1; nChannel <= CHANS; nChannel++)
	{
		if (m_nChannelEnable & CH (nChannel))
		{
			nChannelCount++;
		}
	}

	if (nChannelCount == 0)
	{
		return;
	}

	if (nChannelCount == 1)
	{
		Area.ys += 100;
		Area.ye -= 100;
	}

	// draw channels

	UG_S16 sChannelHeight = (Area.ye-Area.ys+1) / nChannelCount;

	unsigned nChannelsDrawn = 0;
	for (unsigned nChannel = 1; nChannel <= CHANS; nChannel++)
	{
		if (m_nChannelEnable & CH (nChannel))
		{
			UG_AREA ChannelArea;
			ChannelArea.xs = Area.xs;
			ChannelArea.xe = Area.xe;
			ChannelArea.ys = Area.ys + sChannelHeight*nChannelsDrawn;
			ChannelArea.ye = ChannelArea.ys + sChannelHeight-1;

			DrawChannel (nChannel, ChannelArea);

			nChannelsDrawn++;
		}
	}
}