I have a Sparkfun SAMD21 mini breakout that I want to use as a substitute for an arduino. I want to measure pitch and roll from an MPU6050. This has worked fine with an arduino nano (with Serial instead of SerialUSB), but with the SAMD21, I get weird values, It sometimes reads values like -395 degrees or 250 degrees. Past a certain angle, the output reads nan. Is there a difference between the Wire library for Arduino and the Wire library for the SAMD21 boards?
The code is modified from Broxxing's code: Create a 6dof IMU with a gyro and accelerometer for (Arduino) multicopters.
///////////////////////////////////////////////////////////////////////////////////////
/*Terms of use
///////////////////////////////////////////////////////////////////////////////////////
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//THE SOFTWARE.
///////////////////////////////////////////////////////////////////////////////////////
//Support
///////////////////////////////////////////////////////////////////////////////////////
Website: http://www.brokking.net/imu.html
Youtube: https://youtu.be/4BoIE8YQwM8
Version: 1.0 (May 5, 2016)
///////////////////////////////////////////////////////////////////////////////////////
//Connections
///////////////////////////////////////////////////////////////////////////////////////
Power (5V) is provided to the Arduino pro mini by the FTDI programmer
Gyro - Arduino pro mini
VCC - 5V
GND - GND
SDA - A4
SCL - A5
LCD - Arduino pro mini
VCC - 5V
GND - GND
SDA - A4
SCL - A5
*//////////////////////////////////////////////////////////////////////////////////////
//Include LCD and I2C library
//#include <LiquidCrystal_I2C.h>
#include <Wire.h>
//Declaring some global variables
int gyro_x, gyro_y, gyro_z;
long acc_x, acc_y, acc_z, acc_total_vector;
int temperature;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
long loop_timer;
int lcd_loop_counter;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
boolean set_gyro_angles;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch_output, angle_roll_output;
void setup() {
Wire.begin(); //Start I2C as master
//Serial.begin(57600); //Use only for debugging
pinMode(LED_BUILTIN, OUTPUT); //Set output 13 (LED) as output
setup_mpu_6050_registers(); //Setup the registers of the MPU-6050 (500dfs / +/-8g) and start the gyro
digitalWrite(LED_BUILTIN, HIGH); //Set digital output 13 high to indicate startup
SerialUSB.begin(115200); //Initialize the LCD
SerialUSB.println("begin");
for (int cal_int = 0; cal_int < 2000 ; cal_int ++){ //Run this code 2000 times
if(cal_int % 75 == 0) SerialUSB.print("*"); //Print a dot on the LCD every 125 readings
read_mpu_6050_data(); //Read the raw acc and gyro data from the MPU-6050
gyro_x_cal += gyro_x; //Add the gyro x-axis offset to the gyro_x_cal variable
gyro_y_cal += gyro_y; //Add the gyro y-axis offset to the gyro_y_cal variable
gyro_z_cal += gyro_z; //Add the gyro z-axis offset to the gyro_z_cal variable
delay(3); //Delay 3us to simulate the 250Hz program loop
}
gyro_x_cal /= 2000; //Divide the gyro_x_cal variable by 2000 to get the average offset
gyro_y_cal /= 2000; //Divide the gyro_y_cal variable by 2000 to get the average offset
gyro_z_cal /= 2000; //Divide the gyro_z_cal variable by 2000 to get the average offset
/*lcd.clear(); //Clear the LCD
lcd.setCursor(0,0); //Set the LCD cursor to position to position 0,0
lcd.print("Pitch:"); //Print text to screen
lcd.setCursor(0,1); //Set the LCD cursor to position to position 0,1
lcd.print("Roll :"); */ //Print text to screen
digitalWrite(LED_BUILTIN, LOW); //All done, turn the LED off
// loop_timer = micros(); //Reset the loop timer
}
void loop(){
read_mpu_6050_data(); //Read the raw acc and gyro data from the MPU-6050
gyro_x -= gyro_x_cal; //Subtract the offset calibration value from the raw gyro_x value
gyro_y -= gyro_y_cal; //Subtract the offset calibration value from the raw gyro_y value
gyro_z -= gyro_z_cal; //Subtract the offset calibration value from the raw gyro_z value
//Gyro angle calculations
//0.0000611 = 1 / (250Hz / 65.5)
angle_pitch += gyro_x * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable
angle_roll += gyro_y * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_pitch += angle_roll * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel
//Accelerometer angle calculations
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Calculate the total accelerometer vector
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle
//Place the MPU-6050 spirit level and note the values in the following two lines for calibration
angle_pitch_acc -= -0.6; //Accelerometer calibration value for pitch
angle_roll_acc -= -2.7; //Accelerometer calibration value for roll
if(set_gyro_angles){ //If the IMU is already started
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle
}
else{ //At first start
angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle
angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle
set_gyro_angles = true; //Set the IMU started flag
}
//To dampen the pitch and roll angles a complementary filter is used
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value
SerialUSB.print("Pitch: ");
SerialUSB.print(angle_pitch_output);
SerialUSB.print(" roll: ");
SerialUSB.println(angle_roll_output); //Write the roll and pitch values to the LCD display
}
