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