void sample_MiniMU9() //Main Loop { if((millis()-timer)>=100) //20 // Main loop runs at 50Hz { counter++; timer_old = timer; timer=millis(); if (timer>timer_old) G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time) else G_Dt = 0; // *** DCM algorithm // Data adquisition Read_Gyro(); // This read gyro data Read_Accel(); // Read I2C accelerometer if (counter > 5) // Read compass data at 10Hz... (5 loop runs) { counter=0; Read_Compass(); // Read I2C magnetometer Compass_Heading(); // Calculate magnetic heading } // Calculations... Matrix_update(); Normalize(); Drift_correction(); Euler_angles(); // *** printdata(); } }
void readImu() { timestamp_old = timestamp; timestamp = millis(); if (timestamp > timestamp_old) G_Dt = (float)(timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time) else G_Dt = 0; // Update sensor readings read_sensors(); if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS) // We're in calibration mode { check_reset_calibration_session(); // Check if this session needs a reset } else if (output_mode == OUTPUT__MODE_ANGLES) // Output angles { // Apply sensor calibration compensate_sensor_errors(); // Run DCM algorithm Compass_Heading(); // Calculate magnetic heading Matrix_update(); Normalize(); Drift_correction(); Euler_angles(); } }
// Read every sensor and record a time stamp // Init DCM with unfiltered orientation // TODO re-init global vars? void reset_sensor_fusion() { float temp1[3]; float temp2[3]; float xAxis[] = { 1.0f, 0.0f, 0.0f }; read_sensors(); timestamp = millis(); // GET PITCH // Using y-z-plane-component/x-component of gravity vector pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2])); // GET ROLL // Compensate pitch of gravity vector Vector_Cross_Product(temp1, accel, xAxis); Vector_Cross_Product(temp2, xAxis, temp1); // Normally using x-z-plane-component/y-component of compensated gravity vector // roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2])); // Since we compensated for pitch, x-z-plane-component equals z-component: roll = atan2(temp2[1], temp2[2]); // GET YAW Compass_Heading(); yaw = MAG_Heading; // Init rotation matrix init_rotation_matrix(DCM_Matrix, yaw, pitch, roll); }
int main(void) { My_Init(); Init_Timer(); Init_I2C(); Init_Sensors(); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_1); ///////////////////////////////// SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA); GPIOPinConfigure(GPIO_PA0_U0RX); GPIOPinConfigure(GPIO_PA1_U0TX); GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); //enable GPIO port for LED GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_2); //enable pin for LED PF2 UARTConfigSetExpClk(UART0_BASE, SysCtlClockGet(), 115200, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE)); IntMasterEnable(); //enable processor interrupts IntEnable(INT_UART0); //enable the UART interrupt UARTIntEnable(UART0_BASE, UART_INT_RX | UART_INT_RT); //only enable RX and TX interrupts ///////////////////////////////// Kalman_Sim_initialize(); while(1) { Read_Accelerometer(); Calculate_Acc(); Read_Compass(); Compass_Heading(); Calculate_Compass(); Read_Gyro(); Calculate_Gyro(); fgyro[0] = sen_data.gyro_x; fgyro[1] = sen_data.gyro_y; fgyro[2] = sen_data.gyro_z; facc[0] = sen_data.accel_x; facc[1] = sen_data.accel_y; facc[2] = sen_data.accel_z; fmag[0] = sen_data.magnetom_x; fmag[1] = sen_data.magnetom_y; fmag[2] = sen_data.magnetom_z; Kalman_Sim_step(); data[0]=Out1[0]; data[1]=Out1[1]; data[2]=Out1[2]; Timer_CyRun(); } }
int main(int argc, char**argv) { ros::init(argc,argv,"imu"); ros::NodeHandle n; ros::Publisher out_pub = n.advertise<sensor_msgs::Imu >("imuyrp", 1000); ros::NodeHandle nh; ros::Publisher chatter_pub = nh.advertise<std_msgs::Float64>("yaw", 1000); sensor_msgs::Imu imu_msg; std_msgs::Float64 msg; // Read sensors, init DCM algorithm reset_sensor_fusion(); removegyrooff(); float Y=0.0; int i=0; while(ros::ok()) { // Time to read the sensors again? if((clock() - timestamp) >= OUTPUT__DATA_INTERVAL) { timestamp_old = timestamp; timestamp = clock(); if (timestamp > timestamp_old) G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time) else G_Dt = 0; //cout << G_Dt << endl; // Update sensor readings read_sensors(); // Run DCM algorithm Compass_Heading(); // Calculate magnetic heading Matrix_update(); Normalize(); Drift_correction(); Euler_angles(); output_angles(); imu_msg = sensor_msgs::Imu(); imu_msg.header.stamp = ros::Time::now(); imu_msg.header.frame_id = "imu"; imu_msg.orientation.x = TO_DEG(pitch); imu_msg.orientation.y = TO_DEG(roll); imu_msg.orientation.z = TO_DEG(yaw); imu_msg.orientation.w = 0.0; imu_msg.orientation_covariance[0] = -1; imu_msg.angular_velocity.x = 0.0; imu_msg.angular_velocity.y = 0.0; imu_msg.angular_velocity.z = 0.0; imu_msg.angular_velocity_covariance[0] = -1; imu_msg.linear_acceleration.x = 0.0; imu_msg.linear_acceleration.y = 0.0; imu_msg.linear_acceleration.z = 0.0; imu_msg.linear_acceleration_covariance[0] = -1; msg.data = TO_DEG(-atan2(magnetom[1],magnetom[0])); chatter_pub.publish(msg); ROS_INFO("%s %f", "send an imu message",TO_DEG(-atan2(magnetom[1],magnetom[0]))); //ros::spinOnce(); } } }
// 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; }