static int gConfigure(int gFile)
{
CHECK(ITG3200_Init(gFile, ITG3200_ID, true));
struct ITG3200_Acquisition confAcquisition = {
.lowPassFilter = ITG3200_LOWPASSFILTER_42,
.sampleRateDivider = 0,
};
CHECK(ITG3200_ConfigureAcquisition(gFile, &confAcquisition));
struct ITG3200_Power confPower = {
.clockSource = ITG3200_CLOCKSOURCE_PLL_X,
};
CHECK(ITG3200_ConfigurePower(gFile, &confPower));
return 0;
}
int main(void)
{
int adapter;
char filename[20];
for (adapter = 0; adapter < MAX_ADAPTERS; ++adapter)
{
snprintf(filename, 20, "/dev/i2c-%d", adapter);
if (!access(filename, R_OK | W_OK))
break;
}
if (adapter == MAX_ADAPTERS)
{
fprintf(stderr, "No I2C adapter found\n");
return -1;
}
aFile = open(filename, O_RDWR);
if (aConfigure(aFile))
{
fprintf(stderr, "Failed to initialize accelerometer\n");
return closeAndExit(-1);
}
cFile = open(filename, O_RDWR);
if (cConfigure(cFile))
{
fprintf(stderr, "Failed to initialize compass\n");
return closeAndExit(-1);
}
gFile = open(filename, O_RDWR);
if (gConfigure(gFile))
{
fprintf(stderr, "Failed to initialize gyroscope\n");
return closeAndExit(-1);
}
ADXL345_ReadData(aFile, &aX.data, &aY.data, &aZ.data);
HMC5883L_ReadData(cFile, &cX.data, &cY.data, &cZ.data);
ITG3200_ReadData(gFile, &gX.data, &gY.data, &gZ.data);
ITG3200_ReadTemperature(gFile, &gT.data);
initCoordinate(&aX);
initCoordinate(&aY);
initCoordinate(&aZ);
initCoordinate(&cX);
initCoordinate(&cY);
initCoordinate(&cZ);
initCoordinate(&gX);
initCoordinate(&gY);
initCoordinate(&gZ);
initCoordinate(&gT);
setupHandlers();
initscr();
noecho();
curs_set(false);
running = 1;
while (running)
{
ADXL345_ReadData(aFile, &aX.data, &aY.data, &aZ.data);
HMC5883L_ReadData(cFile, &cX.data, &cY.data, &cZ.data);
ITG3200_ReadData(gFile, &gX.data, &gY.data, &gZ.data);
ITG3200_ReadTemperature(gFile, &gT.data);
updateCoordinate(&aX);
updateCoordinate(&aY);
updateCoordinate(&aZ);
updateCoordinate(&cX);
updateCoordinate(&cY);
updateCoordinate(&cZ);
updateCoordinate(&gX);
updateCoordinate(&gY);
updateCoordinate(&gZ);
updateCoordinate(&gT);
clear();
mvprintw(0, 0, "%16s: %8d X %8d Y %8d Z\n", "Accelerometer", aX.data, aY.data, aZ.data);
mvprintw(1, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Smoothed", aX.smoothed, aY.smoothed, aZ.smoothed);
mvprintw(2, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Min", aX.min, aY.min, aZ.min);
mvprintw(3, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Max", aX.max, aY.max, aZ.max);
mvprintw(4, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Normalized", aX.normalized, aY.normalized, aZ.normalized);
mvprintw(6, 0, "%16s: %8d X %8d Y %8d Z\n", "Compass", cX.data, cY.data, cZ.data);
mvprintw(7, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Smoothed", cX.smoothed, cY.smoothed, cZ.smoothed);
mvprintw(8, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Min", cX.min, cY.min, cZ.min);
mvprintw(9, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Max", cX.max, cY.max, cZ.max);
mvprintw(10, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Normalized", cX.normalized, cY.normalized, cZ.normalized);
mvprintw(12, 0, "%16s: %8d X %8d Y %8d Z\n", "Gyroscope", gX.data, gY.data, gZ.data);
mvprintw(13, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Smoothed", gX.smoothed, gY.smoothed, gZ.smoothed);
mvprintw(14, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Min", gX.min, gY.min, gZ.min);
mvprintw(15, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Max", gX.max, gY.max, gZ.max);
mvprintw(16, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Normalized", gX.normalized, gY.normalized, gZ.normalized);
mvprintw(18, 0, "%16s: %8d %8.1f °C\n", "Temperature", gT.data, ITG3200_ConvertTemperature(gT.data));
mvprintw(19, 0, "%16s: %8.1f %8.1f °C\n", "Smoothed", gT.smoothed, ITG3200_ConvertTemperature(gT.smoothed));
mvprintw(20, 0, "%16s: %8.1f %8.1f °C\n", "Min", gT.min, ITG3200_ConvertTemperature(gT.min));
mvprintw(21, 0, "%16s: %8.1f %8.1f °C\n", "Max", gT.max, ITG3200_ConvertTemperature(gT.max));
mvprintw(22, 0, "%16s: %8.1f\n", "Normalized", gT.normalized);
refresh();
usleep(DELAY);
}
endwin();
close(aFile);
close(cFile);
close(gFile);
return 0;
}
void AppSampleTask(void *p_arg)
{
(void)p_arg;
/* 监测MPU6050信息 */
while(SensorCheckCnt < 5)
{
if(i2c_CheckDevice(MPU6050_SLAVE_ADDRESS) == 0)
{
bsp_InitMPU6050(); //初始化MPU6050
MPU6050Flag |= NORMAL;
}
else
{
MPU6050Flag &= (~NORMAL);
}
/* 监测HMC5883L信息 */
if (i2c_CheckDevice(HMC5883L_SLAVE_ADDRESS) == 0)
{
bsp_InitHMC5883L(); /* 初始化HMC5883L */
HMC5883LFlag |= NORMAL;
}
else
{
HMC5883LFlag &= (~NORMAL);
}
SensorCheckCnt++;
}
if(SensorCheckCnt == 5)
{
}
while(1)
{
/* 传感器采集数据 */
MPU6050_ReadData();
HMC5883L_ReadData();
MPU6050_DataDeal(); //处理MPU6050获取的数据得到：MPU6050_H结构体
HMC5883L_DataDeal(); //处理HMC5883L获取的数得到：HMC5883L_H结构体
if(HMC5883LFlag & CALI_MODE)
{
}
if(MPU6050Flag & CALI_MODE)
{
if(!(MPU6050FlagOld & CALI_MODE)) //进入校验模式
{
CaliTime = 200;
MPU6050_H.Accel_X_Offset = g_tMPU6050.Accel_X;
MPU6050_H.Accel_Y_Offset = g_tMPU6050.Accel_Y;
MPU6050_H.Accel_Z_Offset = g_tMPU6050.Accel_Z - 65536 / 4;
MPU6050_H.GYRO_X_Offset = g_tMPU6050.GYRO_X;
MPU6050_H.GYRO_Y_Offset = g_tMPU6050.GYRO_Y;
MPU6050_H.GYRO_Z_Offset = g_tMPU6050.GYRO_Z;
}
if(CaliTime == 0)
{
MPU6050Flag &= ~(CALI_MODE);
}
MPU6050_H.Accel_X_Offset = (float)(g_tMPU6050.Accel_X + MPU6050_H.Accel_X_Offset) / 2;
MPU6050_H.Accel_Y_Offset = (float)(g_tMPU6050.Accel_Y + MPU6050_H.Accel_Y_Offset) / 2;
MPU6050_H.Accel_Z_Offset = (float)(g_tMPU6050.Accel_Z + MPU6050_H.Accel_Z_Offset - 65536 / 4) / 2;
MPU6050_H.GYRO_X_Offset = (float)(g_tMPU6050.GYRO_X + MPU6050_H.GYRO_X_Offset) / 2;
MPU6050_H.GYRO_Y_Offset = (float)(g_tMPU6050.GYRO_Y + MPU6050_H.GYRO_Y_Offset) / 2;
MPU6050_H.GYRO_Z_Offset = (float)(g_tMPU6050.GYRO_Z + MPU6050_H.GYRO_Z_Offset) / 2;
CaliTime--;
if(CaliTime > 200)
CaliTime = 200;
}
if((HMC5883LFlag & NORMAL) && (MPU6050Flag & NORMAL))
{
FlyMain();
}
if(WifiStatus == CONNECTING)
{
if((WIFI_Period++) % 5000 == 0)
{
Mem_Clr(send_data_buf, sizeof(send_data_buf));
sprintf(send_data_buf, "sy:s1:d:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%f",angleAx_temp, angleAy_temp, angleAz_temp,MPU6050_H.Accel_X,MPU6050_H.Accel_Y,MPU6050_H.Accel_Z, \
MPU6050_H.GYRO_X,MPU6050_H.GYRO_Y,MPU6050_H.GYRO_Z,HMC5883L_H.X,HMC5883L_H.Y,HMC5883L_H.Z,MPU6050_H.Accel_X_Offset,MPU6050_H.Accel_Y_Offset,MPU6050_H.Accel_Z_Offset, \
MPU6050_H.GYRO_X_Offset,MPU6050_H.GYRO_Y_Offset,MPU6050_H.GYRO_Z_Offset);
ESP8266_send_data(send_data_buf);
bsp_LedToggle(2);
}
// if(CurrentGeneralReinforce != SetGeneralReinforce)
// {
// if(SetGeneralReinforce > 100) //防止增益超出范围
// SetGeneralReinforce = 100;
//
// for(index=0; index<4; index++)
// {
// PwmTemp = (uint32_t)(CurrentPwmValue[index] * SetGeneralReinforce / 100); //计算出当前的PWM实际值
// if(PwmTemp > 100) //防止PWM占空比超出范围
// PwmTemp = 100;
//
// bsp_SetPWMDutyCycle(PwmTemp, index+1);
// }
//
// CurrentGeneralReinforce = SetGeneralReinforce; //增益赋值
// }
for(index=0; index<4; index++)
{
if(CurrentPwmValue[index] != SetPwmValue[index])
{
if(SetPwmValue[index] > 100) //防止增益超出范围
SetPwmValue[index] = 100;
bsp_SetPWMDutyCycle(SetPwmValue[index], index+1);
CurrentPwmValue[index] = SetPwmValue[index];
}
// if(SetPwmDirection[index] != CurrentPwmDirection[index])
// SetPwmDirection[index] = CurrentPwmDirection[index];
}
}
MPU6050FlagOld = MPU6050Flag;
BSP_OS_TimeDlyMs(2);
}
}
int main(void)
{
/* initialize the core clock and the systick timer */
InitClock();
InitSysTick();
/* initialize the RGB led */
LED_Init();
/* Initialize UART0 */
InitUart0();
/* double rainbow all across the sky */
DoubleFlash();
/* initialize the I2C bus */
I2C_Init();
#if DATA_FUSE_MODE
/* signaling for fusion */
FusionSignal_Init();
#endif // DATA_FUSE_MODE
/* initialize UART fifos */
RingBuffer_Init(&uartInputFifo, &uartInputData, UART_RX_BUFFER_SIZE);
RingBuffer_Init(&uartOutputFifo, &uartOutputData, UART_TX_BUFFER_SIZE);
/* initialize UART0 interrupts */
Uart0_InitializeIrq(&uartInputFifo, &uartOutputFifo);
Uart0_EnableReceiveIrq();
/* initialize I2C arbiter */
InitI2CArbiter();
/* initialize the IMUs */
InitHMC5883L();
InitMPU6050();
// InitMPU6050();
#if ENABLE_MMA8451Q
InitMMA8451Q();
#endif
/* Wait for the config messages to get flushed */
//TrafficLight();
DoubleFlash();
RingBuffer_BlockWhileNotEmpty(&uartOutputFifo);
#if ENABLE_MMA8451Q
/* initialize the MMA8451Q data structure for accelerometer data fetching */
mma8451q_acc_t acc;
MMA8451Q_InitializeData(&acc);
#endif
/* initialize the MPU6050 data structure */
mpu6050_sensor_t accgyrotemp, previous_accgyrotemp;
MPU6050_InitializeData(&accgyrotemp);
MPU6050_InitializeData(&previous_accgyrotemp);
/* initialize the HMC5883L data structure */
hmc5883l_data_t compass, previous_compass;
HMC5883L_InitializeData(&compass);
HMC5883L_InitializeData(&previous_compass);
/* initialize HMC5883L reading */
uint32_t lastHMCRead = 0;
const uint32_t readHMCEvery = 1000 / 75; /* at 75Hz, data come every (1000/75Hz) ms. */
/************************************************************************/
/* Fetch scaler values */
/************************************************************************/
#if DATA_FUSE_MODE
const fix16_t mpu6050_accelerometer_scaler = mpu6050_accelerometer_get_scaler();
const fix16_t mpu6050_gyroscope_scaler = mpu6050_gyroscope_get_scaler();
const fix16_t hmc5883l_magnetometer_scaler = hmc5883l_magnetometer_get_scaler();
#endif // DATA_FUSE_MODE
/************************************************************************/
/* Prepare data fusion */
/************************************************************************/
#if DATA_FUSE_MODE
uint32_t last_transmit_time = 0;
uint32_t last_fusion_time = systemTime();
fusion_initialize();
#endif // DATA_FUSE_MODE
/************************************************************************/
/* Main loop */
/************************************************************************/
for(;;)
{
/* helper variables to track data freshness */
uint_fast8_t have_gyro_data = 0;
uint_fast8_t have_acc_data = 0;
uint_fast8_t have_mag_data = 0;
/************************************************************************/
/* Determine if sensor data fetching is required */
/************************************************************************/
/* helper variables for event processing */
int eventsProcessed = 0;
int readMPU, readHMC;
#if ENABLE_MMA8451Q
int readMMA;
#endif
/* atomic detection of fresh data */
__disable_irq();
#if ENABLE_MMA8451Q
readMMA = poll_mma8451q;
#endif
readMPU = poll_mpu6050;
poll_mma8451q = 0;
poll_mpu6050 = 0;
__enable_irq();
/* detection of HMC read */
/*
* TODO: read synchronized with MPU
*/
readHMC = 0;
uint32_t time = systemTime();
if ((time - lastHMCRead) >= readHMCEvery)
{
readHMC = 1;
lastHMCRead = time;
}
/************************************************************************/
/* Fetching MPU6050 sensor data if required */
/************************************************************************/
/* read accelerometer/gyro */
if (readMPU)
{
LED_BlueOff();
I2CArbiter_Select(MPU6050_I2CADDR);
MPU6050_ReadData(&accgyrotemp);
/* mark event as detected */
eventsProcessed = 1;
/* check for data freshness */
have_acc_data = (accgyrotemp.accel.x != previous_accgyrotemp.accel.x)
|| (accgyrotemp.accel.y != previous_accgyrotemp.accel.y)
|| (accgyrotemp.accel.z != previous_accgyrotemp.accel.z);
have_gyro_data = (accgyrotemp.gyro.x != previous_accgyrotemp.gyro.x)
|| (accgyrotemp.gyro.y != previous_accgyrotemp.gyro.y)
|| (accgyrotemp.gyro.z != previous_accgyrotemp.gyro.z);
/* loop current data --> previous data */
previous_accgyrotemp = accgyrotemp;
}
/************************************************************************/
/* Fetching HMC5883L sensor data if required */
/************************************************************************/
/* read compass data */
if (readHMC)
{
I2CArbiter_Select(HMC5883L_I2CADDR);
HMC5883L_ReadData(&compass);
/* mark event as detected */
eventsProcessed = 1;
/* check for data freshness */
have_mag_data = (compass.x != previous_compass.x)
|| (compass.y != previous_compass.y)
|| (compass.z != previous_compass.z);
/* loop current data --> previous data */
previous_compass = compass;
}
/************************************************************************/
/* Fetching MMA8451Q sensor data if required */
/************************************************************************/
#if ENABLE_MMA8451Q
/* read accelerometer */
if (readMMA)
{
LED_RedOff();
I2CArbiter_Select(MMA8451Q_I2CADDR);
MMA8451Q_ReadAcceleration14bitNoFifo(&acc);
/* mark event as detected */
eventsProcessed = 1;
}
#endif
/************************************************************************/
/* Raw sensor data output over serial */
/************************************************************************/
#if DATA_FETCH_MODE
/* data availability + sanity check
* This sent me on a long bug hunt: Sometimes the interrupt would be raised
* even if not all data registers were written. This always resulted in a
* z data register not being fully written which, in turn, resulted in
* extremely jumpy measurements.
*/
if (readMPU && accgyrotemp.status != 0)
{
/* write data */
uint8_t type = 0x02;
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)accgyrotemp.data, sizeof(accgyrotemp.data), IO_SendByte);
}
/* data availability + sanity check */
if (readHMC && (compass.status & HMC5883L_SR_RDY_MASK) != 0) /* TODO: check if not in lock state */
{
uint8_t type = 0x03;
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)compass.xyz, sizeof(compass.xyz), IO_SendByte);
}
#if ENABLE_MMA8451Q
/* data availability + sanity check */
if (readMMA && acc.status != 0)
{
uint8_t type = 0x01;
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)acc.xyz, sizeof(acc.xyz), IO_SendByte);
}
#endif
#endif // DATA_FETCH_MODE
/************************************************************************/
/* Sensor data fusion */
/************************************************************************/
#if DATA_FUSE_MODE
// if there were sensor data ...
if (eventsProcessed)
{
v3d gyro, acc, mag;
// convert, calibrate and store gyroscope data
if (have_gyro_data)
{
sensor_prepare_mpu6050_gyroscope_data(&gyro, accgyrotemp.gyro.x, accgyrotemp.gyro.y, accgyrotemp.gyro.z, mpu6050_gyroscope_scaler);
fusion_set_gyroscope_v3d(&gyro);
}
// convert, calibrate and store accelerometer data
if (have_acc_data)
{
sensor_prepare_mpu6050_accelerometer_data(&acc, accgyrotemp.accel.x, accgyrotemp.accel.y, accgyrotemp.accel.z, mpu6050_accelerometer_scaler);
fusion_set_accelerometer_v3d(&acc);
}
// convert, calibrate and store magnetometer data
if (have_mag_data)
{
sensor_prepare_hmc5883l_data(&mag, compass.x, compass.y, compass.z, hmc5883l_magnetometer_scaler);
fusion_set_magnetometer_v3d(&mag);
}
// get the time differential
const uint32_t current_time = systemTime();
const fix16_t deltaT_ms = fix16_from_int(current_time - last_fusion_time);
const fix16_t deltaT = fix16_mul(deltaT_ms, F16(0.001));
last_fusion_time = current_time;
FusionSignal_Predict();
// predict the current measurements
fusion_predict(deltaT);
FusionSignal_Update();
// correct the measurements
fusion_update(deltaT);
FusionSignal_Clear();
#if 0
fix16_t yaw, pitch, roll;
fusion_fetch_angles(&roll, &pitch, &yaw);
#if 0
float yawf = fix16_to_float(yaw),
pitchf = fix16_to_float(pitch),
rollf = fix16_to_float(roll);
IO_SendInt16((int16_t)yawf);
IO_SendInt16((int16_t)pitchf);
IO_SendInt16((int16_t)rollf);
IO_SendByteUncommited('\r');
IO_SendByte('\n');
#else
if (current_time - last_transmit_time >= 100)
{
/* write data */
uint8_t type = 42;
fix16_t buffer[3] = { roll, pitch, yaw };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
last_transmit_time = current_time;
}
#endif
#else
if (current_time - last_transmit_time >= 100)
{
/* write data */
switch (output_mode)
{
case RPY:
{
fix16_t roll, pitch, yaw;
fusion_fetch_angles(&roll, &pitch, &yaw);
/* write data */
uint8_t type = 42;
fix16_t buffer[3] = { roll, pitch, yaw };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
case QUATERNION:
{
qf16 orientation;
fusion_fetch_quaternion(&orientation);
uint8_t type = 43;
fix16_t buffer[4] = { orientation.a, orientation.b, orientation.c, orientation.d };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
case QUATERNION_RPY:
{
fix16_t roll, pitch, yaw;
fusion_fetch_angles(&roll, &pitch, &yaw);
qf16 orientation;
fusion_fetch_quaternion(&orientation);
uint8_t type = 44;
fix16_t buffer[7] = { orientation.a, orientation.b, orientation.c, orientation.d, roll, pitch, yaw };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
case SENSORS_RAW:
{
uint8_t type = 0;
fix16_t buffer[6] = { acc.x, acc.y, acc.z, mag.x, mag.y, mag.z };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
}
last_transmit_time = current_time;
}
#endif
}
#endif // DATA_FUSE_MODE
/************************************************************************/
/* Read user data input */
/************************************************************************/
/* as long as there is data in the buffer */
while(!RingBuffer_Empty(&uartInputFifo))
{
/* light one led */
LED_RedOn();
/* fetch byte */
uint8_t data = IO_ReadByte();
output_mode = (output_mode_t)data;
LED_RedOff();
#if 0
/* echo to output */
IO_SendByte(data);
/* mark event as detected */
eventsProcessed = 1;
#endif
}
/************************************************************************/
/* Save energy if you like to */
/************************************************************************/
/* in case of no events, allow a sleep */
if (!eventsProcessed)
{
/*
* Care must be taken with this instruction here, as it can lead
* to a condition where after being woken up (e.g. by the SysTick)
* and looping through, immediately before entering WFI again
* an interrupt would yield a true condition for the branches below.
* In this case this loop would be blocked until the next IRQ,
* which, in case of a 1ms SysTick timer, could be too late.
*
* To counter this behaviour, SysTick has been speed up by factor
* four (0.25ms).
*/
#if 0
__WFI();
#endif
}
}
return 0;
}
