Demo entry 6688435

C

   

Submitted by anonymous on Jan 03, 2018 at 09:27
Language: C. Code size: 22.3 kB.

#include <stdio.h>  
#include "stm32f10x.h"  
#include "stm32f10x_it.h"  
  
#include "./systick/bsp_SysTick.h"  
  
#include "./led/bsp_led.h"  
#include "./usart/bsp_usart.h"  
#include "./i2c/bsp_i2c.h"  
#include "./exti/bsp_exti.h"  
#include "stm32f10x_usart.h"  
#include "./usart/USART2.h"  
#include "./nrf24L01/exti.h"  
#include "./timer2/timer2.h"  
#include "./nrf24L01/spi.h"  
#include "./nrf24L01/nrf24l01p.h"  
#include "delay.h"  
  
#include "mltypes.h"  
#include "inv_mpu_dmp_motion_driver.h"  
#include "log.h"  
#include "invensense.h"  
#include "invensense_adv.h"  
#include "eMPL_outputs.h"  
#include "mpu.h"  
#include "inv_mpu.h"  
#include "log.h"  
#include "packet.h"  
#include "MadgwickAHRS.h"  
#include "eMPL_outputs.h"  
  
  
  
  
#define COMPASS_ENABLED  
#define MPU9250  
/* Data read from MPL. */  
#define PRINT_ACCEL     (0x01)  
#define PRINT_GYRO      (0x02)  
#define PRINT_QUAT      (0x04)  
#define PRINT_COMPASS   (0x08)  
#define PRINT_EULER     (0x10)  
#define PRINT_ROT_MAT   (0x20)  
#define PRINT_HEADING   (0x40)  
#define PRINT_PEDO      (0x80)  
#define PRINT_LINEAR_ACCEL (0x100)  
#define PRINT_GRAVITY_VECTOR (0x200)  
  
#define DEFAULT_MPU_HZ (100)  
#define COMPASS_READ_MS (20)  
#define MAIN_FOR_MY_OWN 1  
  
#define ACCEL_ON        (0x01)  
#define GYRO_ON         (0x02)  
#define COMPASS_ON      (0x04)  
#define TEMP_READ_MS    (500)  
#define PEDO_READ_MS    (1000)  
  
unsigned char *mpl_key = (unsigned char*)"eMPL 5.1";  
struct rx_s {  
    unsigned char header[3];  
    unsigned char cmd;  
};  
  
struct hal_s {  
    unsigned char lp_accel_mode;  
    unsigned char sensors;  
    unsigned char dmp_on;  
    unsigned char wait_for_tap;  
    volatile unsigned char new_gyro;  
    unsigned char motion_int_mode;  
    unsigned long no_dmp_hz;  
    unsigned long next_pedo_ms;  
    unsigned long next_temp_ms;  
    unsigned long next_compass_ms;  
    unsigned int report;  
    unsigned short dmp_features;  
    struct rx_s rx;  
};  
static struct hal_s hal = {0};  
  
/* Platform-specific information. Kinda like a boardfile. */  
struct platform_data_s {  
    signed char orientation[9];  
};  
  
/* The sensors can be mounted onto the board in any orientation. The mounting 
 * matrix seen below tells the MPL how to rotate the raw data from the 
 * driver(s). 
 * TODO: The following matrices refer to the configuration on internal test 
 * boards at Invensense. If needed, please modify the matrices to match the 
 * chip-to-body matrix for your particular set up. 
 */  
static struct platform_data_s gyro_pdata = {  
  
         .orientation = { 1, 0, 0,  
                     0, 1, 0,  
                     0, 0, 1}  
};  
  
static struct platform_data_s compass_pdata = {  
    .orientation = { 0, 1, 0,  
                     1, 0, 0,  
                     0, 0, -1}  
};  
  
int new_gyro = 0;  
void gyro_data_ready_cb(void)  
{  
    new_gyro = 1;  
    hal.new_gyro = 1;  
}  
  
