/**
* Read from the device
*
* This function is called when the device is read in real-time context.
*
*/
static ssize_t mpu9150_rtdm_read_rt(struct rtdm_dev_context *context,
rtdm_user_info_t * user_info, void *buf,
size_t nbyte)
{
int ret;
if( mpu9150_device )
{
/* take the semaphore */
rtdm_sem_down( &sem );
mpu9150_read_accel();
mpu9150_read_gyro();
mpu9150_read_mag();
mpu9150_read_temp();
ret = rtdm_safe_copy_to_user(user_info, buf, buffer.data.alldata, DATA_BUFFER_SIZE);
/* if an error has occurred, send it to user */
if( ret )
return ret;
/* release the semaphore */
rtdm_sem_up( &sem );
}
else
return 0;
return DATA_BUFFER_SIZE;
}
int mpu9150_read(mpudata_t *mpu)
{
if (mpu9150_read_dmp(mpu) != 0)
return -1;
if (mpu9150_read_mag(mpu) != 0)
return -1;
calibrate_data(mpu);
return data_fusion(mpu);
}
int mpu9150_read(mpudata_t *mpu)
{
if (mpu9150_read_dmp(mpu) != 0)
return -1;
#ifdef AK89xx_SECONDARY
if (mpu9150_read_mag(mpu) != 0)
return -1;
#endif
calibrate_data(mpu);
return data_fusion(mpu);
}
int mpu9150_read(mpudata_t *mpu)
{
//printk("mpu9150_read...");
int _start = jiffies;
if (mpu9150_read_dmp(mpu) != 0)
return -1;
if (mpu9150_read_mag(mpu) != 0)
return -1;
//calibrate_data(mpu);
//printk("Time for read mpu: %d\n", jiffies_to_msecs(jiffies-_start));
return 0; //data_fusion(mpu);
}
int main (int argc, char **argv) {
t_mpu9150 accel;
mpu9150_open(&accel);
mpu9150_init(&accel);
mpu9150_open_mag(&accel);
mpu9150_init_mag(&accel);
mpu9150_start_mag(&accel);
usleep(10000);
while(1) {
mpu9150_read_data(&accel);
mpu9150_read_mag(&accel);
printf("\r\t%f\t%f\t%f\t|\t%f\t%f\t%f\t|\t%f\t%f\t%f\t",accel.acc[0],accel.acc[1],accel.acc[2],accel.gyr[0],accel.gyr[1],accel.gyr[2],accel.mag[0],accel.mag[1],accel.mag[2]);
usleep(10000);
}
return 0;
}
void read_loop(unsigned int sample_rate)
{
int i, change;
unsigned long loop_delay;
mpudata_t mpu;
if (sample_rate == 0)
return;
memset(&mpu, 0, sizeof(mpudata_t));
for (i = 0; i < 3; i++) {
minVal[i] = 0x7fff;
maxVal[i] = 0x8000;
}
loop_delay = (1000 / sample_rate) - 2;
printf("\nEntering read loop (ctrl-c to exit)\n\n");
linux_delay_ms(loop_delay);
while (!done) {
change = 0;
if (mag_mode) {
if (mpu9150_read_mag(&mpu) == 0) {
for (i = 0; i < 3; i++) {
if (mpu.rawMag[i] < minVal[i]) {
minVal[i] = mpu.rawMag[i];
change = 1;
}
if (mpu.rawMag[i] > maxVal[i]) {
maxVal[i] = mpu.rawMag[i];
change = 1;
}
}
}
}
else {
if (mpu9150_read_dmp(&mpu) == 0) {
for (i = 0; i < 3; i++) {
if (mpu.rawAccel[i] < minVal[i]) {
minVal[i] = mpu.rawAccel[i];
change = 1;
}
if (mpu.rawAccel[i] > maxVal[i]) {
maxVal[i] = mpu.rawAccel[i];
change = 1;
}
}
}
}
if (change) {
if (mag_mode)
print_mag(&mpu);
else
print_accel(&mpu);
}
linux_delay_ms(loop_delay);
}
printf("\n\n");
}
