1,1,1 // MAGX, MAGY, MAGZ,
};
-#if 0
-
-double read_adc(uint8_t index)
-{
- int16_t value;
-
- switch(index) {
- case 0:
- itg3200_read_axe(0, &value);
- return (double) (SENSOR_SIGN[index] * value);
- case 1:
- itg3200_read_axe(1, &value);
- return (double) (SENSOR_SIGN[index] * value);
- case 2:
- itg3200_read_axe(2, &value);
- return (double) (SENSOR_SIGN[index] * value);
- case 3:
- bma150_read_axe(0, &value);
- return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
- case 4:
- bma150_read_axe(1, &value);
- return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
- case 5:
- bma150_read_axe(2, &value);
- return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
- }
- return 0.0;
-}
-
-
-uint8_t measure_time = 0;
-void read_sensors(void)
-{
- uint8_t flags;
- uint8_t err;
- uint16_t axes[3];
-
- /* read mag */
- measure_time ++;//= (measure_time +1)%3;
- if (measure_time%2 == 0) {
- err = ak8500_start_measure();
- if (err) {
- printf("mag start err %X\r\n", err);
- measure_time = 0;
- }
- }
- else if (measure_time%2 == 1) {
- err = ak8500_read_all_axes(&axes);
- if (err == 0) {
- /*
- Magnet_Vector[0] = (double)SENSOR_SIGN[6] * (double)axes[0];
- Magnet_Vector[1] = (double)SENSOR_SIGN[7] * (double)axes[1];
- Magnet_Vector[2] = (double)SENSOR_SIGN[8] * (double)axes[2];
- */
- /*
- */
- mag_x = SENSOR_SIGN[6] * axes[0];
- mag_y = SENSOR_SIGN[7] * axes[1];
- mag_z = SENSOR_SIGN[8] * axes[2];
- Magnet_Vector[0] = mag_x;
- Magnet_Vector[1] = mag_y;
- Magnet_Vector[2] = mag_z;
-
- }
- else {
- printf("mag read err %X\r\n", err);
- }
- }
- /*
- printf("%d %d %d\r\n",
- mag_x,
- mag_y,
- mag_z
- );
- */
- /*
- printf("%f %f %f\r\n",
- Magnet_Vector[0],
- Magnet_Vector[1],
- Magnet_Vector[2]);
- */
- /*
- Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
- Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch
- Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw
- */
- Gyro_Vector[0]=ToRad(read_adc(0)); //gyro x roll
- Gyro_Vector[1]=ToRad(read_adc(1)); //gyro y pitch
- Gyro_Vector[2]=ToRad(read_adc(2)); //gyro Z yaw
-
- Accel_Vector[0]=9.81 * read_adc(3); // acc x
- Accel_Vector[1]=9.81 * read_adc(4); // acc y
- Accel_Vector[2]=9.81 * read_adc(5); // acc z
-
-}
-
-#endif
-
-
void quaternion2euler(void)
{
/*