static void read_from_mpl() {  
    long data[9];  
    int8_t accuracy;  
    unsigned long timestamp;  
    float roll,pitch,yaw;  
  
    if (inv_get_sensor_type_euler(data, &accuracy,(inv_time_t*)×tamp))  
    {  
        pitch =data[0]*1.0/(1<<16) ;  
        roll = data[1]*1.0/(1<<16);  
        yaw = data[2]*1.0/(1<<16);  
  
        printf("%6.2f, %6.2f, %6.2f", roll, pitch, yaw);  
    }  
}  
  
  
static inline void run_self_test(void)  
{  
    int result;  
    long gyro[3], accel[3];  
    result = mpu_run_6500_self_test(gyro, accel, 0);  
    if (result == 0x7) {  
    MPL_LOGI("Passed!\n");  
        MPL_LOGI("accel: %7.4f %7.4f %7.4f\n",  
                    accel[0]/65536.f,  
                    accel[1]/65536.f,  
                    accel[2]/65536.f);  
        MPL_LOGI("gyro: %7.4f %7.4f %7.4f\n",  
                    gyro[0]/65536.f,  
                    gyro[1]/65536.f,  
                    gyro[2]/65536.f);  
        /* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/  
  
#ifdef USE_CAL_HW_REGISTERS  
        /* 
         * This portion of the code uses the HW offset registers that are in the MPUxxxx devices 
         * instead of pushing the cal data to the MPL software library 
         */  
        unsigned char i = 0;  
  
        for(i = 0; i<3; i++) {  
            gyro[i] = (long)(gyro[i] * 32.8f); //convert to +-1000dps  
            accel[i] *= 2048.f; //convert to +-16G  
            accel[i] = accel[i] >> 16;  
            gyro[i] = (long)(gyro[i] >> 16);  
        }  
  
        mpu_set_gyro_bias_reg(gyro);  
  
        mpu_set_accel_bias_6500_reg(accel);  
#else  
        /* Push the calibrated data to the MPL library. 
         * 
         * MPL expects biases in hardware units << 16, but self test returns 
         * biases in g's << 16. 
         */  
        unsigned short accel_sens;  
        float gyro_sens;  
  
        mpu_get_accel_sens(&accel_sens);  
        accel[0] *= accel_sens;  
        accel[1] *= accel_sens;  
        accel[2] *= accel_sens;  
        inv_set_accel_bias(accel, 3);  
        mpu_get_gyro_sens(&gyro_sens);  
        gyro[0] = (long) (gyro[0] * gyro_sens);  
        gyro[1] = (long) (gyro[1] * gyro_sens);  
        gyro[2] = (long) (gyro[2] * gyro_sens);  
        inv_set_gyro_bias(gyro, 3);  
#endif  
    }  
    else {  
            if (!(result & 0x1))  
                MPL_LOGE("Gyro failed.\n");  
            if (!(result & 0x2))  
                MPL_LOGE("Accel failed.\n");  
            if (!(result & 0x4))  
                MPL_LOGE("Compass failed.\n");  
     }  
  
}  
  
  
  
  
#define q30  1073741824.0f  
#define q16  65536.0f  
  
static signed char gyro_orientation[9] = { 1, 0, 0,  
                                           0, 1, 0,  
                                           0, 0, 1};  
  
static signed char comp_orientation[9] = { 0, 1, 0,  
                                           1, 0, 0,  
                                           0, 0,-1};  
  
u8 run_self_test_1(void)  
{  
    int result;  
    //char test_packet[4] = {0};  
    long gyro[3], accel[3];  
    result = mpu_run_6500_self_test(gyro, accel,0);  
    if (result == 0x7)  
    {  
        /* Test passed. We can trust the gyro data here, so let's push it down 
        * to the DMP. 
        */  
        unsigned short accel_sens;  
        float gyro_sens;  
  
        mpu_get_gyro_sens(&gyro_sens);  
        gyro[0] = (long)(gyro[0] * gyro_sens);  
        gyro[1] = (long)(gyro[1] * gyro_sens);  
        gyro[2] = (long)(gyro[2] * gyro_sens);  
        //inv_set_gyro_bias(gyro, 3);  
        dmp_set_gyro_bias(gyro);  
        mpu_get_accel_sens(&accel_sens);  
        accel[0] *= accel_sens;  
        accel[1] *= accel_sens;  
        accel[2] *= accel_sens;  
       // inv_set_accel_bias(accel, 3);  
        dmp_set_accel_bias(accel);  
        return 0;  
    }else return 1;  
}  
  
