Description
World's First Published Open-source Arduino Nano RP2040 Connect Drone Flight Controller
INTRODUCTION
The Arduino Nano RP2040 Connect comes with a built in IMU. It's fast speed and ability to program it with the Arduino IDE, it makes for a great microcontroller to serve as a quadcopter flight controller.
What's special about this project is that at the time of writing this article, there are no search results via Google, Bing, or Yahoo that can find a published opensource code for the Arduino Nano RP2040 Connect for flight control. This makes this project the first published open-source version!
HARDWARE
These are the components we are using for our drone:
- Microcontroller: Arduino Nano RP2040 Connect
- Motors: QWinOut A2212 1000KV Brushless Outrunner Motor 13T
- ESCs: QWinOut 2-4S 30A/40A RC Brushless ESC Simonk Firmware Electric Speed Controller with 5V 3A BEC
- Battery: HRB 4S 3300mAh 14.8v Lipo RC Battery 60C
This hardware works with the code out of the box. The drone frame is up to the builder.
PCB DESIGN CONSIDERATIONS
The biggest design consideration for this board was the logic level. The RP2040 GPIO is designed for 3.3V and not 5V tolerant. However, the ESCs and the radio receiver communicate on 5V. So, any pin to the RP2040 that was receiving a signal required a voltage divider to protect the RP2040.
The second consideration was battery management. Many of the LiPo batteries available are long lasting with a ton of amps to deliver quickly but must not be allowed to go under their minimum cell voltage. We ballooned a LiPo once in the house. It's terrifying!
In turn, a means of letting the drone pilot know the battery is draining too low had to be designed. We achieved this by applying a voltage divider here as well. In our case, we had a 16.8V (full charge of a 4S battery) divided to just under 3Vs. The closer the battery approaches undervoltage (12.8V), the beeper will increase its beep rate until it is too annoying to want to continue use.
Otherwise, the rest of the design was just focused on providing reliable wire hookups between the drone receiver and motor electronic speed controllers (ESCs).
CODE
Here are some key aspects to our code:
- 100% Open Source (you can see how it is possible to fly!)
- Web Interface to allow flight controller tuning with your mobile phone
- Github Repository
FLIGHT CONTROLLER TUNING
The Maker revolution has been driven a lot by the availability of the IMU. The key to a stable and responsive drone is all about the coded PID calculations. However, those calculations have gain parameters that must be dialed in for your drone design. Store bought drones are already dialed in. There's no chance that a custom built drone won't need some degree of tuning.
So, we integrated a web server in our code. This allows us to connect with the drone on startup and feed it constants to help dial in the tuning. Below is a snapshot of one of the interface pages. It uses Bootstrap responsive styling to give a modern and ever evolving look-and-feel.
Code
Rawpter V1.2
C/C++
//Rawpter 1.7 by Sean J. Miller //Flight Controller code for an Arduino Nano RP2040 Connect //Go to https://raisingawesome.site/projects for more info //MIT License - use at your own risk #include <SPI.h> #include <WiFiNINA.h> //WiFi #include <Arduino_LSM6DSOX.h> //IMU get from the Arduino IDE Library Manager #include <Servo.h> //get from the Arduino IDE Library Manager //========================================================================================================================// // USER-SPECIFIED VARIABLES // //========================================================================================================================// //EASYCHAIR is used to put in some key dummy variables so you can sit in the easy chair with just the Arduino on a cord wiggling it around and seeing how it responds. //Set EASYCHAIR to false when you are wanting to install it in the drone. #define EASYCHAIR false //The LOOP_TIMING is based on the IMU. For the Arduino_LSM6DSOX, it is 104Hz. So, the loop time is set a little longer so the IMU has time to update from the control change. #define LOOP_TIMING 100 //The battery alarm code handles 14.8 or 7.4V LIPOs. Set to your type of battery. If not hooked up, it will pull down and beep often. #define BATTERYTYPE 14.8 #define BUZZER_PIN 9 int batteryVoltage = 777; //just a default for the battery monitoring routine unsigned long next_voltage_check = 0; //used in loopBuzzer to check for voltage on the main battery. bool beeping = false; //For tracking beeping when the battery is getting low. //Radio failsafe values for every channel in the event that bad reciever data is detected. //These are for it to stay stable and descend safely versus totally cutting throttle and drop like a rock. unsigned long PWM_throttle_zero = 1000; //used when we want to take throttle to zero. Failsafe is something higher as it is expected that failsafe is a value needed to safely land. unsigned long PWM_throttle_fs = 1000; //throttle will allow it to descend to the ground if you adjust the throttlecutswitch_fs to 2000 unsigned long PWM_roll_fs = 1500; //ail pretty much in the middle so it quits turning unsigned long PWM_elevation_fs = 1500; //elev unsigned long PWM_rudd_fs = 1500; //rudd unsigned long PWM_ThrottleCutSwitch_fs = 1000; //SWA less than 1300, cut throttle - must config a switch to Channel 5 in your remote. bool UPONLYMODE = false; //UPONLYMODE is used to take the radio out of it other than thrust. The intent is to get it tuned so it can at least take off. Should be false once tuned. float stick_dampener = 0.1; //0.1-1 Lower=slower, higher=noiser default 0.7 bool failsafed = false; //IMU calibration parameters - calibrate IMU using calculate_IMU_error() in the void setup() to get these values, then comment out calculate_IMU_error() float AccErrorX = -0.03; float AccErrorY = 0.00; float AccErrorZ = 0.01; float GyroErrorX = 0.37; float GyroErrorY = -0.04; float GyroErrorZ = -0.45; float Gyro_filter = .97; float Accel_filter = .97; float B_madgwick = 0.02; //(default 0.04) float q0 = 1.0f; //Initialize quaternion for madgwick filter float q1 = 0.0f; float q2 = 0.0f; float q3 = 0.0f; //Controller parameters (this is where you "tune it". It's best to use the WiFi interface to do it live and then update once its tuned.): float i_limit = 60; //Integrator saturation level, mostly for safety (default 25.0) float maxRoll = 18.0; //Max roll angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode (default 30.0) float maxPitch = 18.0; //Max pitch angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode (default 30.0) float maxYaw = 10.0; //Max yaw rate in deg/sec (default 160.0) float maxMotor = 0.8; float Kp_roll_angle = 10; //Roll P-gain float Ki_roll_angle = 8; //Roll I-gain float Kd_roll_angle = 2; //Roll D-gain float Kp_pitch_angle = 15; //Pitch P-gain float Ki_pitch_angle = 8; //Pitch I-gain float Kd_pitch_angle = 2; //Pitch D-gain float Kp_yaw = 0; //Yaw P-gain default 30 float Ki_yaw = 0; //Yaw I-gain default 5 float Kd_yaw = 0; //Yaw D-gain default .015 (be careful when increasing too high, motors will begin to overheat!) //========================================================================================================================// // DECLARE PINS // //========================================================================================================================// //Radio: //used so I quit getting confused begween the ARduino pins and the Radio Channel pins. const int stickRightHorizontal = 2; //right horizontal stick const int stickRightVertical = 3; //right vertical stick const int stickLeftVertical = 4; //left vertical stick const int stickLeftHorizontal = 5; //left horizontal stick const int SwitchA = 6; //SWA switch const int throttlePin = stickLeftVertical; //throttle - up and down on the const int rollPin = stickRightHorizontal; //ail (roll) const int upDownPin = stickRightVertical; //ele const int ruddPin = stickLeftHorizontal; //rudd const int throttleCutSwitchPin = SwitchA; //gear (throttle cut) //variables for reading PWM from the radio receiver unsigned long rising_edge_start_1, rising_edge_start_2, rising_edge_start_3, rising_edge_start_4, rising_edge_start_5, rising_edge_start_6; unsigned long channel_1_raw = 0; unsigned long channel_2_raw = 0; unsigned long channel_3_raw = 0; unsigned long channel_4_raw = 0; unsigned long channel_5_raw = 0; int ppm_counter = 0; unsigned long time_ms = 0; int throttleCutCounter = 0; int throttleNotCutCounter = 0; //Motor Electronic Speed Control Modules (ESC): const int m1Pin = 10; //10 const int m2Pin = 15; //15 const int m3Pin = 16; //16 const int m4Pin = 14; //14 Servo m1PWM, m2PWM, m3PWM, m4PWM; //========================================================================================================================// //DECLARE GLOBAL VARIABLES //========================================================================================================================// //General stuff for controlling timing of things float deltaTime; float invFreq = (1.0 / LOOP_TIMING) * 1000000.0; unsigned long current_time, prev_time; unsigned long print_counter, serial_counter; bool throttle_is_cut = true; //used to force the pilot to manually set the throttle to zero after the switch is used to throttle cut //Radio communication: unsigned long PWM_throttle, PWM_roll, PWM_Elevation, PWM_Rudd, PWM_ThrottleCutSwitch; unsigned long PWM_throttle_prev, PWM_roll_prev, PWM_Elevation_prev, PWM_Rudd_prev; //IMU: float AccX, AccY, AccZ; float AccX_prev, AccY_prev, AccZ_prev; float GyroX, GyroY, GyroZ; float GyroX_prev, GyroY_prev, GyroZ_prev; float roll_IMU, pitch_IMU, yaw_IMU; float roll_IMU_prev, pitch_IMU_prev; //Normalized desired state: float thro_des, roll_des, pitch_des, yaw_des; //Controller: float error_roll, error_roll_prev, roll_des_prev, integral_roll, integral_roll_il, integral_roll_ol, integral_roll_prev, integral_roll_prev_il, integral_roll_prev_ol, derivative_roll, roll_PID = 0; float error_pitch, error_pitch_prev, pitch_des_prev, integral_pitch, integral_pitch_il, integral_pitch_ol, integral_pitch_prev, integral_pitch_prev_il, integral_pitch_prev_ol, derivative_pitch, pitch_PID = 0; float error_yaw, error_yaw_prev, integral_yaw, integral_yaw_prev, derivative_yaw, yaw_PID = 0; //Mixer float m1_command_scaled, m2_command_scaled, m3_command_scaled, m4_command_scaled; int m1_command_PWM, m2_command_PWM, m3_command_PWM, m4_command_PWM; unsigned long buzzer_millis; unsigned long buzzer_spacing = 10000; //WiFi Begin int keyIndex = 0; int status = WL_IDLE_STATUS; bool ALLOW_WIFI = true; WiFiServer server(80); //WiFi End //========================================================================================================================// //BEGIN THE CLASSIC SETUP AND LOOP //========================================================================================================================// void setup() { //Bootup operations analogReadResolution(12); //The RP2040 has a 12 bit ADC versus the standard 10 from the Uno setupSerial(); if (ALLOW_WIFI) setupWiFi(); //At first power on, a WiFi hotspot is set up for talking to the drone. (SSID Rawpter, 12345678) setupDrone(); setupBatteryMonitor(); } void loop() { tick(); //stamp the start time of the loop to keep our timing to 100Hz. See tock() below. if (throttle_is_cut) { //We are only going to allow these to slow down the loop if the throttle is cut. Otherwise, the loop speed is too irregular. loopBuzzer(); if (ALLOW_WIFI) loopWiFi(); } loopDrone(); tock(); } void setupDrone() { //Initialize all pins pinMode(m1Pin, OUTPUT); pinMode(m2Pin, OUTPUT); pinMode(m3Pin, OUTPUT); pinMode(m4Pin, OUTPUT); m1PWM.attach(m1Pin, 1060, 1860); m2PWM.attach(m2Pin, 1060, 1860); m3PWM.attach(m3Pin, 1060, 1860); m4PWM.attach(m4Pin, 1060, 1860); //Set built in LED to turn on to signal startup delay(5); //Initialize radio communication radioSetup(); setToFailsafe(); PWM_throttle = PWM_throttle_zero; //zero may not necessarily be the failsafe, but on startup we want zero. //Initialize IMU communication IMUinit(); //Get IMU error to zero accelerometer and gyro readings, assuming vehicle is level when powered up // calculate_IMU_error(); //Calibration parameters printed to serial monitor. Paste these in the user specified variables section, then comment this out forever. if (getRadioPWM(1) > 1800 && getRadioPWM(1) < 2400 & !EASYCHAIR) calibrateESCs(); //if the throttle is up, first calibrate ESCs before going into the loop m1_command_PWM = 0; //Will send the default for motor stopped for Simonk firmware m2_command_PWM = 0; m3_command_PWM = 0; m4_command_PWM = 0; while (getRadioPWM(1) > 1060 && getRadioPWM(1) < 2400 && !EASYCHAIR && getRadioPWM(5) < 1300) //wait until the throttle is turned down and throttlecut switch is not engaged before allowing anything else to happen. { delay(1000); } } void loopDrone() { getIMUdata(); //Pulls raw gyro andaccelerometer data from IMU and applies LP filters to remove noise Madgwick6DOF(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ); //Updates roll_IMU, pitch_IMU, and yaw_IMU angle estimates (degrees) getDesiredAnglesAndThrottle(); //Convert raw commands to normalized values based on saturated control limits PIDControlCalcs(); //The PID functions. Stabilize on angle setpoint from getDesiredAnglesAndThrottle controlMixer(); //Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here scaleCommands(); //Scales motor commands to 0-1 throttleCut(); //Directly sets motor commands to off based on channel 5 being switched #if EASYCHAIR Troubleshooting(); //will do the print routines that are uncommented #endif commandMotors(); //Sends command pulses to each ESC pin to drive the motors getRadioSticks(); //Gets the PWM from the radio receiver failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup } void setupBatteryMonitor() { buzzer_millis = millis(); pinMode(BUZZER_PIN, OUTPUT); pinMode(A6, INPUT); } void loopBuzzer() { //this monitors the battery. the lower it gets, the faster it beeps. unsigned long myTime=millis(); if (!beeping) { if (myTime - buzzer_millis > (buzzer_spacing)) { digitalWrite(BUZZER_PIN, HIGH); beeping = true; buzzer_millis = myTime; } } else { if (myTime - buzzer_millis > 80) { beeping = false; buzzer_millis = myTime; digitalWrite(BUZZER_PIN, LOW); } } if (myTime > next_voltage_check) { next_voltage_check = myTime + 10000; //checkvoltage once every 10 seconds versus every loop. batteryVoltage = analogRead(A6); if (BATTERYTYPE == 14.8) { //based on 330K and 51K voltage divider that takes 16.8V to 2.25V if (batteryVoltage > 1457) buzzer_spacing = 40000; else if (batteryVoltage > 1440 ) buzzer_spacing = 30000; else if (batteryVoltage > 1400) buzzer_spacing = 20000; else if (batteryVoltage > 1350) buzzer_spacing = 10000; else if (batteryVoltage > 1301) buzzer_spacing = 500; else buzzer_spacing = 100; } else { //Depends on your resistors and battery choice } } } void Troubleshooting() { //Print data at 100hz (uncomment one at a time for troubleshooting) //printRadioData(); //Prints radio pwm values (expected: 1000 to 2000) //printDesiredState(); //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle) //printGyroData(); //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest) //printAccelData(); //Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level) //printRollPitchYaw(); //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level) //printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1) //printMotorCommands(); //Prints the values being written to the motors (expected: 1000 to 2000) //printtock(); //Prints the time between loops in microseconds (expected: microseconds between loop iterations and less the Gyro/Acc update speed. Set by LOOP_TIMING) #if EASYCHAIR printJSON(); #endif } void setupSerial() { Serial.begin(2000000); delay(100); } void setupWiFi() { char ssid[] = "_Rawpter"; char pass[] = "12345678"; // check for the WiFi module: if (WiFi.status() == WL_NO_MODULE) { // don't continue because this is probably not an RP2040 while (true) ; } String fv = WiFi.firmwareVersion(); if (fv < WIFI_FIRMWARE_LATEST_VERSION) { Serial.println("Please upgrade the firmware"); } // Just picked this out of the air. Throw back to Jeff Gordon WiFi.config(IPAddress(192, 168, 2, 4)); // Create open network. Change this line if you want to create an WEP network: status = WiFi.beginAP(ssid, pass); if (status != WL_AP_LISTENING) { // don't continue while (true) ; } // wait 1 seconds for connection: delay(1000); // start the web server on port 80 server.begin(); } void loopWiFi() { // compare the previous status to the current status WiFiClient client = server.available(); // listen for incoming clients if (client) { // if you get a client, String currentLine = ""; // make a String to hold incoming data from the client while (client.connected()) { // loop while the client's connected if (client.available()) { // if there's bytes to read from the client, char c = client.read(); // read a byte, then if (c == '\n') { // if the current line is blank, you got two newline characters in a row. // that's the end of the client HTTP request, so send a response: if (currentLine.length() == 0) { break; } else { // if you got a newline, then clear currentLine: currentLine = ""; } } else if (c != '\r') { // if you got anything else but a carriage return character, currentLine += c; // add it to the end of the currentLine } if (currentLine.endsWith("GET /?")) { setTheValuesFromUserForm(client); //Handle clicking the Begin button MakeWebPage(client, "<meta http-equiv = \"refresh\" content = \"0; url = http://192.168.2.4 \"/>"); } else if (currentLine.endsWith("GET / HTTP")) { //Handle hitting the basic page (1st connection) String myMsg = "<h1>Rawpter V1.7</h1><small>by Raising Awesome</small><br>"; //For the battery voltage calc, you can use ohm's law on your chosen voltage divider resistors and get the voltage ratio of the 12bit ADC. I simply recorded values against fed voltages from my bench power supply and fit //a line. Good ole' y=mx+b. myMsg += "<b>Snapshot:</b><br>Desired Roll=" + String(roll_des) + "° IMU Roll=" + String(roll_IMU) + "°<br>Desired Pitch= " + String(pitch_des) + "° IMU Pitch=" + String(pitch_IMU) + "°<br>Loop Time= " + String(int(round(1 / (deltaTime)))) + " Throttle PWM=" + String(PWM_throttle) + "<br>Battery=" + String( (4.4695/104.0+((float)batteryVoltage/104.0)),1) + "V (" + String(batteryVoltage) + ")<br>"; if (batteryVoltage<1350) myMsg+="<br><div class='alert alert-danger'>DANGER: BATTERY LOW!</div><br>"; myMsg += GetParameters() + "<br><input class='mt-2 btn btn-primary' type=submit value='submit' />"; MakeWebPage(client, myMsg); } } } // close the connection: client.stop(); } } void setTheValuesFromUserForm(WiFiClient client) { String currentLine = ""; UPONLYMODE = false; while (client.connected()) { // loop while the client's connected if (client.available()) { // if there's bytes to read from the client, char c = client.read(); // read a byte, then if (c == '\n') { // if the current line is blank, you got two newline characters in a row. // that's the end of the client HTTP request, so send a response: if (currentLine.length() == 0) { break; } else { // if you got a newline, then clear currentLine: currentLine = ""; } } else if (c != '\r') { // if you got anything else but a carriage return character, currentLine += c; // add it to the end of the currentLine } if (currentLine.endsWith("maxMotor=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } maxMotor = myValue.toFloat(); } if (currentLine.endsWith("stick_dampener=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } stick_dampener = myValue.toFloat(); } if (currentLine.endsWith("i_limit=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } i_limit = myValue.toFloat(); } if (currentLine.endsWith("Accel_filter=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } Accel_filter = myValue.toFloat(); } if (currentLine.endsWith("Gyro_filter=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } Gyro_filter = myValue.toFloat(); } if (currentLine.endsWith("UPONLYMODE=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } if (myValue == "true") UPONLYMODE = true; else UPONLYMODE = false; } if (currentLine.endsWith("kp_roll_angle=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } Kp_roll_angle = myValue.toFloat(); } if (currentLine.endsWith("ki_roll_angle=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } Ki_roll_angle = myValue.toFloat(); } if (currentLine.endsWith("kd_roll_angle=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } Kd_roll_angle = myValue.toFloat(); } // if (currentLine.endsWith("kp_pitch_angle=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } Kp_pitch_angle = myValue.toFloat(); } if (currentLine.endsWith("ki_pitch_angle=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } Ki_pitch_angle = myValue.toFloat(); } if (currentLine.endsWith("kd_pitch_angle=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } Kd_pitch_angle = myValue.toFloat(); } //yaw if (currentLine.endsWith("kp_yaw=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } Kp_yaw = myValue.toFloat(); } if (currentLine.endsWith("ki_yaw=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } Ki_yaw = myValue.toFloat(); } if (currentLine.endsWith("kd_yaw=")) { String myValue = ""; while (!currentLine.endsWith("&")) { c = client.read(); if (c != '&') myValue += c; currentLine += c; } Kd_yaw = myValue.toFloat(); return; } } } } String GetParameters() { String myString; myString = myString + "<table><tr><td>Max Motor Speed (0.0-1.0):</td><td><input type=number step=.001 name=maxMotor style='width:70px;' value='" + String(maxMotor) + "'></td></tr>"; myString = myString + "<tr><td>Stick Dampening (0.01-1.0):<br>0.1=slow/steady, 1.0=noisy/fast</td><td><input type=number step=.001 name=stick_dampener style='width:70px;' value='" + String(stick_dampener) + "'></td></tr>"; myString = myString + "<tr><td>Integral Accumulation (25 default):</td><td><input type=number step=.001 name=i_limit style='width:70px;' value='" + String(i_limit) + "'></td></tr>"; myString = myString + "<tr><td>Accel Dampening (0.1-1.0):<br>0.1=slow/steady, 1.0=noisy/fast</td><td><input type=number step=.001 name=Accel_filter style='width:70px;' value='" + String(Accel_filter) + "'></td></tr>"; myString = myString + "<tr><td>Gyro Dampening (0.1-1.0 ):<br>0.1=slow/steady, 1.0=noisy/fast</td><td><input type=number step=.001 name=Gyro_filter style='width:70px;' value='" + String(Gyro_filter) + "'></td></tr>"; String myVal; if (!UPONLYMODE) myVal = ""; else myVal = "checked"; myString = myString + "<tr><td>Up Mode Only:</td><td><input type=CHECKBOX name=UPONLYMODE value='true' " + (myVal) + "></td></tr>"; myString = myString + "</table>"; myString = myString + "<table class=table><thead class=thead-dark><th></th><th>Kp</th><th>Ki</th><th>Kd</th></thead>"; myString = myString + "<tr><td>Roll:</td><td><input name=kp_roll_angle style='width:70px;' type=number step=.001 value='" + String(Kp_roll_angle) + "'></td><td><input style='width:70px;' name=ki_roll_angle type=number step=.001 value='" + String(Ki_roll_angle) + "'></td><td><input name=kd_roll_angle type=number step=.001 style='width:70px;' value='" + String(Kd_roll_angle) + "'></td></tr>"; myString = myString + "<tr><td>Pitch:</td><td><input name=kp_pitch_angle style='width:70px;' type=number step=.001 value='" + String(Kp_pitch_angle) + "'></td><td><input style='width:70px;' name=ki_pitch_angle type=number step=.001 value='" + String(Ki_pitch_angle) + "'></td><td><input name=kd_pitch_angle type=number step=.001 style='width:70px;' value='" + String(Kd_pitch_angle) + "'></td></tr>"; myString = myString + "<tr><td>Yaw:</td><td><input name=kp_yaw type=number step=.001 style='width:70px;' value='" + String(Kp_yaw) + "'></td><td><input style='width:70px;' type=number step=.001 name=ki_yaw value='" + String(Ki_yaw) + "'></td><td><input type=number step=.001 name=kd_yaw style='width:70px;' value='" + String(Kd_yaw) + "'></td></tr><input type=hidden name=ender value='0'>"; myString = myString + "</table><br>Tip: To use your parameters beyond this flight session, snapshot the screen for reference and update the code.<br>"; return myString; } void MakeWebPage(WiFiClient client, String html) { // HTTP headers always start with a response code (e.g. HTTP/1.1 200 OK) // and a content-type so the client knows what's coming, then a blank line: client.println("HTTP/1.1 200 OK"); client.println("Content-type:text/html"); client.println(); // the content of the HTTP response follows the header: client.print("<head>"); client.print("<meta name=\"viewport\" content=\"width=device-width, initial-scale=1\">"); client.print("<link href=\"https://cdn.jsdelivr.net/npm/bootstrap@5.1.3/dist/css/bootstrap.min.css\" rel=\"stylesheet\" integrity=\"sha384-1BmE4kWBq78iYhFldvKuhfTAU6auU8tT94WrHftjDbrCEXSU1oBoqyl2QvZ6jIW3\" crossorigin=\"anonymous\">"); client.print("</head>"); client.print("<style>"); client.print("</style>"); client.print("<form method=get><div class='container'>"); client.print(html); client.print(tuningProcedure()); client.print("</div></form>"); client.print("<script src=\"https://cdn.jsdelivr.net/npm/bootstrap@5.1.3/dist/js/bootstrap.bundle.min.js\" integrity=\"sha384-ka7Sk0Gln4gmtz2MlQnikT1wXgYsOg+OMhuP+IlRH9sENBO0LRn5q+8nbTov4+1p\" crossorigin=\"anonymous\"></script>"); client.println(); // The HTTP response ends with another blank line: } void tick() { //Keep track of what time it is and how much time has elapsed since the last loop prev_time = current_time; current_time = micros(); deltaTime = (current_time - prev_time) / 1000000.0; //division takes it from micros to seconds. 1000000 ms=1 second } //========================================================================================================================// // FUNCTIONS // //========================================================================================================================// void controlMixer() { //DESCRIPTION: Mixes scaled commands from PID controller to actuator outputs based on vehicle configuration /* * Takes roll_PID, pitch_PID, and yaw_PID computed from the PID controller and appropriately mixes them for the desired * vehicle configuration. For example on a quadcopter, the left two motors should have +roll_PID while the right two motors * should have -roll_PID. Front two should have -pitch_PID and the back two should have +pitch_PID etc... every motor has * normalized (0 to 1) thro_des command for throttle control. * *Relevant variables: *thro_des - direct thottle control *roll_PID, pitch_PID, yaw_PID - stabilized axis variables */ //Quad mixing. maxMotor is used to keep the motors from being too violent if you have a big battery and concers about that. m1_command_scaled = maxMotor * (thro_des) - pitch_PID + roll_PID + yaw_PID; //Front left m2_command_scaled = maxMotor * (thro_des) - pitch_PID - roll_PID - yaw_PID; //Front right m3_command_scaled = maxMotor * (thro_des) + pitch_PID - roll_PID + yaw_PID; //Back Right m4_command_scaled = maxMotor * (thro_des) + pitch_PID + roll_PID - yaw_PID; //Back Left m1_command_scaled = constrain(m1_command_scaled, 0, 1.0); m2_command_scaled = constrain(m2_command_scaled, 0, 1.0); m3_command_scaled = constrain(m3_command_scaled, 0, 1.0); m4_command_scaled = constrain(m4_command_scaled, 0, 1.0); } void IMUinit() { //DESCRIPTION: Initialize IMU if (!IMU.begin()) { while (true) ; } delay(15); } void getIMUdata() { //DESCRIPTION: Request full dataset from IMU //Accelerometer if (IMU.accelerationAvailable()) { IMU.readAcceleration(AccX, AccY, AccZ); AccX = AccX - AccErrorX; AccY = AccY - AccErrorY; AccY = AccY - AccErrorZ; //Correct the outputs with the calculated error values /* //This was used to suppress noise. However, it just slows things down. There is already internal filters to the IMU. AccX = (1.0 - Accel_filter) * AccX_prev + Accel_filter * AccX; AccY = (1.0 - Accel_filter) * AccY_prev + Accel_filter * AccY; AccZ = (1.0 - Accel_filter) * AccZ_prev + Accel_filter * AccZ; AccX_prev = AccX; AccY_prev = AccY; AccZ_prev = AccZ; */ } else return; if (IMU.gyroscopeAvailable()) { //Gyro IMU.readGyroscope(GyroX, GyroY, GyroZ); //Correct the outputs with the calculated error values GyroX = GyroX - GyroErrorX; GyroY = GyroY - GyroErrorY; GyroZ = GyroZ - GyroErrorZ; /* //This was used to suppress noise. However, it just slows things down. There is already internal filters to the IMU. GyroX = (1.0 - Gyro_filter) * GyroX_prev + Gyro_filter * GyroX; GyroY = (1.0 - Gyro_filter) * GyroY_prev + Gyro_filter * GyroY; GyroZ = (1.0 - Gyro_filter) * GyroZ_prev + Gyro_filter * GyroZ; GyroX_prev = GyroX; GyroY_prev = GyroY; GyroZ_prev = GyroZ; */ } } void calculate_IMU_error() { //DESCRIPTION: Computes IMU accelerometer and gyro error on startup. Note: vehicle should be powered up on flat surface /* * The error values it computes are applied to the raw gyro and * accelerometer values AccX, AccY, AccZ, GyroX, GyroY, GyroZ in getIMUdata(). * this will correct for sensor drift and crookedness in your copter. */ float AcX, AcY, AcZ, GyX, GyY, GyZ; Serial.println("Calculating IMU Error with 10000 iterations. Please stand by..."); //Read IMU values 1000 times. Should take around 2 minutes at 104Hz IMU timing. int c = 0;AccErrorX=0;AccErrorY=0;AccErrorZ=0;GyroErrorX=0;GyroErrorY=0;GyroErrorZ=0; while (c < 10000) { while (!IMU.accelerationAvailable()&!IMU.gyroscopeAvailable()) delay(1); IMU.readAcceleration(AcX, AcY, AcZ); IMU.readGyroscope(GyX, GyY, GyZ); AccX = AcX; AccY = AcY; AccZ = AcZ; GyroX = GyX; GyroY = GyY; GyroZ = GyZ; //Sum all readings AccErrorX = AccErrorX + AccX; AccErrorY = AccErrorY + AccY; AccErrorZ = AccErrorZ + AccZ; GyroErrorX = GyroErrorX + GyroX; GyroErrorY = GyroErrorY + GyroY; GyroErrorZ = GyroErrorZ + GyroZ; c++; Serial.println(String(10000-c)); } //Divide the sum by 12000 to get the error value AccErrorX = AccErrorX / c; AccErrorY = AccErrorY / c; AccErrorZ = AccErrorZ / c - 1.0; GyroErrorX = GyroErrorX / c; GyroErrorY = GyroErrorY / c; GyroErrorZ = GyroErrorZ / c; Serial.print("float AccErrorX = "); Serial.print(AccErrorX); Serial.println(";"); Serial.print("float AccErrorY = "); Serial.print(AccErrorY); Serial.println(";"); Serial.print("float AccErrorZ = "); Serial.print(AccErrorZ); Serial.println(";"); Serial.print("float GyroErrorX = "); Serial.print(GyroErrorX); Serial.println(";"); Serial.print("float GyroErrorY = "); Serial.print(GyroErrorY); Serial.println(";"); Serial.print("float GyroErrorZ = "); Serial.print(GyroErrorZ); Serial.println(";"); Serial.println("Paste these values in user specified variables section and comment out calculate_IMU_error() in void setup."); while (true) delay(1000); } void Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az) { //DESCRIPTION: Attitude estimation through sensor fusion - 6DOF /* * See description of Madgwick() for more information. This is a 6DOF implimentation for when magnetometer data is not * available (for example when using the recommended MPU6050 IMU for the default setup). */ float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3; //Convert gyroscope degrees/sec to radians/sec gx *= 0.0174533f; gy *= 0.0174533f; gz *= 0.0174533f; //Rate of change of quaternion from gyroscope qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); //Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { //Normalise accelerometer measurement recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; //Auxiliary variables to avoid repeated arithmetic _2q0 = 2.0f * q0; _2q1 = 2.0f * q1; _2q2 = 2.0f * q2; _2q3 = 2.0f * q3; _4q0 = 4.0f * q0; _4q1 = 4.0f * q1; _4q2 = 4.0f * q2; _8q1 = 8.0f * q1; _8q2 = 8.0f * q2; q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; //Gradient decent algorithm corrective step s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); //normalise step magnitude s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; s3 *= recipNorm; //Apply feedback step qDot1 -= B_madgwick * s0; qDot2 -= B_madgwick * s1; qDot3 -= B_madgwick * s2; qDot4 -= B_madgwick * s3; } //Integrate rate of change of quaternion to yield quaternion q0 += qDot1 * deltaTime; q1 += qDot2 * deltaTime; q2 += qDot3 * deltaTime; q3 += qDot4 * deltaTime; //Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; //Compute angles roll_IMU = atan2(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2) * 57.29577951; //degrees pitch_IMU = -asin(-2.0f * (q1 * q3 - q0 * q2)) * 57.29577951; //degrees yaw_IMU = -atan2(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3) * 57.29577951; //degrees } void getDesiredAnglesAndThrottle() { //DESCRIPTION: Normalizes desired control values to appropriate values /* * Updates the desired state variables thro_des, roll_des, pitch_des, and yaw_des. These are computed by using the raw * RC pwm commands and scaling them to be within our limits defined in setup. thro_des stays within 0 to 1 range. * roll_des and pitch_des are scaled to be within max roll/pitch amount in degrees * yaw_des is scaled to be within max yaw in degrees/sec. */ thro_des = (PWM_throttle - 1000.0) / 1000.0; //Between 0 and 1 roll_des = (PWM_roll - 1500.0) / 500.0; //Between -1 and 1 pitch_des = (PWM_Elevation - 1500.0) / 500.0; //Between -1 and 1 yaw_des = (PWM_Rudd - 1500.0) / 500.0; //Between -1 and 1 //Constrain within normalized bounds thro_des = constrain(thro_des, 0.0, 1.0); //Between 0 and 1 roll_des = constrain(roll_des, -1.0, 1.0) * maxRoll; //Between -maxRoll and +maxRoll pitch_des = constrain(pitch_des, -1.0, 1.0) * maxPitch; //Between -maxPitch and +maxPitch yaw_des = constrain(yaw_des, -1.0, 1.0) * maxYaw; //Between -maxYaw and +maxYaw } void PIDControlCalcs() { //DESCRIPTION: Computes control commands based on state error (angle) /* * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des computed in * getDesiredAnglesAndThrottle(). Error is simply the desired state minus the actual state (ex. roll_des - roll_IMU). Two safety features * are implimented here regarding the I terms. The I terms are saturated within specified limits on startup to prevent * excessive buildup. This can be seen by holding the vehicle at an angle and seeing the motors ramp up on one side until * they've maxed out throttle...saturating I to a specified limit fixes this. The second feature defaults the I terms to 0 * if the throttle is at the minimum setting. This means the motors will not start spooling up on the ground, and the I * terms will always start from 0 on takeoff. This function updates the variables roll_PID, pitch_PID, and yaw_PID which * can be thought of as 1-D stablized signals. They are mixed to the configuration of the vehicle in controlMixer(). */ if (PWM_throttle < 1030) { //This will keep the motors from spinning with the throttle at zero should the drone be sitting unlevel. integral_roll_prev = 0; integral_pitch_prev = 0; error_yaw_prev = 0; integral_yaw_prev = 0; roll_PID=0; pitch_PID=0; yaw_PID=0; return; } if (UPONLYMODE) { //For troubleshooting. UPONLYMODE can be set with the web interface. roll_des = 0; pitch_des = 0; yaw_PID = 0; } //Roll error_roll = roll_des - roll_IMU; integral_roll = integral_roll_prev + error_roll * deltaTime; integral_roll = constrain(integral_roll, -i_limit, i_limit); //Limit integrator to prevent saturating derivative_roll = GyroX; //(roll_des-roll_IMU-roll_des-previous_IMU)/dt=current angular velocity since last IMU read and therefore GyroX in deg/s roll_PID = 0.0001 * (Kp_roll_angle * error_roll + Ki_roll_angle * integral_roll - Kd_roll_angle * derivative_roll); //Scaled by .0001 to bring within -1 to 1 range //Pitch error_pitch = pitch_des - pitch_IMU; integral_pitch = integral_pitch_prev + error_pitch * deltaTime; integral_pitch = constrain(integral_pitch, -i_limit, i_limit); derivative_pitch = GyroY; pitch_PID = .0001 * (Kp_pitch_angle * error_pitch + Ki_pitch_angle * integral_pitch - Kd_pitch_angle * derivative_pitch); //Scaled by .0001 to bring within -1 to 1 range //Yaw, stablize on rate from GyroZ versus angle. In other words, your stick is setting y axis rotation speed - not the angle to get to. error_yaw = yaw_des - GyroZ; integral_yaw = integral_yaw_prev + error_yaw * deltaTime; integral_yaw = constrain(integral_yaw, -i_limit, i_limit); derivative_yaw = (error_yaw - error_yaw_prev) / deltaTime; yaw_PID = .0001 * (Kp_yaw * error_yaw + Ki_yaw * integral_yaw + Kd_yaw * derivative_yaw); //Scaled by .0001 to bring within -1 to 1 range //Update roll variables integral_roll_prev = integral_roll; integral_pitch_prev = integral_pitch; error_yaw_prev = error_yaw; integral_yaw_prev = integral_yaw; } void scaleCommands() { //DESCRIPTION: Scale normalized actuator commands to values for ESC protocol /* * The actual pulse width is set at the servo attach. */ //Scale to Servo PWM 0-180 degrees for stop to full speed. No need to constrain since mx_command_scaled already is. m1_command_PWM = m1_command_scaled * 180; m2_command_PWM = m2_command_scaled * 180; m3_command_PWM = m3_command_scaled * 180; m4_command_PWM = m4_command_scaled * 180; } void getRadioSticks() { //DESCRIPTION: Get raw PWM values for every channel from the radio /* * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of * the loop. * The raw radio commands are filtered with a first order low-pass filter to eliminate any really high frequency noise. */ PWM_throttle = getRadioPWM(1); PWM_roll = getRadioPWM(2); PWM_Elevation = getRadioPWM(3); PWM_Rudd = getRadioPWM(4); PWM_ThrottleCutSwitch = getRadioPWM(5); //Low-pass the critical commands and update previous values if (PWM_throttle - PWM_throttle_prev < 0) { //Going down - slow PWM_throttle = (.95) * PWM_throttle_prev + 0.05 * PWM_throttle; } else { //Going up - fast PWM_throttle = (stick_dampener)*PWM_throttle_prev + (1 - stick_dampener) * PWM_throttle; } PWM_roll = (1.0 - stick_dampener) * PWM_roll_prev + stick_dampener * PWM_roll; PWM_Elevation = (1.0 - stick_dampener) * PWM_Elevation_prev + stick_dampener * PWM_Elevation; PWM_Rudd = (1.0 - stick_dampener) * PWM_Rudd_prev + stick_dampener * PWM_Rudd; PWM_throttle_prev = PWM_throttle; PWM_roll_prev = PWM_roll; PWM_Elevation_prev = PWM_Elevation; PWM_Rudd_prev = PWM_Rudd; } void setToFailsafe() { PWM_throttle = PWM_throttle_fs; PWM_roll = PWM_roll_fs; PWM_Elevation = PWM_elevation_fs; PWM_Rudd = PWM_rudd_fs; PWM_ThrottleCutSwitch = PWM_ThrottleCutSwitch_fs; //this is so the throttle cut routine doesn't override the fail safes. } void failSafe() { //DESCRIPTION: If radio gives garbage values, set all commands to default values /* * Radio connection failsafe used to check if the getRadioSticks() function is returning acceptable pwm values. If any of * the commands are lower than 800 or higher than 2200, then we can be certain that there is an issue with the radio * connection (most likely hardware related). If any of the channels show this failure, then all of the radio commands * channel_x_pwm are set to default failsafe values specified in the setup. Comment out this function when troubleshooting * your radio connection in case any extreme values are triggering this function to overwrite the printed variables. */ //Triggers for failure criteria //unsigned minVal = 800; //unsigned maxVal = 2200; //if (PWM_ThrottleCutSwitch>maxVal || PWM_ThrottleCutSwitch<minVal || PWM_throttle > maxVal || PWM_throttle < minVal || PWM_roll > maxVal ||PWM_roll < minVal || PWM_Elevation > maxVal || PWM_Elevation < minVal || PWM_Rudd > maxVal || PWM_Rudd < minVal) if (PWM_throttle < 800 || PWM_throttle > 2200) //this is the less conservative version to get through this routine faster. { failsafed = true; setToFailsafe(); } else failsafed = false; #if EASYCHAIR PWM_throttle = 1500; //For testing in the easy chair with the Arduino out of the drone. See the compiler directive at the top of the code. #endif } void commandMotors() { //DESCRIPTION: Send pulses to motor pins m1PWM.write(m1_command_PWM); m2PWM.write(m2_command_PWM); m3PWM.write(m3_command_PWM); m4PWM.write(m4_command_PWM); } void calibrateESCs() { //DESCRIPTION: Used in void setup() to allow standard ESC calibration procedure with the radio to take place. /* * Simulates the void loop(), but only for the purpose of providing throttle pass through to the motors, so that you can * power up with throttle at full, let ESCs begin arming sequence, and lower throttle to zero. */ m1PWM.write(180); m2PWM.write(180); m3PWM.write(180); m4PWM.write(180); delay(5000); m1PWM.write(0); m2PWM.write(0); m3PWM.write(0); m4PWM.write(0); delay(5000); } void throttleCut() { //DESCRIPTION: Directly set actuator outputs to minimum value if triggered /* * Monitors the state of radio command PWM_throttle and directly sets the mx_command_PWM values to minimum (1060 is * minimum , 0 is minimum for standard PWM servo library used) if channel 5 is high. This is the last function * called before commandMotors() is called so that the last thing checked is if the user is giving permission to command * the motors to anything other than minimum value. Safety first. */ #if EASYCHAIR //This is set above in compiler directives for when you are testing the Arduino outside of the drone. throttle_is_cut = false; return; #endif if (throttle_is_cut) killMotors(); //make sure we keep those motors at 0 if the throttle was cut. if (PWM_ThrottleCutSwitch < 1300) { //throttleCutCounter will ensure it is not just a blip that has caused a false cut. if (++throttleCutCounter > 10) killMotors(); return; } if (throttle_is_cut && PWM_ThrottleCutSwitch > 1500) { //reset only if throttle is down to prevent a jolting suprise if (PWM_throttle < 1040 && ++throttleNotCutCounter > 10) { throttle_is_cut = false; throttleNotCutCounter = 0; throttleCutCounter = 0; } else killMotors(); return; } //Handle when things are going upside down if (roll_IMU > 55 || roll_IMU < -55 || pitch_IMU > 55 || pitch_IMU < -55) { if (++throttleCutCounter > 4) killMotors(); return; } throttleNotCutCounter = 0; throttleCutCounter = 0; } void killMotors() { //sets the PWM to its lowest value to shut off a motor such as whne the throttle cut switch is fliped. throttle_is_cut = true; throttleCutCounter=10; //to prevent overflowing float m1_command_PWM = 0; m2_command_PWM = 0; m3_command_PWM = 0; m4_command_PWM = 0; } void tock() { //DESCRIPTION: Regulate main loop rate to specified frequency in Hz /* * It's good to operate at a constant loop rate for filters to remain stable and whatnot. Interrupt routines running in the * background cause the loop rate to fluctuate. This function basically just waits at the end of every loop iteration until * the correct time has passed since the start of the current loop for the desired loop rate in Hz. I have matched * it just below the Gyro update frequency. invFreq is set at the top of the code. */ //Sit in loop until appropriate time has passed while checking for any WiFi client activity. while (invFreq > (micros() - current_time)) { if (ALLOW_WIFI&&throttle_is_cut) loopWiFi(); }; } void printRadioData() { if (current_time - print_counter > 10000) { print_counter = micros(); Serial.print(F(" CH1: ")); Serial.print(getRadioPWM(1)); Serial.print(F(" CH2: ")); Serial.print(getRadioPWM(2)); Serial.print(F(" CH3: ")); Serial.print(getRadioPWM(3)); Serial.print(F(" CH4: ")); Serial.println(getRadioPWM(4)); Serial.print(F(" CH5: ")); Serial.print(PWM_ThrottleCutSwitch); } } void printDesiredState() { if (current_time - print_counter > 10000) { print_counter = micros(); Serial.print(F("thro_des: ")); Serial.print(thro_des); Serial.print(F(" roll_des: ")); Serial.print(roll_des); Serial.print(F(" pitch_des: ")); Serial.print(pitch_des); Serial.print(F(" yaw_des: ")); Serial.println(yaw_des); } } void printGyroData() { if (current_time - print_counter > 10000) { print_counter = micros(); Serial.print(F("GyroX: ")); Serial.print(GyroX); Serial.print(F(" GyroY: ")); Serial.print(GyroY); Serial.print(F(" GyroZ: ")); Serial.println(GyroZ); } } void printAccelData() { if (current_time - print_counter > 10000) { print_counter = micros(); Serial.print(F("AccX: ")); Serial.print(AccX); Serial.print(F(" AccY: ")); Serial.print(AccY); Serial.print(F(" AccZ: ")); Serial.println(AccZ); } } void printJSON() { if (current_time - print_counter > 34000) { //Don't go too fast or it slows down the main loop print_counter = micros(); Serial.print(F("{\"roll\": ")); Serial.print(roll_IMU); Serial.print(F(", \"pitch\": ")); Serial.print(pitch_IMU); Serial.print(F(", \"yaw\": ")); Serial.print(yaw_IMU); Serial.print(F(", \"ErrorRoll\": ")); Serial.print(error_roll); Serial.print(F(", \"IntegralRoll\": ")); Serial.print(integral_roll); Serial.print(F(", \"DerivativeRoll\": ")); Serial.print(derivative_roll); Serial.print(F(", \"RollKp\": ")); Serial.print(Kp_roll_angle); Serial.print(F(", \"RollKi\": ")); Serial.print(Ki_roll_angle); Serial.print(F(", \"RollKd\": ")); Serial.print(Kd_roll_angle); Serial.print(F(", \"m1\": ")); Serial.print(m1_command_PWM); Serial.print(F(", \"m2\": ")); Serial.print(m2_command_PWM); Serial.print(F(", \"m3\": ")); Serial.print(m3_command_PWM); Serial.print(F(", \"m4\": ")); Serial.print(m4_command_PWM); Serial.print(F(", \"AccX\": ")); Serial.print(AccX); Serial.print(F(", \"AccY\": ")); Serial.print(AccY); Serial.print(F(", \"AccZ\": ")); Serial.print(AccZ); Serial.print(F(", \"GyroX\": ")); Serial.print(GyroX); Serial.print(F(", \"GyroY\": ")); Serial.print(GyroY); Serial.print(F(", \"GyroZ\": ")); Serial.print(GyroZ); Serial.print(F(", \"RollIMU\": ")); Serial.print(roll_IMU); Serial.print(F(", \"PitchIMU\": ")); Serial.print(pitch_IMU); Serial.print(F(", \"YawIMU\": ")); Serial.print(yaw_IMU); Serial.print(F(", \"ThroDes\": ")); Serial.print(thro_des); Serial.print(F(", \"RollDes\": ")); Serial.print(roll_des); Serial.print(F(", \"PitchDes\": ")); Serial.print(pitch_des); Serial.print(F(", \"YawDes\": ")); Serial.print(yaw_des); Serial.print(F(", \"Pitch_PID\": ")); Serial.print(pitch_PID); Serial.print(F(", \"Roll_PID\": ")); Serial.print(roll_PID); Serial.print(F(", \"Yaw_PID\": ")); Serial.print(yaw_PID); Serial.print(F(", \"PWM_throttle\": ")); Serial.print(PWM_throttle); Serial.print(F(", \"PWM_roll\": ")); Serial.print(PWM_roll); Serial.print(F(", \"PWM_Elevation\": ")); Serial.print(PWM_Elevation); Serial.print(F(", \"PWM_Rudd\": ")); Serial.print(PWM_Rudd); Serial.print(F(", \"PWM_ThrottleCutSwitch\": ")); Serial.print(PWM_ThrottleCutSwitch); Serial.print(F(", \"Throttle_is_Cut\": ")); Serial.print(throttle_is_cut); Serial.print(F(", \"Failsafe\": ")); Serial.print(failsafed); Serial.print(F(", \"DeltaTime\": ")); Serial.print(deltaTime * 1000000.0); Serial.println("}"); } } void printRollPitchYaw() { if (current_time - print_counter > 10000) { print_counter = micros(); Serial.print(F("roll: ")); Serial.print(roll_IMU); Serial.print(F(" pitch: ")); Serial.print(pitch_IMU); Serial.print(F(" yaw: ")); Serial.println(yaw_IMU); } } void printPIDoutput() { if (current_time - print_counter > 10000) { print_counter = micros(); Serial.print(F("roll_PID: ")); Serial.print(roll_PID); Serial.print(F(" pitch_PID: ")); Serial.print(pitch_PID); Serial.print(F(" yaw_PID: ")); Serial.println(yaw_PID); } } void printMotorCommands() { if (current_time - print_counter > 10000) { print_counter = micros(); Serial.print(F("m1_command: ")); Serial.print(m1_command_PWM); Serial.print(F(" m2_command: ")); Serial.print(m2_command_PWM); Serial.print(F(" m3_command: ")); Serial.print(m3_command_PWM); Serial.print(F(" m4_command: ")); Serial.println(m4_command_PWM); } } void printtock() { if (current_time - print_counter > 10000) { print_counter = micros(); Serial.print(F("deltaTime = ")); Serial.println(deltaTime * 1000000.0); } } //========================================================================================================================// //This file contains all necessary functions and code used for radio communication to avoid cluttering the main code void radioSetup() { //PWM Receiver //Declare interrupt pins pinMode(throttlePin, INPUT_PULLUP); pinMode(rollPin, INPUT_PULLUP); pinMode(upDownPin, INPUT_PULLUP); pinMode(ruddPin, INPUT_PULLUP); pinMode(throttleCutSwitchPin, INPUT_PULLUP); delay(20); //Attach interrupt and point to corresponding ISR functions attachInterrupt(digitalPinToInterrupt(throttlePin), getCh1, CHANGE); attachInterrupt(digitalPinToInterrupt(rollPin), getCh2, CHANGE); attachInterrupt(digitalPinToInterrupt(upDownPin), getCh3, CHANGE); attachInterrupt(digitalPinToInterrupt(ruddPin), getCh4, CHANGE); attachInterrupt(digitalPinToInterrupt(throttleCutSwitchPin), getCh5, CHANGE); delay(20); } unsigned long getRadioPWM(int ch_num) { //DESCRIPTION: Get current radio commands from interrupt routines unsigned long returnPWM = 0; if (ch_num == 1) { returnPWM = channel_1_raw; } else if (ch_num == 2) { returnPWM = channel_2_raw; } else if (ch_num == 3) { returnPWM = channel_3_raw; } else if (ch_num == 4) { returnPWM = channel_4_raw; } else if (ch_num == 5) { returnPWM = channel_5_raw; } return returnPWM; } //========================================================================================================================// //INTERRUPT SERVICE ROUTINES (for reading PWM and PPM) void getCh1() { int trigger = digitalRead(throttlePin); if (trigger == 1) { rising_edge_start_1 = micros(); } else if (trigger == 0) { channel_1_raw = micros() - rising_edge_start_1; } } void getCh2() { int trigger = digitalRead(rollPin); if (trigger == 1) { rising_edge_start_2 = micros(); } else if (trigger == 0) { channel_2_raw = micros() - rising_edge_start_2; } } void getCh3() { int trigger = digitalRead(upDownPin); if (trigger == 1) { rising_edge_start_3 = micros(); } else if (trigger == 0) { channel_3_raw = micros() - rising_edge_start_3; } } void getCh4() { int trigger = digitalRead(ruddPin); if (trigger == 1) { rising_edge_start_4 = micros(); } else if (trigger == 0) { channel_4_raw = micros() - rising_edge_start_4; } } void getCh5() { int trigger = digitalRead(throttleCutSwitchPin); if (trigger == 1) { rising_edge_start_5 = micros(); } else if (trigger == 0) { channel_5_raw = micros() - rising_edge_start_5; } } String tuningProcedure() { return "<hr><h3><b>Ziegler-Nichols</b></h3> <p>The Ziegler-Nichols tuning method is one of the most famous ways to experimentally tune a PID controller. The basic algorithm is as follows:</p><ol><li>Turn off the Integral and Derivative components for the controller; only use Proportional control.</li><li>Slowly increase the gain (i.e. <em>K<sub>p</sub></em>, the Proportion constant) until the process starts to oscillate<br />This final gain value is known as the ultimate gain, or <em>K<sub>u</sub></em><br />The period of oscillation is the ultimate period, or <em>T<sub>u</sub></em></li><li>Use the following table to derive the PID variables</li></ol></div></div><div class=\"sqs-block code-block sqs-block-code\" data-block-type=\"23\" id=\"block-yui_3_17_2_1_1598301478634_207710\"><div class=\"sqs-block-content\"><style type=\"text/css\">.tg {border-collapse:collapse;border-spacing:0;}.tg td{border-color:black;border-style:solid;border-width:1px; overflow:hidden;padding:10px 5px;word-break:normal;}.tg th{border-color:black;border-style:solid;border-width:1px; font-weight:normal;overflow:hidden;padding:10px 5px;word-break:normal;}.tg .tg-gr1d{background-color:#e9f4fc;border-color:#337494;font-weight:bold;text-align:left;vertical-align:top}.tg .tg-m4ds{border-color:#337494;text-align:center;vertical-align:top}.tg .tg-bxxa{border-color:#337494;text-align:left;vertical-align:top}.tg .tg-hk19{background-color:#e9f4fc;border-color:#337494;font-weight:bold;text-align:center;vertical-align:top}.tg .tg-vaq8{background-color:#e9f4fc;border-color:#337494;text-align:left;vertical-align:top}.tg .tg-e1cu{background-color:#e9f4fc;border-color:#337494;text-align:center;vertical-align:top}</style><table class=\"table\"><thead> <tr> <th class=\"tg-gr1d\">Controller</th> <th class=\"tg-hk19\">K<sub>p</sub></th> <th class=\"tg-hk19\">T<sub>i</sub></th> <th class=\"tg-hk19\">T<sub>d</sub></th> </tr></thead><tbody> <tr> <td class=\"tg-bxxa\">P</td> <td class=\"tg-m4ds\">K<sub>u</sub>/2</td> <td class=\"tg-m4ds\"></td> <td class=\"tg-m4ds\"></td> </tr> <tr> <td class=\"tg-vaq8\">PI</td> <td class=\"tg-e1cu\">K<sub>u</sub>/2.5</td> <td class=\"tg-e1cu\">T<sub>u</sub>/1.25</td> <td class=\"tg-e1cu\"></td> </tr> <tr> <td class=\"tg-bxxa\">PID</td> <td class=\"tg-m4ds\">0.6K<sub>u</sub></td> <td class=\"tg-m4ds\">T<sub>u</sub>/2</td> <td class=\"tg-m4ds\">T<sub>u</sub>/8</td> </tr> <tr> <td class=\"tg-vaq8\">Pessen Integral Rule<br></td> <td class=\"tg-e1cu\">0.7K<sub>u</sub></td> <td class=\"tg-e1cu\">0.4T<sub>u</sub></td> <td class=\"tg-e1cu\">0.15T<sub>u</sub></td> </tr> <tr> <td class=\"tg-bxxa\">Moderate overshoot</td> <td class=\"tg-m4ds\">K<sub>u</sub>/3</td> <td class=\"tg-m4ds\">T<sub>u</sub>/2</td> <td class=\"tg-m4ds\">T<sub>u</sub>/3</td> </tr> <tr> <td class=\"tg-vaq8\">No overshoot</td> <td class=\"tg-e1cu\">K<sub>u</sub>/5</td> <td class=\"tg-e1cu\">T<sub>u</sub>/2</td> <tdclass=\"tg-e1cu\">T<sub>u</sub>/3</td> </tr></tbody></table>"; } //HELPER FUNCTIONS float invSqrt(float x) { //Fast inverse sqrt for madgwick filter /* float halfx = 0.5f * x; float y = x; long i = *(long*)&y; i = 0x5f3759df - (i>>1); y = *(float*)&i; y = y * (1.5f - (halfx * y * y)); y = y * (1.5f - (halfx * y * y)); return y; */ /* //alternate form: unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1); float tmp = *(float*)&i; float y = tmp * (1.69000231f - 0.714158168f * x * tmp * tmp); return y; */ return 1.0 / sqrtf(x); //Teensy is fast enough to just take the compute penalty lol suck it arduino nano }
Schematic and Layout
Schematic
schematic.png
Oct 06,2022
4,939 views
end-flag
World's First Published Open-source Arduino Nano RP2040 Connect Drone Flight Controller
A printed circuit board that houses an Arduino Nano RP2040 Connect to allow reliable connections to quadcopter (aka drone) ESCs.
4939
8
2
6.75 (1)
Published: Oct 06,2022
Download Gerber file 16
PCBWay Donate 10% cost To Author
*PCBWay community is a sharing platform. We are not responsible for any design issues and parameter issues (board thickness, surface finish, etc.) you choose.
Under the
Attribution-ShareAlike (CC BY-SA)
License.
Raspberry Pi 5 7 Inch Touch Screen IPS 1024x600 HD LCD HDMI-compatible Display for RPI 4B 3B+ OPI 5 AIDA64 PC Secondary Screen（Without Speaker）
BUY NOW
ESP32-S3 4.3inch Capacitive Touch Display Development Board, 800×480, 5-point Touch, 32-bit LX7 Dual-core Processor
BUY NOW
Raspberry Pi 5 7 Inch Touch Screen IPS 1024x600 HD LCD HDMI-compatible Display for RPI 4B 3B+ OPI 5 AIDA64 PC Secondary Screen（Without Speaker）
BUY NOW
- Comments(2)
- Likes(8)
Log in to post comments.
Upload photo
0 / 10000
Reply- Reply
VOTING
1 votes
- 1 USER VOTES
6.75
- YOUR VOTE 0.00 0.00
-
8design
-
6usability
-
7creativity
-
6content
6.75
More by Sean Miller
-
World's First Published Open-source Arduino Nano RP2040 Connect Drone Flight Controller
INTRODUCTIONThe Arduino Nano RP2040 Connect comes with a built in IMU. It's fast speed and ability t...
-
Spiderman Spidey Sense and Webslinger
INTRODUCTIONSince I was six, I thought it would be cool to make my own web caster. Not knowing much ...
-
LaserCutter_PCB_2022-02-27
GRBL shield for the Arduino Uno.Designed to allow wiring up your motors and separate motor drivers. ...
-
IoT Smart Desk Distance and KPI Meter
HTTPS://WWW.RAISINGAWESOME.SITEINTRODUCTIONThis IoT project was born out of the uncertain times pres...
You may also like
-
-
-
Modifying a Hotplate to a Reflow Solder Station
745 1 5 -
MPL3115A2 Barometric Pressure, Altitude, and Temperature Sensor
402 0 1 -
-
-
V2 Commodore AMIGA USB-C Power Sink Delivery High Efficiency Supply Triple Output 5V ±12V OLED display ATARI compatible shark 100W
961 4 2 -
How to measure weight with Load Cell and HX711
636 0 3
