AHRS library, modified version of Peter Bartz work.
AHRS.cpp
- Committer:
- tylerjw
- Date:
- 2012-11-08
- Revision:
- 1:da3b20b5d38a
- Parent:
- 0:014ee3239c80
File content as of revision 1:da3b20b5d38a:
/* This file is part of the Razor AHRS Firmware */ #include "AHRS.h" #include <math.h> void IMU::read_sensors() { Read_Gyro(); // Read gyroscope (and temperature) Read_Accel(); // Read accelerometer Read_Magn(); // Read magnetometer } // Read every sensor and record a time stamp // Init DCM with unfiltered orientation // TODO re-init global vars? void IMU::reset_sensor_fusion() { float temp0[3] = { accel[0], accel[1], accel[2] }; float temp1[3]; float temp2[3]; float xAxis[] = {1.0f, 0.0f, 0.0f}; read_sensors(); timestamp = timer.read_ms(); // GET PITCH // Using y-z-plane-component/x-component of gravity vector pitch = -atan2(accel[0], sqrt((double)(accel[1] * accel[1] + accel[2] * accel[2]))); // GET ROLL // Compensate pitch of gravity vector Vector_Cross_Product(temp1, temp0, 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); } // Apply calibration to raw sensor readings void IMU::compensate_sensor_errors() { // Compensate accelerometer error accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE; accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE; accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE; // Compensate magnetometer error magnetom[0] = (magnetom[0] - MAGN_X_OFFSET) * MAGN_X_SCALE; magnetom[1] = (magnetom[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE; magnetom[2] = (magnetom[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE; // Compensate gyroscope error gyro[0] -= GYRO_AVERAGE_OFFSET_X; gyro[1] -= GYRO_AVERAGE_OFFSET_Y; gyro[2] -= GYRO_AVERAGE_OFFSET_Z; } // Reset calibration session if reset_calibration_session_flag is set void IMU::check_reset_calibration_session() { // Raw sensor values have to be read already, but no error compensation applied // Reset this calibration session? if (!reset_calibration_session_flag) return; // Reset acc and mag calibration variables for (int i = 0; i < 3; i++) { accel_min[i] = accel_max[i] = accel[i]; magnetom_min[i] = magnetom_max[i] = magnetom[i]; } // Reset gyro calibration variables gyro_num_samples = 0; // Reset gyro calibration averaging gyro_average[0] = gyro_average[1] = gyro_average[2] = 0; reset_calibration_session_flag = false; } void IMU::turn_output_stream_on() { output_stream_on = true; statusLed = 1; } void IMU::turn_output_stream_off() { output_stream_on = false; statusLed = 0; } // Blocks until another byte is available on serial port char IMU::readChar() { while (pc.rxBufferGetCount() < 1) { } // Block return pc.getc(); } void IMU::readInput() { // Read incoming control messages if (pc.rxBufferGetCount() >= 2) { if (pc.getc() == '#') { // Start of new control message int command = pc.getc(); // Commands if (command == 'f') // request one output _f_rame output_single_on = true; else if (command == 's') { // _s_ynch request // Read ID char id[2]; id[0] = readChar(); id[1] = readChar(); // Reply with synch message pc.printf("#SYNCH"); pc.putc(id[0]); pc.putc(id[1]); pc.printf(NEW_LINE); } else if (command == 'o') { // Set _o_utput mode char output_param = readChar(); if (output_param == 'n') { // Calibrate _n_ext sensor curr_calibration_sensor = (curr_calibration_sensor + 1) % 3; reset_calibration_session_flag = true; } else if (output_param == 't') // Output angles as _t_ext output_mode = OUTPUT__MODE_ANGLES_TEXT; else if (output_param == 'b') // Output angles in _b_inary form output_mode = OUTPUT__MODE_ANGLES_BINARY; else if (output_param == 'c') { // Go to _c_alibration mode output_mode = OUTPUT__MODE_CALIBRATE_SENSORS; reset_calibration_session_flag = true; } else if (output_param == 's') // Output _s_ensor values as text output_mode = OUTPUT__MODE_SENSORS_TEXT; else if (output_param == '0') { // Disable continuous streaming output turn_output_stream_off(); reset_calibration_session_flag = true; } else if (output_param == '1') { // Enable continuous streaming output reset_calibration_session_flag = true; turn_output_stream_on(); } else if (output_param == 'e') { // _e_rror output settings char error_param = readChar(); if (error_param == '0') output_errors = false; else if (error_param == '1') output_errors = true; else if (error_param == 'c') { // get error count pc.printf("#AMG-ERR:%d,%d,%d" NEW_LINE,num_accel_errors,num_magn_errors,num_gyro_errors); } } } #if OUTPUT__HAS_RN_BLUETOOTH == true // Read messages from bluetooth module // For this to work, the connect/disconnect message prefix of the module has to be set to "#". else if (command == 'C') // Bluetooth "#CONNECT" message (does the same as "#o1") turn_output_stream_on(); else if (command == 'D') // Bluetooth "#DISCONNECT" message (does the same as "#o0") turn_output_stream_off(); #endif // OUTPUT__HAS_RN_BLUETOOTH == true } else { } // Skip character } } IMU::IMU() : gyro_num_samples(0) , yaw(0) , pitch(0) , roll(0) , MAG_Heading(0) , timestamp(0) , timestamp_old(0) , G_Dt(0) , output_mode(OUTPUT__MODE_ANGLES_BINARY) // Select your startup output mode here!// Select your startup output mode here! , output_stream_on(false) , output_single_on(true) , curr_calibration_sensor(0) , reset_calibration_session_flag(true) , num_accel_errors(0) , num_magn_errors(0) , num_gyro_errors(0) , output_errors(true) , statusLed(LED1) , pc(USBTX, USBRX) , Wire(p28, p27) { accel[0] = accel_min[0] = accel_max[0] = magnetom[0] = magnetom_min[0] = magnetom_max[0] = gyro[0] = gyro_average[0] = 0; accel[1] = accel_min[1] = accel_max[1] = magnetom[1] = magnetom_min[1] = magnetom_max[1] = gyro[1] = gyro_average[1] = 0; accel[2] = accel_min[2] = accel_max[2] = magnetom[2] = magnetom_min[2] = magnetom_max[2] = gyro[2] = gyro_average[2] = 0; Accel_Vector[0] = Gyro_Vector[0] = Omega_Vector[0] = Omega_P[0] = Omega_I[0] = Omega[0] = errorRollPitch[0] = errorYaw[0] = 0; Accel_Vector[1] = Gyro_Vector[1] = Omega_Vector[1] = Omega_P[1] = Omega_I[1] = Omega[1] = errorRollPitch[1] = errorYaw[1] = 0; Accel_Vector[2] = Gyro_Vector[2] = Omega_Vector[2] = Omega_P[2] = Omega_I[2] = Omega[2] = errorRollPitch[2] = errorYaw[2] = 0; DCM_Matrix[0][0] = 1; DCM_Matrix[0][1] = 0; DCM_Matrix[0][2] = 0; DCM_Matrix[1][0] = 0; DCM_Matrix[1][1] = 1; DCM_Matrix[1][2] = 0; DCM_Matrix[2][0] = 0; DCM_Matrix[2][1] = 0; DCM_Matrix[2][2] = 1; Update_Matrix[0][0] = 0; Update_Matrix[0][1] = 1; Update_Matrix[0][2] = 2; Update_Matrix[1][0] = 3; Update_Matrix[1][1] = 4; Update_Matrix[1][2] = 5; Update_Matrix[2][0] = 6; Update_Matrix[2][1] = 7; Update_Matrix[2][2] = 8; Temporary_Matrix[0][0] = 0; Temporary_Matrix[0][1] = 0; Temporary_Matrix[0][2] = 0; Temporary_Matrix[1][0] = 0; Temporary_Matrix[1][1] = 0; Temporary_Matrix[1][2] = 0; Temporary_Matrix[2][0] = 0; Temporary_Matrix[2][1] = 0; Temporary_Matrix[2][2] = 0; timer.start(); // Init serial output pc.baud(OUTPUT__BAUD_RATE); // Init status LED statusLed = 0; // Init sensors wait_ms(50); // Give sensors enough time to start I2C_Init(); Accel_Init(); Magn_Init(); Gyro_Init(); // Read sensors, init DCM algorithm wait_ms(20); // Give sensors enough time to collect data reset_sensor_fusion(); // Init output #if (OUTPUT__HAS_RN_BLUETOOTH == true) || (OUTPUT__STARTUP_STREAM_ON == false) turn_output_stream_off(); #else turn_output_stream_on(); #endif } // Main loop void IMU::loop() { timestamp_old = timestamp; timestamp = timer.read_ms(); 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 if (output_stream_on || output_single_on) output_calibration(curr_calibration_sensor); } else if (output_mode == OUTPUT__MODE_SENSORS_TEXT) { // Apply sensor calibration compensate_sensor_errors(); if (output_stream_on || output_single_on) output_sensors(); } else if (output_mode == OUTPUT__MODE_ANGLES_TEXT || output_mode == OUTPUT__MODE_ANGLES_BINARY) { // 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(); } output_single_on = false; #if DEBUG__PRINT_LOOP_TIME == true pc.printf("loop time (ms) = %f" NEW_LINE, timer.read_ms() - timestamp); #endif } // Main loop void IMU::loop(float *data) { timestamp_old = timestamp; timestamp = timer.read_ms(); 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(); // Apply sensor calibration //compensate_sensor_errors(); // Run DCM algorithm Compass_Heading(); // Calculate magnetic heading Matrix_update(); Normalize(); Drift_correction(); Euler_angles(); data[0] = temperature; data[1] = TO_DEG(yaw); data[2] = TO_DEG(pitch); data[3] = TO_DEG(roll); }