Components and supplies
GY-521 MPU-6050 3 Axis Gyroscope + Accelerometer Module For Arduino
Arduino Nano R3
Project description
Code
MultiWii.ino
c_cpp
MultiWii.ino
c_cpp
Downloadable files
Schematics
Schematics
Comments
Only logged in users can leave comments
ju47ja
10 months ago
Nice was working on something similar, but needed it to be simpler Can you check this code out #include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" #include "Wire.h" #include <RF24.h> #include <nRF24L01.h> #include <SPI.h> #include <Servo.h> #include <NewPing.h> #define OUTPUT_READABLE_REALACCEL #define INTERRUPT_PIN 2 // index locations for data and instructions #define YAW 0 #define PITCH 1 #define ROLL 2 #define THROTTLE 3 #define X 0 //X axis #define Y 1 //Y axis #define Z 2 //Z axis MPU6050 mpu; // ------------motor variables // esc pins Servo motor_lf; Servo motor_lb; Servo motor_rf; Servo motor_rb; // throttle values for each motor int motor_lf_throttle {1000}, motor_lb_throttle {1000}, motor_rf_throttle {1000}, motor_rb_throttle {1000}; // MPU control/status vars bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer // orientation/motion vars Quaternion q; // [w, x, y, z] quaternion container VectorInt16 aa; // [x, y, z] accel sensor measurements VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements VectorFloat gravity; // [x, y, z] gravity vector float euler[3]; // [psi, theta, phi] Euler angle container float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector float errors[3]; float error_sum[3] = {0, 0, 0}; float previous_error[3] ={0, 0, 0}; float measures[3] = {0, 0, 0}; float Kp[3] = {2, 4, 4}; // p coefficients float Ki[3] = {0.004, 0.004, 0.004}; // i coefficients float Kd[3] = {4, 7, 7}; //{1.8, 1.5, 0}; //d coefficients float i {0}; float start_seconds {0}; float instruction[4] ={0, 4, 0, 0}; // ------------nrf24 variables RF24 radio(9, 10); // nRF24L01 (CE, CSN) const byte address[6] = { 0000, 0100, 0200, 0300, 0400, 0500 }; const byte remoteAddress = "1111"; unsigned long lastReceiveTime = 0; unsigned long currentTime = 0; // Max size of this struct is 32 bytes - NRF24L01 buffer limit struct Data_Package { byte jPotX; byte jPotY; byte jButton; byte button1; byte button2; byte button3; byte button4; byte button5; byte button6; byte button7; byte button8; }; Data_Package data; //Create a variable with the above structure volatile bool mpuInterrupt = true; // indicates whether MPU interrupt pin has gone high void dmpDataReady() { mpuInterrupt = true; } bool started = true; uint8_t encoded_details[43] = { 0 }; // Use this function to print out the encoded_details as a // space-delimited string of hexadecimal characters. void dumpRegData() { for (uint8_t i = 0; i < 43; ++i) { Serial.print(encoded_details[i], HEX); if (i < 42) Serial.print(F(" ")); } } void setup() { Serial.begin(115200); Wire.begin(); Serial.println( "wire began"); SPI.begin(); Serial.println( "spi began"); radio.begin(); Serial.println( "radio began"); radio.openReadingPipe(0, remoteAddress); Serial.println( "reading began"); radio.setAutoAck(false); radio.setDataRate(RF24_2MBPS); radio.setPALevel(RF24_PA_MAX); radio.startListening(); // Set the module as receiver Serial.println( "listening began"); mpu.initialize(); Serial.println( "mpu began"); pinMode(INTERRUPT_PIN, INPUT); Serial.println(mpu.testConnection() ? F("mputest connection began") : F("MPU6050 connection failed")); devStatus = mpu.dmpInitialize(); Serial.println( "dmp began"); mpu.setXGyroOffset(220); mpu.setYGyroOffset(76); mpu.setZGyroOffset(-85); mpu.setZAccelOffset(1788); // 1688 factory default for my test chip motor_lf.attach(8, 1000, 2000); motor_lb.attach(4, 1000, 2000); motor_rf.attach(2, 1000, 2000); motor_rb.attach(3, 1000, 2000); if (devStatus == 0) { // Calibration Time: generate offsets and calibrate our MPU6050 mpu.CalibrateAccel(6); mpu.CalibrateGyro(6); mpu.PrintActiveOffsets(); // turn on the DMP, now that it's ready mpu.setDMPEnabled(true); // enable Arduino interrupt detection attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus(); // set our DMP Ready flag so the main loop() function knows it's okay to use it dmpReady = true; // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); } else { devStatus = 0;} delay(2000); started = true; } void sensor(){ const int sonar_num = 3; int max_distance = 370; NewPing sonar[sonar_num] = { // Sensor object array. NewPing(5, 5, max_distance), // Each sensor's trigger pin, echo pin, and max distance to ping. NewPing(6, 6, max_distance), NewPing(7, 7, max_distance) }; double getDistance[sonar_num]; for (uint8_t i = 0; i < sonar_num; i++) { getDistance[i] = sonar[i].ping_cm(); }; } void yawpitchroll() { if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); ypr[0] = ypr[0] * 180/M_PI; ypr[1] = ypr[1] * 180/M_PI; ypr[2] = ypr[2] * 180/M_PI; } } void gyro() { if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetEuler(euler, &q); double getAngularVelocityX = euler[0] * 180/M_PI; double getAngularVelocityY = euler[1] * 180/M_PI; double getAngularVelocityZ = euler[2] * 180/M_PI; } } void accelerometer() { if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); double getAccelerationX = aaReal.x; double getAccelerationY = aaReal.y; double getAccelerationZ = aaReal.z; return(getAccelerationX, getAccelerationY, getAccelerationZ); } } void applyMotorSpeeds(){ // Serial.print(motor_lf_throttle); // Serial.print("\t"); // Serial.print(motor_rf_throttle); // Serial.print("\t"); // Serial.print(motor_lb_throttle); // Serial.print("\t"); // Serial.print(motor_rb_throttle); // scale values to be within acceptable range for motors motor_lf_throttle = map(motor_lf_throttle, 1000, 2000, 0, 180); motor_rf_throttle = map(motor_rf_throttle, 1000, 2000, 0, 180); motor_lb_throttle = map(motor_lb_throttle, 1000, 2000, 0, 180); motor_rb_throttle = map(motor_rb_throttle, 1000, 2000, 0, 180); motor_lf.write(motor_lf_throttle); motor_rf.write(motor_rf_throttle); motor_lb.write(motor_lb_throttle); motor_rb.write(motor_rb_throttle); } float minMax(float value, float min_value, float max_value) { if (value > max_value) { value = max_value; } else if (value < min_value) { value = min_value; } return value; } void stopMotors() { motor_lf.write(0); motor_rf.write(0); motor_lb.write(0); motor_rb.write(0); } void calculateAnglesv2() { // wait for mpu interupt of extra packets available while (!mpuInterrupt && fifoCount < packetSize) { if (mpuInterrupt && fifoCount < packetSize) { // try to get out of the infinite loop-----limbo fifoCount = mpu.getFIFOCount(); } } // reset interrupt flag and get int status byte mpuInterrupt = false; mpuIntStatus = mpu.getIntStatus(); // get current fifo count fifoCount = mpu.getFIFOCount(); // check for overflow (this should never happen unless our code is insufficient) if (true) { // reset so we can continue cleanly mpu.resetFIFO(); // track fifo count here incase there >1 packet available // (this lets us immediately read more without waiting for an interrupt) mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(measures, &q, &gravity); measures[YAW] = measures[YAW]* 180/M_PI; measures[PITCH] = measures[PITCH]* 180/M_PI; measures[ROLL] = measures[ROLL]* 180/M_PI; // Serial.print(measures[YAW]); // Serial.print("\t"); // Serial.print(measures[PITCH]); // Serial.print("\t"); // Serial.print(measures[ROLL]); } // } // calculate errors of yaw, pitch, roll; this is simply the measure - the command void calculateErrors() { // note- currently roll measurements are very noisy // consider removing from pid calculations? errors[YAW] = instruction[YAW] - measures[YAW]; errors[PITCH] = instruction[PITCH] -measures[PITCH]; errors[ROLL] = instruction[ROLL] - measures[ROLL]; // Serial.print(errors[YAW]); // Serial.print("\t"); // Serial.print(errors[PITCH]); // Serial.print("\t"); // Serial.print(errors[ROLL]); } void pidController() { float delta_err[3] = {0, 0, 0}; float yaw_pid = 0; float pitch_pid = 0; float roll_pid = 0; // take no action if there is no throttle to the motors if(instruction[THROTTLE] > 0) { // calculate sum of errors : integral coefficients error_sum[YAW] += errors[YAW]; error_sum[PITCH] += errors[PITCH]; error_sum[ROLL] += errors[ROLL]; // calsulate error delta :derivative coefficients delta_err[YAW] = errors[YAW] - previous_error[YAW]; delta_err[PITCH] = errors[PITCH] - previous_error[PITCH]; delta_err[ROLL] = errors[ROLL] - previous_error[ROLL]; // save current error as previous_error for next time previous_error[YAW] = errors[YAW]; previous_error[PITCH] = errors[PITCH]; previous_error[ROLL] = errors[ROLL]; // pid = e.Kp + intSe.Ki + deltae.Kd yaw_pid = (errors[YAW] * Kp[YAW]) + (error_sum[YAW] * Ki[YAW]) + (delta_err[YAW] * Kd[YAW]); pitch_pid = (errors[PITCH] * Kp[PITCH]) + (error_sum[PITCH] * Ki[PITCH]) + (delta_err[PITCH] * Kd[PITCH]); roll_pid = (errors[ROLL] * Kp[ROLL]) + (error_sum[ROLL] * Ki[ROLL]) + (delta_err[ROLL] * Kd[ROLL]); // calculate new target throttle for each motor // note; these depends on setup of drone, verify setup is proper and consider changing the plus and minuses here if issues happen // if drone is in proper setup these make sense motor_lf_throttle = instruction[THROTTLE] + roll_pid + pitch_pid - yaw_pid; motor_rb_throttle = instruction[THROTTLE] - roll_pid + pitch_pid + yaw_pid; // back motors are way more powerful motor_lb_throttle = instruction[THROTTLE] + roll_pid - pitch_pid + yaw_pid; motor_rb_throttle = instruction[THROTTLE] - roll_pid - pitch_pid - yaw_pid; } } void readController() { // read new packet data // Check whether we keep receving data, or we have a connection between the two modules currentTime = millis(); if ( currentTime - lastReceiveTime > 2000 ) { // If current time is more then 1 second since we have recived the last data, that means we have lost connection // Reset the values when there is no radio connection - Set initial default values data.jPotX = 0; data.jPotY = 1300; data.jButton = 1; data.button1 = 0; data.button2 = 0; data.button3 = 0; data.button4 = 0; data.button5 = 0; data.button6 = 0; data.button7 = 0; data.button8 = 0; }; // If connection is lost, reset the data. It prevents unwanted behavior, for example if a drone has a throttle up and we lose connection, it can keep flying unless we reset the values // Check whether there is data to be received radio.startListening(); if (radio.available()) { started = true; radio.read(&data, sizeof(Data_Package)); // Read the whole data and store it into the 'data' structure Serial.println("started"); Serial.print(data.jPotX); Serial.print(data.jPotY); Serial.print(data.jButton); Serial.print(data.button1); Serial.print(data.button2); Serial.print(data.button3); Serial.print(data.button4); Serial.print(data.button5); Serial.print(data.button6); Serial.print(data.button7); Serial.println(data.button8); lastReceiveTime = millis(); // At this moment we have received the data } // Print the data in the Serial Monitor if (data.button7) { instruction[THROTTLE] += 100; } if (data.button8) { instruction[THROTTLE] -= 100; } //turn right if (data.button4) { instruction[ROLL] += 15; } // turn left if (data.button5) { instruction[ROLL] -= 15; } // move backward if (data.button2) { // started = true; instruction[PITCH] += 15; } else { while ( instruction[PITCH] > 0 && instruction[THROTTLE] == 0){ instruction[PITCH] -= 1; } } // move forward if (data.button3) { // started = false; instruction[PITCH] -= 15; } else { while (instruction[PITCH] < 0 && instruction[THROTTLE] == 0){ instruction[PITCH] += 1; } } if (data.button8) { stopMotors(); } // // perform yaw // if (data.button8) { // instruction[YAW] = 0; // while (instruction[YAW] < 359) { // instruction[YAW] += 6; // delay(40); // } // } // // perform roll // if (data.button5) { // instruction[ROLL] = 0; // while (instruction[ROLL] < 359) { // instruction[ROLL] += 6; // delay(40); // } // } // hover if (data.button1) { instruction[YAW] = 0; instruction[PITCH] = 0; instruction[ROLL] = 0; } delay(100); } void loop() { // if (!dmpReady) { // // code to transmit led ON on remote // return; // } radio.startListening(); // code for calculating the running frequency of the program // i++; // Serial.println(i/(millis()/1000-start_seconds)); // 3 read input from nrf24 reomte controller readController(); // if (radio.available()) { // radio.read(&data, sizeof(Data_Package)); // Serial.print(data.jButton); // Serial.print(data.button1); // Serial.print(data.button2); // Serial.print(data.button3); // Serial.print(data.button4); // Serial.print(data.button5); // Serial.print(data.button6); // Serial.print(data.button7); // Serial.println(data.button8); // } // lastReceiveTime = millis(); // At this moment we have received the data started = true; instruction[YAW] = 0; instruction[PITCH] = 0; instruction[ROLL] = 0; for (int pos = 1000; pos <= 2000; pos += 100) { // goes from 0 degrees to 180 degrees // in steps of 1 degree instruction[THROTTLE] = pos; // tell servo to go to position in variable 'pos' calculateAnglesv2(); calculateErrors(); pidController(); applyMotorSpeeds(); delay(150); // waits 15 ms for the servo to reach the position } for (int pos = 2000; pos >= 1000; pos -= 50) { // goes from 180 degrees to 0 degrees instruction[THROTTLE] = pos; // tell servo to go to position in variable 'pos' calculateAnglesv2(); calculateErrors(); pidController(); applyMotorSpeeds(); delay(70); // waits 15 ms for the servo to reach the position } // // 2 calculate actual angles from raw data // calculateAnglesv2(); // // 4 calculate errors based on new inputs // calculateErrors(); // if (started) { // // 5 calculate new motor speeds // pidController(); // applyMotorSpeeds(); // } // else{ // stopMotors(); // } }
yoavdelarea
a year ago
Thank you for the wonderful blog Can I use arguing uno with the same code?
Anonymous user
2 years ago
This article very good!,But you should introduce how to use GPS Module
hailass
2 years ago
this is a wonderfull work which support begginers.
Anonymous user
2 years ago
you are providing 11.1V directly to this tiny arduino....are you sure about that? also you mentioned bluetooth connectivity via RX and TX but they are not shown in schematics. also why you used it?
Anonymous user
2 years ago
Yes , you are correct...... I am sure arduino Nano can handle 12v but current from lipo is to high for arduino Nano to handle it's not safe for arduino + while flying it can also cause serious problem So, I don't prefer to use lipo directly on aduino.
JimRussell
2 years ago
This looks like a really sexy project, must give it a try. As for the 11.1v supply, it's ok, the nano has a built in 5v regulator so can take about 12v on that pin no problem. Also, I bought a few nanos for other projects on ebay really cheaply.
AKJ
2 years ago
I think the schematics are incomplete as the motors are not shown connected to the controller board too. 12 V to the arduino Nano is also not a good idea.
Anonymous user
2 years ago
Yes , I am agree with you ,we can use BEC to it
Anonymous user
2 years ago
I have completed the flight controller and have built the frame for a school project. I am trying to use a Bluetooth connection instead of having to buy an expensive RC controller. I am using a HC05 Bluetooth chip to do so. When I connected the drone to the Samsung smart phone on the EZ GUI ground station app, it immediately says "Warning no data received". I made sure that the connections were properly made between the Bluetooth chip and the Arduino, and it still says the same thing. I researched into the warning and it says I need to modify the baud rate of the Bluetooth chip. I am new to Arduino projects and coding so my knowledge is very limited in this field. I was wondering if anyone could give advice on how to fix this baud rate issue in the multiwii code. Thanks.
Anonymous user
2 years ago
I think you can use 433Mhz FS1000A transmitter and receiver to talk with your copter upto 350m in open sky.
Anonymous user
2 years ago
I think you can also use the NRF24L01+ module (https://robu.in/product/2-4ghz-nrf24l01palna-sma-antenna-wireless-transceiver-communication-module-1km/?gclid=Cj0KCQiArt6PBhCoARIsAMF5waiJJ4-Fmjenc7v2cGkPbjI6ykgr1qP1jdtfVYbqeABIZjpgU2ASEUAaApQFEALw_wcB) for both transmitter and receiver. You would however have to make your own transmitter. I am planning to build my own quadcoper using arduino flight controller as well as a transmitter made using Arduino and NRF24L01+. I found this great diy transmitter here https://howtomechatronics.com/projects/diy-arduino-rc-transmitter/ and the receiver https://howtomechatronics.com/projects/diy-arduino-rc-receiver/ Maybe this is something that you can try as well :)
Anonymous user
2 years ago
can I use normal Arduino uno for this project . with the same code . but what about connections
Anonymous user
2 years ago
when we want to use GPS, we only use I2C bus ,Because the seril wire was used other port
Anonymous user
2 years ago
What programming tools used by this project ?
Anonymous user
2 years ago
Ola! ficou otimo ! gostaria de ver este projeto funcionado com barometro e gps .bom gps seria muito bom.
rachidrobot
2 years ago
i dont inderstand the way to grouped al these betwen us
Anonymous user
2 years ago
I would like the quadcopter with firefighters to avoid fire
Anonymous user
3 years ago
Ola! ficou otimo ! gostaria de ver este projeto funcionado com barometro e gps .bom gps seria muito bom.
rachidrobot
3 years ago
i dont inderstand the way to grouped al these betwen us
hailass
4 years ago
this is a wonderfull work which support begginers.
Anonymous user
6 years ago
I have completed the flight controller and have built the frame for a school project. I am trying to use a Bluetooth connection instead of having to buy an expensive RC controller. I am using a HC05 Bluetooth chip to do so. When I connected the drone to the Samsung smart phone on the EZ GUI ground station app, it immediately says "Warning no data received". I made sure that the connections were properly made between the Bluetooth chip and the Arduino, and it still says the same thing. I researched into the warning and it says I need to modify the baud rate of the Bluetooth chip. I am new to Arduino projects and coding so my knowledge is very limited in this field. I was wondering if anyone could give advice on how to fix this baud rate issue in the multiwii code. Thanks.
Anonymous user
2 years ago
I think you can use 433Mhz FS1000A transmitter and receiver to talk with your copter upto 350m in open sky.
Anonymous user
2 years ago
I think you can also use the NRF24L01+ module (https://robu.in/product/2-4ghz-nrf24l01palna-sma-antenna-wireless-transceiver-communication-module-1km/?gclid=Cj0KCQiArt6PBhCoARIsAMF5waiJJ4-Fmjenc7v2cGkPbjI6ykgr1qP1jdtfVYbqeABIZjpgU2ASEUAaApQFEALw_wcB) for both transmitter and receiver. You would however have to make your own transmitter. I am planning to build my own quadcoper using arduino flight controller as well as a transmitter made using Arduino and NRF24L01+. I found this great diy transmitter here https://howtomechatronics.com/projects/diy-arduino-rc-transmitter/ and the receiver https://howtomechatronics.com/projects/diy-arduino-rc-receiver/ Maybe this is something that you can try as well :)
Anonymous user
6 years ago
sEnÄ°n BeN aNa sKm KĂ¼TĂ¼PaNeYi aNaNıN aMıNdAn Mı BuLcAz Oç aNa sKm
Bartcephus
2 years ago
That's what I said.... (LOL)
Anonymous user
2 years ago
what do you mean? -------------------------------
Alsharif
6 years ago
I would like the quadcopter with firefighters to avoid fire
Anonymous user
6 years ago
What programming tools used by this project ?
Anonymous user
6 years ago
when we want to use GPS, we only use I2C bus ,Because the seril wire was used other port
Anonymous user
6 years ago
can I use normal Arduino uno for this project . with the same code . but what about connections
Anonymous user
6 years ago
This article very good!,But you should introduce how to use GPS Module
Anonymous user
6 years ago
you are providing 11.1V directly to this tiny arduino....are you sure about that? also you mentioned bluetooth connectivity via RX and TX but they are not shown in schematics. also why you used it?
Anonymous user
2 years ago
This looks like a really sexy project, must give it a try. As for the 11.1v supply, it's ok, the nano has a built in 5v regulator so can take about 12v on that pin no problem. Also, I bought a few nanos for other projects on ebay really cheaply.
AKJ
2 years ago
I think the schematics are incomplete as the motors are not shown connected to the controller board too. 12 V to the arduino Nano is also not a good idea.
Anonymous user
2 years ago
Yes , I am agree with you ,we can use BEC to it
Anonymous user
2 years ago
Yes , you are correct...... I am sure arduino Nano can handle 12v but current from lipo is to high for arduino Nano to handle it's not safe for arduino + while flying it can also cause serious problem So, I don't prefer to use lipo directly on aduino.
robocircuits
19 Followers
•11 Projects
126
38
deathrider6
7 months ago
is it possible to do this in a ardiuno uno r3 ?