uint32_t imu_get_euler_angle(float *adata, float *gdata, float *mdata, attitude_t *angle) { MadgwickAHRSupdate( (float)gdata[0]*PI/180, (float)gdata[1]*PI/180, (float)gdata[2]*PI/180, (float)adata[0], (float)adata[1], (float)adata[2], (float)mdata[0], (float)mdata[1], (float)mdata[2], angle); return 0; }
/* * Madgwick stream thread */ static msg_t stream_madgwick_thread(void *arg) { attitude_t attitude_data; uint16_t period = *(uint16_t *)arg; systime_t time = chTimeNow(); while (TRUE) { float mx = ((float)mag_data.x - (-440.0)) / (510 - (-440)) * 2 - 1.0; float my = ((float)mag_data.y - (-740.0)) / (380 - (-740)) * 2 - 1.0; float mz = ((float)mag_data.z - (-500.0)) / (500 - (-500)) * 2 - 1.0; MadgwickAHRSupdate((-gyro_data.x / 57.143) * 3.141592 / 180.0, (gyro_data.y / 57.143) * 3.141592 / 180.0, -(gyro_data.z / 57.143) * 3.141592 / 180.0, -acc_data.x / 1000.0, acc_data.y / 1000.0, acc_data.z / 1000.0, mx, -my, -mz); getMadAttitude(&attitude_data); chprintf((BaseSequentialStream*)&SERIAL_DRIVER, "%6d %f %f %f\r\n", (int)time, attitude_data.roll, attitude_data.pitch, attitude_data.yaw); time += MS2ST(period); chThdSleepUntil(time); } return 0; }
int main() { // double gpsTime; // unsigned short gpsWeek, status; // VnVector3 ypr, latitudeLognitudeAltitude, nedVelocity; // float attitudeUncertainty, positionUncertainty, velocityUncertainty; VnVector3 magnetic, acceleration, angularRate; float temperature, pressure; float mx, my, mz, ax, ay, az, gx, gy, gz; Vn200 vn200; // Vn100 vn100; int i = 0; vn200_connect(&vn200, COM_PORT, BAUD_RATE); while(1) { vn200_getCalibratedSensorMeasurements( &vn200, &magnetic, &acceleration, &angularRate, &temperature, &pressure); mx = (float) magnetic.c0; my = (float) magnetic.c1; mz = (float) magnetic.c2; ax = (float) acceleration.c0; ay = (float) acceleration.c1; az = (float) acceleration.c2; gx = (float) angularRate.c0; gy = (float) angularRate.c1; gz = (float) angularRate.c2; MadgwickAHRSupdate(gx, gy, gz, ax, ay, az, mx, my, mz); // printf("IMU Solution: \n" // "i: %d\n" // "magnetic.x: %+#7.2f\n" // "magnetic.y: %+#7.2f\n" // "magnetic.z: %+#7.2f\n" // "acceleration.x: %+#7.2f\n" // "acceleration.y: %+#7.2f\n" // "acceleration.z: %+#7.2f\n" // "angularRate.x: %+#7.2f\n" // "angularRate.y: %+#7.2f\n" // "angularRate.z: %+#7.2f\n", // i, // magnetic.c0, // magnetic.c1, // magnetic.c2, // acceleration.c0, // acceleration.c1, // acceleration.c2, // angularRate.c0, // angularRate.c1, // angularRate.c2); // vn200_getInsSolution( // &vn200, // &gpsTime, // &gpsWeek, // &status, // &ypr, // &latitudeLognitudeAltitude, // &nedVelocity, // &attitudeUncertainty, // &positionUncertainty, // &velocityUncertainty); // printf("INS Solution:\n" // " i: %d\n" // " GPS Time: %f\n" // " GPS Week: %u\n" // " INS Status: %.4X\n" // " YPR.Yaw: %+#7.2f\n" // " YPR.Pitch: %+#7.2f\n" // " YPR.Roll: %+#7.2f\n" // " LLA.Lattitude: %+#7.2f\n" // " LLA.Longitude: %+#7.2f\n" // " LLA.Altitude: %+#7.2f\n" // " Velocity.North: %+#7.2f\n" // " Velocity.East: %+#7.2f\n" // " Velocity.Down: %+#7.2f\n" // " Attitude Uncertainty: %+#7.2f\n" // " Position Uncertainty: %+#7.2f\n" // " Velocity Uncertainty: %+#7.2f\n", // i, // gpsTime, // gpsWeek, // status, // ypr.c0, // ypr.c1, // ypr.c2, // latitudeLognitudeAltitude.c0, // latitudeLognitudeAltitude.c1, // latitudeLognitudeAltitude.c2, // nedVelocity.c0, // nedVelocity.c1, // nedVelocity.c2, // attitudeUncertainty, // positionUncertainty, // velocityUncertainty); printf("\n\n"); // sleep(0.05); i++; } vn200_disconnect(&vn200); return 0; }
int main(void) { uint32_t currentTime; // High Speed Telemetry Test Code Begin char numberString[12]; // High Speed Telemetry Test Code End RCC_GetClocksFreq(&clocks); USB_Interrupts_Config(); Set_USBClock(); USB_Init(); // Wait until device configured //while(bDeviceState != CONFIGURED); testInit(); LED0_ON; systemReady = true; //nrf_tx_mode_no_aa(addr,5,40); nrf_rx_mode_dual(addr,5,40); { uint8_t status = nrf_read_reg(NRF_STATUS); nrf_write_reg(NRF_WRITE_REG|NRF_STATUS,status); // clear IRQ flags nrf_write_reg(NRF_FLUSH_RX, 0xff); nrf_write_reg(NRF_FLUSH_TX, 0xff); } while (1) { uint8_t buf[64]; static uint8_t last_tx_done = 0; if(ring_buf_pop(nrf_rx_buffer,buf,32)){ // get data from the adapter switch(buf[0]){ case SET_ATT: break; case SET_MOTOR: break; case SET_MODE: report_mode = buf[1]; break; } last_tx_done = 1; } if(tx_done){ tx_done = 0; // report ACK success last_tx_done = 1; } if(ring_buf_pop(nrf_tx_buffer,buf,32)){ if(last_tx_done){ last_tx_done = 0; nrf_ack_packet(0, buf, 32); } } if (frame_50Hz) { int16_t motor_val[4]; frame_50Hz = false; currentTime = micros(); deltaTime50Hz = currentTime - previous50HzTime; previous50HzTime = currentTime; //memcpy(buf, accelSummedSamples100Hz, 12); //memcpy(buf+12, gyroSummedSamples100Hz, 12); //memcpy(buf+24, magSumed, 6); if(report_mode == DT_ATT){ buf[0] = DT_ATT; memcpy(buf + 1, &sensors.attitude200Hz[0], 12); memcpy(buf + 13, &executionTime200Hz, 4); motor_val[0] = motor[0]; motor_val[1] = motor[1]; motor_val[2] = motor[2]; motor_val[3] = motor[3]; memcpy(buf + 17, motor_val, 8); usb_send_data(buf , 64); executionTime50Hz = micros() - currentTime; }else if(report_mode == DT_SENSOR){ buf[0] = DT_SENSOR; memcpy(buf + 1, gyroSummedSamples100Hz, 12); memcpy(buf + 13, accelSummedSamples100Hz, 12); memcpy(buf + 25, magSumed, 6); } //nrf_tx_packet(buf,16); //if(nrf_rx_packet(buf,16) == NRF_RX_OK) //{ // LED0_TOGGLE; //} ring_buf_push(nrf_tx_buffer, buf, 32); } if(frame_10Hz) { frame_10Hz = false; magSumed[XAXIS] = magSum[XAXIS]; magSumed[YAXIS] = magSum[YAXIS]; magSumed[ZAXIS] = magSum[ZAXIS]; magSum[XAXIS] = 0; magSum[YAXIS] = 0; magSum[ZAXIS] = 0; newMagData = true; } if (frame_100Hz) { frame_100Hz = false; computeAxisCommands(dt100Hz); mixTable(); writeServos(); writeMotors(); } if (frame_200Hz) { frame_200Hz = false; currentTime = micros(); deltaTime200Hz = currentTime - previous200HzTime; previous200HzTime = currentTime; dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop #if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS) sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS]; sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS]; sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS]; sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]); sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]); sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]); computeGyroTCBias(); sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; #endif #if defined(USE_MADGWICK_AHRS) MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW], sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], sensorConfig.accelCutoff, newMagData, dt200Hz ); newMagData = false; q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 ); sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) ); sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 ); #endif #if defined(USE_MARG_AHRS) MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW], sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], sensorConfig.accelCutoff, newMagData, dt200Hz ); newMagData = false; q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 ); sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) ); sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 ); #endif executionTime200Hz = micros() - currentTime; } } systemInit(); systemReady = true; while (1) { /////////////////////////////// if (frame_50Hz) { frame_50Hz = false; currentTime = micros(); deltaTime50Hz = currentTime - previous50HzTime; previous50HzTime = currentTime; processFlightCommands(); executionTime50Hz = micros() - currentTime; } /////////////////////////////// if (frame_10Hz) { LED0_TOGGLE; frame_10Hz = false; currentTime = micros(); deltaTime10Hz = currentTime - previous10HzTime; previous10HzTime = currentTime; sensors.mag10Hz[XAXIS] = -((float)magSum[XAXIS] / 5.0f * magScaleFactor[XAXIS] - sensorConfig.magBias[XAXIS]); sensors.mag10Hz[YAXIS] = (float)magSum[YAXIS] / 5.0f * magScaleFactor[YAXIS] - sensorConfig.magBias[YAXIS]; sensors.mag10Hz[ZAXIS] = -((float)magSum[ZAXIS] / 5.0f * magScaleFactor[ZAXIS] - sensorConfig.magBias[ZAXIS]); magSum[XAXIS] = 0; magSum[YAXIS] = 0; magSum[ZAXIS] = 0; newMagData = true; pressureAverage = pressureSum / 10; pressureSum = 0; calculateTemperature(); calculatePressureAltitude(); sensors.pressureAltitude10Hz = pressureAlt; serialCom(); if ( EKF_Initialized == false ) EKF_Init( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS] ); executionTime10Hz = micros() - currentTime; } /////////////////////////////// if (frame_500Hz) { frame_500Hz = false; currentTime = micros(); deltaTime500Hz = currentTime - previous500HzTime; previous500HzTime = currentTime; dt500Hz = (float)deltaTime500Hz * 0.000001f; // For integrations in 500 Hz loop computeGyroTCBias(); sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; #if defined(USE_CHR6DM_AHRS) if ( EKF_Initialized == true ) EKF_Predict( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], dt500Hz ); sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi; sensors.attitude200Hz[PITCH] = gEstimatedStates.theta; sensors.attitude200Hz[YAW ] = gEstimatedStates.psi; #endif executionTime500Hz = micros() - currentTime; } /////////////////////////////// if (frame_200Hz) { frame_200Hz = false; currentTime = micros(); deltaTime200Hz = currentTime - previous200HzTime; previous200HzTime = currentTime; dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop #if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS) sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS]; sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS]; sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS]; sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]); sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]); sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]); computeGyroTCBias(); sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; #endif #if defined(USE_MADGWICK_AHRS) MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW], sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], sensorConfig.accelCutoff, newMagData, dt200Hz ); newMagData = false; q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 ); sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) ); sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 ); #endif #if defined(USE_MARG_AHRS) MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW], sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], sensorConfig.accelCutoff, newMagData, dt200Hz ); newMagData = false; q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 ); sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) ); sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 ); #endif executionTime200Hz = micros() - currentTime; } /////////////////////////////// if (frame_100Hz) { frame_100Hz = false; currentTime = micros(); deltaTime100Hz = currentTime - previous100HzTime; previous100HzTime = currentTime; dt100Hz = (float)deltaTime100Hz * 0.000001f; // For integrations in 100 Hz loop sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS]; sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS]; sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS]; sensors.accel100Hz[XAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[XAXIS], &fourthOrder100Hz[AX_FILTER]); sensors.accel100Hz[YAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[YAXIS], &fourthOrder100Hz[AY_FILTER]); sensors.accel100Hz[ZAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[ZAXIS], &fourthOrder100Hz[AZ_FILTER]); computeGyroTCBias(); sensors.gyro100Hz[ROLL ] = ((float)gyroSummedSamples100Hz[ROLL] / 10.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; sensors.gyro100Hz[PITCH] = -((float)gyroSummedSamples100Hz[PITCH] / 10.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; sensors.gyro100Hz[YAW ] = -((float)gyroSummedSamples100Hz[YAW] / 10.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; #if defined(USE_CHR6DM_AHRS) if ( EKF_Initialized == true ) EKF_Update( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], sensorConfig.accelCutoff, newMagData ); newMagData = false; sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi; sensors.attitude200Hz[PITCH] = gEstimatedStates.theta; sensors.attitude200Hz[YAW ] = gEstimatedStates.psi; #endif computeAxisCommands(dt100Hz); mixTable(); writeServos(); writeMotors(); // High Speed Telemetry Test Code Begin if ( highSpeedAccelTelemEnabled == true ) { // 100 Hz Accels ftoa(sensors.accel100Hz[XAXIS], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.accel100Hz[YAXIS], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.accel100Hz[ZAXIS], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedGyroTelemEnabled == true ) { // 100 Hz Gyros ftoa(sensors.gyro100Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.gyro100Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedRollRateTelemEnabled == true ) { // Roll Rate, Roll Rate Command ftoa(sensors.gyro100Hz[ROLL], numberString); uartPrint(numberString); uartPrint(","); ftoa(rxCommand[ROLL], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedPitchRateTelemEnabled == true ) { // Pitch Rate, Pitch Rate Command ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(","); ftoa(rxCommand[PITCH], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedYawRateTelemEnabled == true ) { // Yaw Rate, Yaw Rate Command ftoa(sensors.gyro100Hz[YAW], numberString); uartPrint(numberString); uartPrint(","); ftoa(rxCommand[YAW], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedAttitudeTelemEnabled == true ) { // 200 Hz Attitudes ftoa(sensors.attitude200Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.attitude200Hz[PITCH], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.attitude200Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n"); } // High Speed Telemetry Test Code End executionTime100Hz = micros() - currentTime; } /////////////////////////////// if (frame_5Hz) { frame_5Hz = false; currentTime = micros(); deltaTime5Hz = currentTime - previous5HzTime; previous5HzTime = currentTime; executionTime5Hz = micros() - currentTime; } /////////////////////////////// if (frame_1Hz) { frame_1Hz = false; currentTime = micros(); deltaTime1Hz = currentTime - previous1HzTime; previous1HzTime = currentTime; executionTime1Hz = micros() - currentTime; } //////////////////////////////// } }
/** * @brief Main IMU function that should be called all the time in the main while loop. It updates the values of the sensors, calculates the real values from the raw reading and does the Madgwick algorithm to calculate the Euler angles from the thes IMU readings when the data is ready. The dataReady flags are set in the interrupt.cpp file in the ISR(TWI_vect) when the data is ready. The time since the last update is also measured independently for each sensor to get the effective update rate of each sensor. This is done by dividing sum variable (which is the total time since the last sensor rate update) by the sumCount variable (which is the number of readings taken since the last sensor rate update). These variables are global and can be used anywhere to measure this update rate. These values must be put to 0 after the rate is taken. */ void IMU::updateImuAndMadgwick() { if(ak8963DataReady) { ak8963DataReady = false; ak8963.setRawMagneticField(currentRawMag); ak8963.calculateMag(); //calculate the time it takes before the next call of this function Now = TCNT0; //the 8 is counter 0 prescaler deltaTimeAk8963 = ((Now - lastUpdateAk8963 + OCR0A*timer0OverflowCountAk8963)*8/(float)F_CPU); timer0OverflowCountAk8963 = 0; lastUpdateAk8963 = Now; sumAk8963 += deltaTimeAk8963; // sum for averaging filter update rate sumCountAk8963++; // Page 38 of PS-MPU-9255 gives the axes orientations // the x axis on the MPU9255 represents the forward direction. Since on our board the front is actually the y // direction, a 90 degree rotation clockwise has to be made. For this x = y and y = -x for all 3 sensors. // on top of this, to make sure the convention of positive pitch yaw roll is respected, the final x is multiplied by -1 // for all sensors (x,y,z) becomes (-y,-x,z) // for the magnetometer, an extra modification is required // Sensors x (y)-axis of the accelerometer/gyro is aligned with the y (x)-axis of the magnetometer; // the magnetometer z-axis (+ down) is misaligned with z-axis (+ up) of accelerometer and gyro! // so we interchange x and y and multiply the Z axis by -1. // the total change is (x,y,z) becomes (-x,-y,-z)) xMagneticFieldMadgwick = -(float)ak8963.getMagneticFieldX(); yMagneticFieldMadgwick = -(float)ak8963.getMagneticFieldY(); zMagneticFieldMadgwick = -(float)ak8963.getMagneticFieldZ(); MadgwickAHRSupdate(xAccelerationMadgwick, yAccelerationMadgwick, zAccelerationMadgwick, xRotationMadgwick, yRotationMadgwick, zRotationMadgwick, xMagneticFieldMadgwick, yMagneticFieldMadgwick, zMagneticFieldMadgwick); calcEulerAngles(); } else if(bmp180DataReady) { bmp180DataReady = false; bmp180.setRawTemperature(currentRawTemp); bmp180.setRawPressure(currentRawPress); bmp180.calculateTrueValues(); //calculate the time it takes before the next call of this function Now = TCNT0; //the 8 is counter 0 prescaler deltaTimeBmp180 = ((Now - lastUpdateBmp180 + OCR0A*timer0OverflowCountBmp180)*8/(float)F_CPU); timer0OverflowCountBmp180 = 0; lastUpdateBmp180 = Now; sumBmp180 += deltaTimeBmp180; // sum for averaging filter update rate sumCountBmp180++; } else if(mpu9255DataReady) { mpu9255DataReady = false; //These functions updates the values of the class variables with the FSM buffer variables mpu9255.setRawAcceleration(currentRawAcc); mpu9255.setRawRotation(currentRawGyr); mpu9255.calculateAccRotTemp(); //calculate the time it takes before the next call of this function Now = TCNT0; //the 8 is counter 0 prescaler deltaTimeMpu9255 = ((Now - lastUpdateMpu9255 + OCR0A*timer0OverflowCountMpu9255)*8/(float)F_CPU); timer0OverflowCountMpu9255 = 0; lastUpdateMpu9255 = Now; sumMpu9255 += deltaTimeMpu9255; // sum for averaging filter update rate sumCountMpu9255++; xAccelerationMadgwick = -(float)mpu9255.getAccelerationY()/10000; yAccelerationMadgwick = -(float)mpu9255.getAccelerationX()/10000; zAccelerationMadgwick = (float)mpu9255.getAccelerationZ()/10000; xRotationMadgwick = -(float)mpu9255.getRotationY()*M_PI_OVER_1800; yRotationMadgwick = -(float)mpu9255.getRotationY()*M_PI_OVER_1800; zRotationMadgwick = (float)mpu9255.getRotationZ()*M_PI_OVER_1800; MadgwickAHRSupdateIMU(xAccelerationMadgwick, yAccelerationMadgwick, zAccelerationMadgwick, xRotationMadgwick, yRotationMadgwick, zRotationMadgwick); calcEulerAngles(); } }
int main(void) { uint32_t currentTime; // High Speed Telemetry Test Code Begin char numberString[12]; // High Speed Telemetry Test Code End systemInit(); systemReady = true; while (1) { /////////////////////////////// if (frame_50Hz) { frame_50Hz = false; currentTime = micros(); deltaTime50Hz = currentTime - previous50HzTime; previous50HzTime = currentTime; processFlightCommands(); executionTime50Hz = micros() - currentTime; } /////////////////////////////// if (frame_10Hz) { frame_10Hz = false; currentTime = micros(); deltaTime10Hz = currentTime - previous10HzTime; previous10HzTime = currentTime; sensors.mag10Hz[XAXIS] = -((float)magSum[XAXIS] / 5.0f * magScaleFactor[XAXIS] - sensorConfig.magBias[XAXIS]); sensors.mag10Hz[YAXIS] = (float)magSum[YAXIS] / 5.0f * magScaleFactor[YAXIS] - sensorConfig.magBias[YAXIS]; sensors.mag10Hz[ZAXIS] = -((float)magSum[ZAXIS] / 5.0f * magScaleFactor[ZAXIS] - sensorConfig.magBias[ZAXIS]); magSum[XAXIS] = 0; magSum[YAXIS] = 0; magSum[ZAXIS] = 0; newMagData = true; pressureAverage = pressureSum / 10; pressureSum = 0; calculateTemperature(); calculatePressureAltitude(); sensors.pressureAltitude10Hz = pressureAlt; serialCom(); if ( EKF_Initialized == false ) EKF_Init( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS] ); executionTime10Hz = micros() - currentTime; } /////////////////////////////// if (frame_500Hz) { frame_500Hz = false; currentTime = micros(); deltaTime500Hz = currentTime - previous500HzTime; previous500HzTime = currentTime; dt500Hz = (float)deltaTime500Hz * 0.000001f; // For integrations in 500 Hz loop computeGyroTCBias(); sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; #if defined(USE_CHR6DM_AHRS) if ( EKF_Initialized == true ) EKF_Predict( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], dt500Hz ); sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi; sensors.attitude200Hz[PITCH] = gEstimatedStates.theta; sensors.attitude200Hz[YAW ] = gEstimatedStates.psi; #endif executionTime500Hz = micros() - currentTime; } /////////////////////////////// if (frame_200Hz) { frame_200Hz = false; currentTime = micros(); deltaTime200Hz = currentTime - previous200HzTime; previous200HzTime = currentTime; dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop #if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS) sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS]; sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS]; sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS]; sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]); sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]); sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]); computeGyroTCBias(); sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; #endif #if defined(USE_MADGWICK_AHRS) MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW], sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], sensorConfig.accelCutoff, newMagData, dt200Hz ); newMagData = false; q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 ); sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) ); sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 ); #endif #if defined(USE_MARG_AHRS) MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW], sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], sensorConfig.accelCutoff, newMagData, dt200Hz ); newMagData = false; q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 ); sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) ); sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 ); #endif executionTime200Hz = micros() - currentTime; } /////////////////////////////// if (frame_100Hz) { frame_100Hz = false; currentTime = micros(); deltaTime100Hz = currentTime - previous100HzTime; previous100HzTime = currentTime; dt100Hz = (float)deltaTime100Hz * 0.000001f; // For integrations in 100 Hz loop sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS]; sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS]; sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS]; sensors.accel100Hz[XAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[XAXIS], &fourthOrder100Hz[AX_FILTER]); sensors.accel100Hz[YAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[YAXIS], &fourthOrder100Hz[AY_FILTER]); sensors.accel100Hz[ZAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[ZAXIS], &fourthOrder100Hz[AZ_FILTER]); computeGyroTCBias(); sensors.gyro100Hz[ROLL ] = ((float)gyroSummedSamples100Hz[ROLL] / 10.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; sensors.gyro100Hz[PITCH] = -((float)gyroSummedSamples100Hz[PITCH] / 10.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; sensors.gyro100Hz[YAW ] = -((float)gyroSummedSamples100Hz[YAW] / 10.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; #if defined(USE_CHR6DM_AHRS) if ( EKF_Initialized == true ) EKF_Update( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], sensorConfig.accelCutoff, newMagData ); newMagData = false; sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi; sensors.attitude200Hz[PITCH] = gEstimatedStates.theta; sensors.attitude200Hz[YAW ] = gEstimatedStates.psi; #endif computeAxisCommands(dt100Hz); mixTable(); writeServos(); writeMotors(); // High Speed Telemetry Test Code Begin if ( highSpeedAccelTelemEnabled == true ) { // 100 Hz Accels ftoa(sensors.accel100Hz[XAXIS], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.accel100Hz[YAXIS], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.accel100Hz[ZAXIS], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedGyroTelemEnabled == true ) { // 100 Hz Gyros ftoa(sensors.gyro100Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.gyro100Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedRollRateTelemEnabled == true ) { // Roll Rate, Roll Rate Command ftoa(sensors.gyro100Hz[ROLL], numberString); uartPrint(numberString); uartPrint(","); ftoa(rxCommand[ROLL], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedPitchRateTelemEnabled == true ) { // Pitch Rate, Pitch Rate Command ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(","); ftoa(rxCommand[PITCH], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedYawRateTelemEnabled == true ) { // Yaw Rate, Yaw Rate Command ftoa(sensors.gyro100Hz[YAW], numberString); uartPrint(numberString); uartPrint(","); ftoa(rxCommand[YAW], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedAttitudeTelemEnabled == true ) { // 200 Hz Attitudes ftoa(sensors.attitude200Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.attitude200Hz[PITCH], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.attitude200Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n"); } // High Speed Telemetry Test Code End executionTime100Hz = micros() - currentTime; } /////////////////////////////// if (frame_5Hz) { frame_5Hz = false; currentTime = micros(); deltaTime5Hz = currentTime - previous5HzTime; previous5HzTime = currentTime; executionTime5Hz = micros() - currentTime; } /////////////////////////////// if (frame_1Hz) { frame_1Hz = false; currentTime = micros(); deltaTime1Hz = currentTime - previous1HzTime; previous1HzTime = currentTime; executionTime1Hz = micros() - currentTime; } //////////////////////////////// } }
void MainWindow::processSensorsData(TUdpDataSENSORS* data) { int32_t time = data->ticks; /*m_pdaData.frustum[0]*=10; m_pdaData.frustum[1]*=10; m_pdaData.frustum[2]*=10; m_pdaData.frustum[3]*=10;*/ m_pdaData.sphereEnabled = data->sphereEnabled; m_pdaData.ax = -data->ax; m_pdaData.ay = -data->ay; m_pdaData.az = -data->az; m_pdaData.gx = data->gx; m_pdaData.gy = data->gy; m_pdaData.gz = data->gz; m_pdaData.mx = -data->mx; m_pdaData.my = -data->my; m_pdaData.mz = -data->mz; m_pdaData.worldQuat = QQuaternion(data->q0, data->q1, data->q2, data->q3); if (m_pdaData.mx < minX) minX = m_pdaData.mx; if (m_pdaData.my < minY) minY = m_pdaData.my; if (m_pdaData.mz < minZ) minZ = m_pdaData.mz; if (m_pdaData.mx > maxX) maxX = m_pdaData.mx; if (m_pdaData.my > maxY) maxY = m_pdaData.my; if (m_pdaData.mz > maxZ) maxZ = m_pdaData.mz; //-66.75 -46.75 -40.25 // 40.75 40.5 40.5 static int64_t last = 0; if (last == 0) last = time; int64_t diff = time - last; if (diff > 1000) { //qDebug() << "RES"; m_accelQuat = QQuaternion(1, 0, 0, 0); m_gyroQuat = QQuaternion(1, 0, 0, 0); m_gyroAccelQuat = QQuaternion(1, 0, 0, 0); m_accelMagnetQuat = QQuaternion(1, 0, 0, 0); m_fullQuat = QQuaternion(1, 0, 0, 0); last = 0; diff = 100; return; } last = time; //qDebug() << "DIFF" << time << diff; MadgwickAHRSupdateIMU(m_accelQuat, 0, 0, 0, m_pdaData.ax, m_pdaData.ay, m_pdaData.az, (float)diff / 1000.0f * 5); MadgwickAHRSupdateIMU(m_gyroQuat, m_pdaData.gx, m_pdaData.gy, m_pdaData.gz, 0, 0, 0, (float)diff / 1000.0f); MadgwickAHRSupdateIMU(m_gyroAccelQuat, m_pdaData.gx, m_pdaData.gy, m_pdaData.gz, m_pdaData.ax, m_pdaData.ay, m_pdaData.az, (float)diff / 1000.0f); MadgwickAHRSupdate(m_accelMagnetQuat, 0, 0, 0, m_pdaData.ax, m_pdaData.ay, m_pdaData.az, m_pdaData.mx, m_pdaData.my, m_pdaData.mz, (float)diff / 1000.0f); MadgwickAHRSupdate(m_fullQuat, m_pdaData.gx, m_pdaData.gy, m_pdaData.gz, m_pdaData.ax, m_pdaData.ay, m_pdaData.az, m_pdaData.mx, m_pdaData.my, m_pdaData.mz, (float)diff / 1000.0f); ui->visAccel->rotationQuat = m_accelQuat; ui->visGyro->rotationQuat = m_gyroQuat; ui->visGyroAccel->rotationQuat = m_gyroAccelQuat; ui->visAccelMagnet->rotationQuat = QQuaternion::fromAxisAndAngle(0, 0, 1, -90); ui->visAccelMagnet->rotationQuat *= m_accelMagnetQuat; ui->visFull->rotationQuat = QQuaternion::fromAxisAndAngle(0, 0, 1, -90); ui->visFull->rotationQuat *= m_fullQuat; ui->visWorld->rotationQuat = m_pdaData.worldQuat.conjugate(); ui->visWorld2->rotationQuat = m_pdaData.worldQuat.conjugate(); ui->visScreen->rotationQuat = m_pdaData.worldQuat.conjugate(); //static float q = 0; //ui->visWorld->rotationQuat *= QQuaternion::fromAxisAndAngle(0, 1, 1, q += 1); float qq0 = m_fullQuat.scalar(); float qq1 = m_fullQuat.x(); float qq2 = m_fullQuat.y(); float qq3 = m_fullQuat.z(); float q2_2 = qq2*qq2; m_pdaData.pitch = atanf (2.0f*(qq0*qq1 + qq2*qq3) / (1.0f - 2.0f*(qq1*qq1 + q2_2))); m_pdaData.roll = asinf (2.0f*(qq0*qq2 - qq3*qq1)); m_pdaData.yaw = atan2f (2.0f*(qq0*qq3 + qq1*qq2), (1.0f - 2.0f*(q2_2 + qq3*qq3))); ui->rawReads->updateInfo(m_pdaData); }
// Main loop void razor_loop(void) { razor_loop_input(); // Time to read the sensors again? if ((timer_systime() - timestamp) >= OUTPUT__DATA_INTERVAL) { timestamp_old = timestamp; timestamp = timer_systime(); if (timestamp > timestamp_old) G_Dt = (double)MS2S(timestamp - timestamp_old); // Real time of loop run. We use this on the DCM algorithm (gyro integration time) else G_Dt = 0.0; // Update sensor readings read_sensors(); switch (output_mode) { case OUTPUT__MODE_CALIBRATE_SENSORS: // We're in calibration mode { check_reset_calibration_session(); // Check if this session needs a reset if (output_stream_on || output_single_on) output_calibration(curr_calibration_sensor); break; } case OUTPUT__MODE_YPR_RAZOR: { // Apply sensor calibration compensate_sensor_errors(); // Run DCM algorithm Compass_Heading(); // Calculate magnetic heading Matrix_update(); Normalize(); Drift_correction(); Euler_angles(); if (output_stream_on || output_single_on) output_angles(); break; } case OUTPUT__MODE_YPR_MADGWICK: { compensate_sensor_errors(); gyro[0] = GYRO_SCALED_RAD(gyro[0]); gyro[1] = GYRO_SCALED_RAD(gyro[1]); gyro[2] = GYRO_SCALED_RAD(gyro[2]); // acc and mag already scaled in compensate_sensor_errors() // Apply Madgwick algorithm MadgwickAHRSupdate(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2]); //MadgwickAHRSupdate(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2]); //MadgwickAHRSupdate(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, magnetom[0], magnetom[1], magnetom[2]); MadgwickYawPitchRoll(&yaw, &pitch, &roll); if (output_stream_on || output_single_on) output_angles(); break; } case OUTPUT__MODE_YPR_FREEIMU: { compensate_sensor_errors(); gyro[0] = GYRO_SCALED_RAD(gyro[0]); gyro[1] = GYRO_SCALED_RAD(gyro[1]); gyro[2] = GYRO_SCALED_RAD(gyro[2]); // acc and mag already scaled in compensate_sensor_errors() // get sensorManager and initialise sensor listeners //mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE); //initListeners(); // wait for one second until gyroscope and magnetometer/accelerometer // data is initialised then scedule the complementary filter task //fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(), 1000, TIME_CONSTANT); //public void onSensorChanged(SensorEvent event) { //switch(event.sensor.getType()) { //case Sensor.TYPE_ACCELEROMETER: // copy new accelerometer data into accel array and calculate orientation //System.arraycopy(event.values, 0, accel, 0, 3); calculateAccMagOrientation(); //case Sensor.TYPE_GYROSCOPE: // process gyro data //gyroFunction(event); gyroFunction(); //case Sensor.TYPE_MAGNETIC_FIELD: // copy new magnetometer data into magnet array //System.arraycopy(event.values, 0, magnet, 0, 3); calculateFusedOrientation(); yaw = -fusedOrientation[0]; roll = -fusedOrientation[1]; pitch = fusedOrientation[2]; //if (output_stream_on || output_single_on) output_angles_freedom(); if (output_stream_on || output_single_on) output_angles(); break; } case OUTPUT__MODE_SENSORS_RAW: case OUTPUT__MODE_SENSORS_CALIB: case OUTPUT__MODE_SENSORS_BOTH: { if (output_stream_on || output_single_on) output_sensors(); break; } } output_single_on = false; #if DEBUG__PRINT_LOOP_TIME == true //printf("loop time (ms) = %lu\r\n", timer_systime() - timestamp); // Not really useful since RTC measures only 4 ms. #endif } #if DEBUG__PRINT_LOOP_TIME == true else { printf("waiting...\r\n"); } #endif return; }
void psmove_orientation_update(PSMoveOrientation *orientation) { psmove_return_if_fail(orientation != NULL); int frame; long now = psmove_util_get_ticks(); if (now - orientation->sample_freq_measure_start >= 1000) { float measured = ((float)orientation->sample_freq_measure_count) / ((float)(now-orientation->sample_freq_measure_start))*1000.; psmove_DEBUG("Measured sample_freq: %f\n", measured); orientation->sample_freq = measured; orientation->sample_freq_measure_start = now; orientation->sample_freq_measure_count = 0; } /* We get 2 measurements per call to psmove_poll() */ orientation->sample_freq_measure_count += 2; psmove_get_magnetometer_vector(orientation->move, &orientation->output[6], &orientation->output[7], &orientation->output[8]); float q0 = orientation->quaternion[0]; float q1 = orientation->quaternion[1]; float q2 = orientation->quaternion[2]; float q3 = orientation->quaternion[3]; for (frame=0; frame<2; frame++) { psmove_get_accelerometer_frame(orientation->move, frame, &orientation->output[0], &orientation->output[1], &orientation->output[2]); psmove_get_gyroscope_frame(orientation->move, frame, &orientation->output[3], &orientation->output[4], &orientation->output[5]); #if defined(PSMOVE_WITH_MADGWICK_AHRS) MadgwickAHRSupdate(orientation->quaternion, orientation->sample_freq, 0.5f, /* Gyroscope */ orientation->output[3], orientation->output[5], -orientation->output[4], /* Accelerometer */ orientation->output[0], orientation->output[2], -orientation->output[1], /* Magnetometer */ orientation->output[6], orientation->output[8], orientation->output[7] ); #else psmove_DEBUG("Built without Madgwick AHRS - no orientation tracking"); return; #endif if (isnan(orientation->quaternion[0]) || isnan(orientation->quaternion[1]) || isnan(orientation->quaternion[2]) || isnan(orientation->quaternion[3])) { psmove_DEBUG("Orientation is NaN!"); orientation->quaternion[0] = q0; orientation->quaternion[1] = q1; orientation->quaternion[2] = q2; orientation->quaternion[3] = q3; } } }
int main (int argc, char* argv[]) { float gx = 0.0; float gy = 0.0; float gz = 0.0; float ax = 1.0; float ay = 1.0; float az = 1.0; float mx = 0.0; float my = 0.0; float mz = 0.0; int i; printf("Size of float is %d\n", sizeof(float)); printf ("%f, %f, %f, %f\n", q0, q1, q2, q3); for (i = 0; i < 100; i += 1) { MadgwickAHRSupdate(gx, gy, gz, ax, ay, az, mx, my, mz); printf ("%f, %f, %f, %f\n", q0, q1, q2, q3); } return(0); }
void loop(){ static int i=0; static float goffsetx=0,goffsety=0,goffsetz=0; static float coef = 3.1415926599f/180; if(sensor_data_ready_FLAG){ sensor_data_ready_FLAG=FALSE; if(i<1000){ //calc gyro offset with 1000 samples goffsetx+=local_data.gyro[0]; goffsety+=local_data.gyro[1]; goffsetz+=local_data.gyro[2]; i++; if(i==1000){ BSP_LED_On(LED0); goffsetx/=1000; goffsety/=1000; goffsetz/=1000; } }else{ MadgwickAHRSupdate(q, 1.0f/SAMPLINGRATE,(local_data.gyro[0]-goffsetx)*coef,(local_data.gyro[1]-goffsety)*coef,(local_data.gyro[2]-goffsetz)*coef,local_data.acc[0],local_data.acc[1],local_data.acc[2],local_data.mag[1]*-1,local_data.mag[0]*-1,local_data.mag[2]); } sensor_print(NULL); //static char tmpbuf[30]; /*memset(tmpbuf,0,30); sprintf(tmpbuf,"gyro %.2f %.2f %.2f ",data->gyro[0],data->gyro[1],data->gyro[2]); UART_Transmit((uint8_t *)tmpbuf,30); memset(tmpbuf,0,30); sprintf(tmpbuf,"acc %.2f %.2f %.2f\n",data->acc[0],data->acc[1],data->acc[2]); UART_Transmit((uint8_t *)tmpbuf,30); memset(tmpbuf,0,30); sprintf(tmpbuf,"mag %.2f %.2f %.2f\n",data->mag[0],data->mag[1],data->mag[2]); UART_Transmit((uint8_t *)tmpbuf,30);*/ } }