unsigned short inv_row_2_scale(const signed char *row)  
{  
    unsigned short b;  
  
    if (row[0] > 0)  
        b = 0;  
    else if (row[0] < 0)  
        b = 4;  
    else if (row[1] > 0)  
        b = 1;  
    else if (row[1] < 0)  
        b = 5;  
    else if (row[2] > 0)  
        b = 2;  
    else if (row[2] < 0)  
        b = 6;  
    else  
        b = 7;      // error  
    return b;  
}  
  
void mget_ms(unsigned long *time)  
{  
    //*time=(unsigned long)HAL_GetTick();  
}  
  
  
u8 mpu_dmp_init(void)  
{  
    u8 res=0;  
    struct int_param_s int_param;  
    unsigned char accel_fsr;  
    unsigned short gyro_rate, gyro_fsr;  
    unsigned short compass_fsr;  
  
    if(mpu_init(&int_param)==0)  
    {  
        res=inv_init_mpl();  
        if(res)return 1;  
        inv_enable_quaternion();  
        inv_enable_9x_sensor_fusion();  
        inv_enable_fast_nomot();  
        inv_enable_gyro_tc();  
        inv_enable_vector_compass_cal();  
        inv_enable_magnetic_disturbance();  
  
  
        inv_enable_eMPL_outputs();  
  
        res=inv_start_mpl();  
        if(res)return 1;  
        res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL|INV_XYZ_COMPASS);  
        if(res)return 2;  
        res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);  
        if(res)return 3;  
        res=mpu_set_sample_rate(DEFAULT_MPU_HZ);  
        if(res)return 4;  
        res=mpu_set_compass_sample_rate(1000/COMPASS_READ_MS);  
        if(res)return 5;  
        mpu_get_sample_rate(&gyro_rate);  
        mpu_get_gyro_fsr(&gyro_fsr);  
        mpu_get_accel_fsr(&accel_fsr);  
        mpu_get_compass_fsr(&compass_fsr);  
        inv_set_gyro_sample_rate(1000000L/gyro_rate);  
        inv_set_accel_sample_rate(1000000L/gyro_rate);  
        inv_set_compass_sample_rate(COMPASS_READ_MS*1000L);  
        inv_set_gyro_orientation_and_scale(  
        inv_orientation_matrix_to_scalar(gyro_orientation),(long)gyro_fsr<<15);  
        inv_set_accel_orientation_and_scale(  
        inv_orientation_matrix_to_scalar(gyro_orientation),(long)accel_fsr<<15);  
        inv_set_compass_orientation_and_scale(  
        inv_orientation_matrix_to_scalar(comp_orientation),(long)compass_fsr<<15);  
  
        res=dmp_load_motion_driver_firmware();  
        if(res)return 6;  
  
        res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));  
        if(res)return 7;  
        res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|  
            DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|  
            DMP_FEATURE_GYRO_CAL);  
        if(res)return 8;  
        res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);  
        if(res)return 9;  
        res=run_self_test_1();  
//      if(res)return 10;  
        res=mpu_set_dmp_state(1);  
        if(res)return 11;  
    }  
    return 0;  
}  
  
u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)  
{  
    float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;  
    unsigned long sensor_timestamp;  
    short gyro[3], accel[3], sensors;  
    unsigned char more;  
    long quat[4];  
    if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;  
    /* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.  
     * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.  
    **/  
    /*if (sensors & INV_XYZ_GYRO ) 
    send_packet(PACKET_TYPE_GYRO, gyro); 
    if (sensors & INV_XYZ_ACCEL) 
    send_packet(PACKET_TYPE_ACCEL, accel); */  
    /* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30. 
     * The orientation is set by the scalar passed to dmp_set_orientation during initialization. 
    **/  
    if(sensors&INV_WXYZ_QUAT)  
    {  
        q0 = quat[0] / q30;  
        q1 = quat[1] / q30;  
        q2 = quat[2] / q30;  
        q3 = quat[3] / q30;  
  
        *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch  
        *roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll  
        *yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;   //yaw  
    }else return 2;  
    return 0;  
}  
  
