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; }
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++; } } }