u8 mpu_mpl_get_data(float *pitch,float *roll,float *yaw)  
{  
    unsigned long sensor_timestamp,timestamp;  
    short gyro[3], accel_short[3],compass_short[3],sensors;  
    unsigned char more;  
    long compass[3],accel[3],quat[4],temperature;  
    long data[9];  
    int8_t accuracy;  
  
//  if(dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors,&more)){  
//      printf("%d\r\n",dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors,&more));  
//      return 1;  
//  
//  }  
      while(dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors,&more)){  
      };  
  
    if(sensors&INV_XYZ_GYRO)  
    {  
        inv_build_gyro(gyro,sensor_timestamp);          //°ÑÐÂÊý¾Ý·¢Ë͸øMPL  
        mpu_get_temperature(&temperature,&sensor_timestamp);  
        inv_build_temp(temperature,sensor_timestamp);   //°ÑζÈÖµ·¢¸øMPL£¬Ö»ÓÐÍÓÂÝÒÇÐèҪζÈÖµ  
    }  
  
    if(sensors&INV_XYZ_ACCEL)  
    {  
        accel[0] = (long)accel_short[0];  
        accel[1] = (long)accel_short[1];  
        accel[2] = (long)accel_short[2];  
        inv_build_accel(accel,0,sensor_timestamp);      //°Ñ¼ÓËÙ¶ÈÖµ·¢¸øMPL  
    }  
  
    if (!mpu_get_compass_reg(compass_short, &sensor_timestamp))  
    {  
        compass[0]=(long)compass_short[0];  
        compass[1]=(long)compass_short[1];  
        compass[2]=(long)compass_short[2];  
        inv_build_compass(compass,0,sensor_timestamp); //°Ñ´ÅÁ¦¼ÆÖµ·¢¸øMPL  
    }  
    inv_execute_on_data();  
    inv_get_sensor_type_euler(data,&accuracy,×tamp);  
  
    *roll  = (data[0]/q16);  
    *pitch = -(data[1]/q16);  
    *yaw   = -data[2] / q16;  
    return 0;  
}  
  
  
float dmpinvSqrt(float x)   
{  
    float halfx = 0.5f * x;  
    float y = x;  
    long i = *(long*)&y;  
    i = 0x5f3759df - (i >> 1);  
    y = *(float*)&i;  
    y = y * (1.5f - (halfx * y * y));  
    return y;  
}  
  
float dmpsafe_asin(float v)  
{  
    if(isnan(v))   
    {  
        return 0.0;  
    }  
    if(v >= 1.0)  
    {  
        return M_PI / 2;  
    }  
    if(v <= -1.0)   
    {  
        return -M_PI / 2;  
    }  
    return asin(v);  
}  
  
float roll_result, yaw_result, pitch_result;  
float q_result[4];  
int8_t accuracy_result;  
#define BYTE0(dwTemp)       (*(char *)(&dwTemp))  
#define BYTE1(dwTemp)       (*((char *)(&dwTemp) + 1))  
#define BYTE2(dwTemp)       (*((char *)(&dwTemp) + 2))  
#define BYTE3(dwTemp)       (*((char *)(&dwTemp) + 3))  
  
void Data_Send_Quat(u8 *data, float *q, int8_t accuracy)  
{  
    unsigned char i=0;  
    unsigned char _cnt=0,sum = 0;  
    unsigned int _temp;  
  
    rfTxBufCh5[_cnt++]=FRAME_HEAD1;  
    rfTxBufCh5[_cnt++]=FRAME_HEAD2;  
  
    rfTxBufCh5[_cnt++]=data[2];  
    rfTxBufCh5[_cnt++]=data[3];  
    rfTxBufCh5[_cnt++]=data[4];  
    rfTxBufCh5[_cnt++]=data[5];  
    rfTxBufCh5[_cnt++]=data[6];  
    rfTxBufCh5[_cnt++]=data[7];  
    rfTxBufCh5[_cnt++]=data[8];  
    rfTxBufCh5[_cnt++]=data[9];  
    rfTxBufCh5[_cnt++]=data[10];  
  
    _temp = (int)(q[0]*10000);  
    rfTxBufCh5[_cnt++]=BYTE0(_temp);  
    rfTxBufCh5[_cnt++]=BYTE1(_temp);  
    _temp = (int)(q[1]*10000);  
    rfTxBufCh5[_cnt++]=BYTE0(_temp);  
    rfTxBufCh5[_cnt++]=BYTE1(_temp);  
    _temp = (int)(q[2]*10000);  
    rfTxBufCh5[_cnt++]=BYTE0(_temp);  
    rfTxBufCh5[_cnt++]=BYTE1(_temp);  
    _temp = (int)(q[3]*10000);  
    rfTxBufCh5[_cnt++]=BYTE0(_temp);  
    rfTxBufCh5[_cnt++]=BYTE1(_temp);  
    rfTxBufCh5[_cnt++]=accuracy;  
      
    //ºÍУÑé  
    for(i=0;i<_cnt;i++)  
        sum+= rfTxBufCh5[i];  
    rfTxBufCh5[_cnt++]=sum;  
  
    rfTxBufCh5[_cnt++]=FRAME_TAIL;  
       
}  
  
int main()   
{  
    unsigned char accel_fsr = 0;  
    unsigned short gyro_rate = 0, gyro_fsr = 0;  
    unsigned long timestamp;  
    unsigned long next_compass_ms = 0;  
    unsigned short compass_fsr;  
    unsigned char sensors, more;  
    int  has_run_test = 0;  
    int new_data = 0;  
    //变量初始�  
    int result = 0;  
    int new_compass = 0;  
    struct int_param_s int_param;  
  
    short   compass[3] = {0},  
            accel[3] = {0},  
            gyro[3] = {0};  
  
    long    compass_l[3] = {0},  
            accel_l[3] = {0};  
  
    long data[9] = {0};  
    int8_t accuracy;  
  
  
    //系统时钟初始�  
    SysTick_Init();  
    SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk;  
//    DELAY_Init();  
    //LED 初始�  
    LED_GPIO_Config();  
    LED_BLUE;  
  
    //串口初始�  
    USART_Config();  
    Initial_UART2(115200);  
    init_usart2_fifo();  
    EXTI_Pxy_Config();  
      
    rfRxChG=5;  
      
    SPI1_Init();  
    SPI1_SetSpeed(SPI_BaudRatePrescaler_8);  
    RF_initF(rfRxChG);    
    EXTI2_Init();  
//  TIM2_IntInit(999,71);  //1ms  
  
  
    //I2C总线初始�  
    I2C_Bus_Init();  
  
    printf("mpu 9250 start...");  
  
    result = mpu_init(&int_param);  
    if (result) {  
        LED_RED;  
        printf("mpu_init failed: errno = %d", result);  
    }  
    else  
        LED_GREEN;  
  
  
    result = inv_init_mpl();  
  
    /*can't understand*/  
    inv_enable_quaternion();  
    inv_enable_9x_sensor_fusion();  
  
    /*在未运动时更新陀螺仪偏移*/  
    inv_enable_fast_nomot();  
  
    /*在温度变化时更新陀螺仪偏差*/  
    inv_enable_gyro_tc();  
  
  
    inv_enable_vector_compass_cal();  
    inv_enable_magnetic_disturbance();  
    //inv_start_vector_compass_cal();  
      
    inv_enable_eMPL_outputs();  
    result = inv_start_mpl();  
    if (result == INV_ERROR_NOT_AUTHORIZED)  
        printf("Not authorized\n");  
    if (result)  
        printf("Could not start the MPL.\n");  
  
  
    /*å”¤é†’æ‰€æœ‰ä¼ æ„Ÿå™¨*/  
    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);  
  
    /*æŽ¨é€æ‰€æœ‰é™€èžºä»ªå’ŒåŠ é€Ÿåº¦è®¡æ•°æ®åˆ°FIFO*/  
    mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);  
    //设置mpu6500é‡‡æ ·é¢‘çŽ‡  
    mpu_set_sample_rate(DEFAULT_MPU_HZ);  
    //è®¾ç½®ç£åŠ›è®¡é‡‡æ ·é¢‘çŽ?  
    mpu_set_compass_sample_rate(1000/COMPASS_READ_MS);  
  
    mpu_get_sample_rate(&gyro_rate);  
    mpu_get_gyro_fsr(&gyro_fsr);  
    mpu_get_accel_fsr(&accel_fsr);  
    mpu_get_compass_fsr(&compass_fsr);  
  
    inv_set_gyro_sample_rate(1000000L / gyro_rate);  
    inv_set_accel_sample_rate(1000000L / gyro_rate);  
    inv_set_compass_sample_rate(COMPASS_READ_MS * 1000L);  
  
    inv_set_gyro_orientation_and_scale(  
          inv_orientation_matrix_to_scalar(gyro_pdata.orientation),  
          (long)gyro_fsr<<15);  
    inv_set_accel_orientation_and_scale(  
          inv_orientation_matrix_to_scalar(gyro_pdata.orientation),  
          (long)accel_fsr<<15);  
    inv_set_compass_orientation_and_scale(  
          inv_orientation_matrix_to_scalar(compass_pdata.orientation),  
          (long)compass_fsr<<15);  
    while (1) {  
        if (!has_run_test) {  
            run_self_test();  
            has_run_test = 1;  
        }  
        usart2_data_handler();  
        get_tick_count(×tamp);  
        if (timestamp / 100 > next_compass_ms) {  
            next_compass_ms = timestamp / 100 + COMPASS_READ_MS;  
            new_compass = 1;  
        }  
  
        if (new_gyro) {  
            new_gyro = 0;  
            if (!mpu_read_fifo(gyro, accel, ×tamp, &sensors, &more)){  
                if (more)  
                new_gyro = 1;  
  
                if (sensors & INV_XYZ_GYRO) {  
                    new_data = 1;  
                    inv_build_gyro(gyro, timestamp);  
                }  
  
                if (sensors & INV_XYZ_ACCEL) {  
                    new_data = 1;  
  
                    accel_l[0] = (long)accel[0];  
                    accel_l[1] = (long)accel[1];  
                    accel_l[2] = (long)accel[2];  
                    inv_build_accel(accel_l, 0, timestamp);  
  
                }  
            }  
        }  
  
        //ç¬¬ä¸€ç§æ–¹æ¡ˆï¼Œå°±æ˜¯æ ¹æ®æ—¶é—´èŒƒå›´æ¥èŽ·å?  
        if (new_compass) {  
            new_compass = 0;  
            if (!mpu_get_compass_reg(compass, ×tamp)) {  
                new_data = 1;  
  
                compass_l[0] = (long)compass[0];  
                compass_l[1] = (long)compass[1];  
                compass_l[2] = (long)compass[2];  
  
                inv_build_compass(compass_l, 0, timestamp);  
            }  
            else;  
        }  
  
  
        if (new_data)   
        {  
            new_data = 0;  
            inv_execute_on_data();  
   
//            if (inv_get_sensor_type_euler(data, &accuracy,(inv_time_t*)×tamp))  
//            {  
//                pitch_result =data[0]*1.0/(1<<16) ;  
//                roll_result = data[1]*1.0/(1<<16);  
//                yaw_result = data[2]*1.0/(1<<16);  
//            }  
            if (inv_get_sensor_type_quat(data, &accuracy, (inv_time_t*)×tamp)) {  
           /* Sends a quaternion packet to the PC. Since this is used by the Python 
          * test app to visually represent a 3D quaternion, it's sent each time 
          * the MPL has new data. 
          */  
          float q[4] = {0};  
          q[0] = (float)data[0] / ((float)(1L << 30));    
          q[1] = (float)data[1] / ((float)(1L << 30));  
          q[2] = (float)data[2] / ((float)(1L << 30));  
          q[3] = (float)data[3] / ((float)(1L << 30));  
          float norm = dmpinvSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);  
          q[0] = q[0] * norm;  
          q[1] = q[1] * norm;  
          q[2] = q[2] * norm;  
          q[3] = q[3] * norm;    
          accuracy_result = accuracy;  
                
//          roll_result =(atan2(2.0 * (q[0] * q[1] + q[2] * q[3]),  
//                1 - 2.0 * (q[1] * q[1] + q[2] * q[2]))) * 180 / M_PI;  
//            
//          pitch_result = -dmpsafe_asin(2.0 * (q[0] * q[2] - q[3] * q[1])) * 180 / M_PI;  
//            
//          yaw_result = -atan2(2.0 * (q[0] * q[3] + q[1] * q[2]),  
//              1 - 2.0 * (q[2] * q[2] + q[3] * q[3])) * 180 / M_PI;  
          q_result[0] = q[0];  
          q_result[1] = q[1];  
          q_result[2] = q[2];  
          q_result[3] = q[3];  
          Data_Send_Quat(BigArmBUf, q_result, accuracy_result);  
            
      }  
        }  
    }  
}  

This snippet took 0.03 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).