//Arduino/Teensy Flight Controller - dRehmFlight //Author: Nicholas Rehm //Project Start: 1/6/2020 //Last Updated: 7/29/2022 //Version: Beta 1.3 //========================================================================================================================// //CREDITS + SPECIAL THANKS /* Some elements inspired by: http://www.brokking.net/ymfc-32_main.html Madgwick filter function adapted from: https://github.com/arduino-libraries/MadgwickAHRS MPU9250 implementation based on MPU9250 library by: brian.taylor@bolderflight.com http://www.bolderflight.com Thank you to: RcGroups 'jihlein' - IMU implementation overhaul + SBUS implementation. Everyone that sends me pictures and videos of your flying creations! -Nick */ //========================================================================================================================// // USER-SPECIFIED DEFINES // //========================================================================================================================// //Uncomment only one receiver type #define USE_PWM_RX //#define USE_PPM_RX //#define USE_SBUS_RX //#define USE_DSM_RX static const uint8_t num_DSM_channels = 6; //If using DSM RX, change this to match the number of transmitter channels you have //Uncomment only one IMU #define USE_MPU6050_I2C //Default //#define USE_MPU9250_SPI //Uncomment only one full scale gyro range (deg/sec) #define GYRO_250DPS //Default //#define GYRO_500DPS //#define GYRO_1000DPS //#define GYRO_2000DPS //Uncomment only one full scale accelerometer range (G's) #define ACCEL_2G //Default //#define ACCEL_4G //#define ACCEL_8G //#define ACCEL_16G //========================================================================================================================// //REQUIRED LIBRARIES (included with download in main sketch folder) #include //I2c communication #include //SPI communication #include //Commanding any extra actuators, installed with teensyduino installer #if defined USE_SBUS_RX #include "src/SBUS/SBUS.h" //sBus interface #endif #if defined USE_DSM_RX #include "src/DSMRX/DSMRX.h" #endif #if defined USE_MPU6050_I2C #include "src/MPU6050/MPU6050.h" MPU6050 mpu6050; #elif defined USE_MPU9250_SPI #include "src/MPU9250/MPU9250.h" MPU9250 mpu9250(SPI2,36); #else #error No MPU defined... #endif //========================================================================================================================// //Setup gyro and accel full scale value selection and scale factor #if defined USE_MPU6050_I2C #define GYRO_FS_SEL_250 MPU6050_GYRO_FS_250 #define GYRO_FS_SEL_500 MPU6050_GYRO_FS_500 #define GYRO_FS_SEL_1000 MPU6050_GYRO_FS_1000 #define GYRO_FS_SEL_2000 MPU6050_GYRO_FS_2000 #define ACCEL_FS_SEL_2 MPU6050_ACCEL_FS_2 #define ACCEL_FS_SEL_4 MPU6050_ACCEL_FS_4 #define ACCEL_FS_SEL_8 MPU6050_ACCEL_FS_8 #define ACCEL_FS_SEL_16 MPU6050_ACCEL_FS_16 #elif defined USE_MPU9250_SPI #define GYRO_FS_SEL_250 mpu9250.GYRO_RANGE_250DPS #define GYRO_FS_SEL_500 mpu9250.GYRO_RANGE_500DPS #define GYRO_FS_SEL_1000 mpu9250.GYRO_RANGE_1000DPS #define GYRO_FS_SEL_2000 mpu9250.GYRO_RANGE_2000DPS #define ACCEL_FS_SEL_2 mpu9250.ACCEL_RANGE_2G #define ACCEL_FS_SEL_4 mpu9250.ACCEL_RANGE_4G #define ACCEL_FS_SEL_8 mpu9250.ACCEL_RANGE_8G #define ACCEL_FS_SEL_16 mpu9250.ACCEL_RANGE_16G #endif #if defined GYRO_250DPS #define GYRO_SCALE GYRO_FS_SEL_250 #define GYRO_SCALE_FACTOR 131.0 #elif defined GYRO_500DPS #define GYRO_SCALE GYRO_FS_SEL_500 #define GYRO_SCALE_FACTOR 65.5 #elif defined GYRO_1000DPS #define GYRO_SCALE GYRO_FS_SEL_1000 #define GYRO_SCALE_FACTOR 32.8 #elif defined GYRO_2000DPS #define GYRO_SCALE GYRO_FS_SEL_2000 #define GYRO_SCALE_FACTOR 16.4 #endif #if defined ACCEL_2G #define ACCEL_SCALE ACCEL_FS_SEL_2 #define ACCEL_SCALE_FACTOR 16384.0 #elif defined ACCEL_4G #define ACCEL_SCALE ACCEL_FS_SEL_4 #define ACCEL_SCALE_FACTOR 8192.0 #elif defined ACCEL_8G #define ACCEL_SCALE ACCEL_FS_SEL_8 #define ACCEL_SCALE_FACTOR 4096.0 #elif defined ACCEL_16G #define ACCEL_SCALE ACCEL_FS_SEL_16 #define ACCEL_SCALE_FACTOR 2048.0 #endif //========================================================================================================================// // USER-SPECIFIED VARIABLES // //========================================================================================================================// //Radio failsafe values for every channel in the event that bad reciever data is detected. Recommended defaults: unsigned long channel_1_fs = 1000; //thro unsigned long channel_2_fs = 1500; //ail unsigned long channel_3_fs = 1500; //elev unsigned long channel_4_fs = 1500; //rudd unsigned long channel_5_fs = 2000; //gear, greater than 1500 = throttle cut unsigned long channel_6_fs = 2000; //aux1 //Filter parameters - Defaults tuned for 2kHz loop rate; Do not touch unless you know what you are doing: float B_madgwick = 0.04; //Madgwick filter parameter float B_accel = 0.14; //Accelerometer LP filter paramter, (MPU6050 default: 0.14. MPU9250 default: 0.2) float B_gyro = 0.1; //Gyro LP filter paramter, (MPU6050 default: 0.1. MPU9250 default: 0.17) float B_mag = 1.0; //Magnetometer LP filter parameter //Magnetometer calibration parameters - if using MPU9250, uncomment calibrateMagnetometer() in void setup() to get these values, else just ignore these float MagErrorX = 0.0; float MagErrorY = 0.0; float MagErrorZ = 0.0; float MagScaleX = 1.0; float MagScaleY = 1.0; float MagScaleZ = 1.0; //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.0; float AccErrorY = 0.0; float AccErrorZ = 0.0; float GyroErrorX = 0.0; float GyroErrorY= 0.0; float GyroErrorZ = 0.0; //Controller parameters (take note of defaults before modifying!): float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0) float maxRoll = 30.0; //Max roll angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode float maxPitch = 30.0; //Max pitch angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode float maxYaw = 160.0; //Max yaw rate in deg/sec float Kp_roll_angle = 0.2; //Roll P-gain - angle mode float Ki_roll_angle = 0.3; //Roll I-gain - angle mode float Kd_roll_angle = 0.05; //Roll D-gain - angle mode (has no effect on controlANGLE2) float B_loop_roll = 0.9; //Roll damping term for controlANGLE2(), lower is more damping (must be between 0 to 1) float Kp_pitch_angle = 0.2; //Pitch P-gain - angle mode float Ki_pitch_angle = 0.3; //Pitch I-gain - angle mode float Kd_pitch_angle = 0.05; //Pitch D-gain - angle mode (has no effect on controlANGLE2) float B_loop_pitch = 0.9; //Pitch damping term for controlANGLE2(), lower is more damping (must be between 0 to 1) float Kp_roll_rate = 0.15; //Roll P-gain - rate mode float Ki_roll_rate = 0.2; //Roll I-gain - rate mode float Kd_roll_rate = 0.0002; //Roll D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) float Kp_pitch_rate = 0.15; //Pitch P-gain - rate mode float Ki_pitch_rate = 0.2; //Pitch I-gain - rate mode float Kd_pitch_rate = 0.0002; //Pitch D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) float Kp_yaw = 0.3; //Yaw P-gain float Ki_yaw = 0.05; //Yaw I-gain float Kd_yaw = 0.00015; //Yaw D-gain (be careful when increasing too high, motors will begin to overheat!) //========================================================================================================================// // DECLARE PINS // //========================================================================================================================// //NOTE: Pin 13 is reserved for onboard LED, pins 18 and 19 are reserved for the MPU6050 IMU for default setup //Radio: //Note: If using SBUS, connect to pin 21 (RX5), if using DSM, connect to pin 15 (RX3) const int ch1Pin = 15; //throttle const int ch2Pin = 16; //ail const int ch3Pin = 17; //ele const int ch4Pin = 20; //rudd const int ch5Pin = 21; //gear (throttle cut) const int ch6Pin = 22; //aux1 (free aux channel) const int PPM_Pin = 23; //OneShot125 ESC pin outputs: const int m1Pin = 0; const int m2Pin = 1; const int m3Pin = 2; const int m4Pin = 3; const int m5Pin = 4; const int m6Pin = 5; //PWM servo or ESC outputs: const int servo1Pin = 6; const int servo2Pin = 7; const int servo3Pin = 8; const int servo4Pin = 9; const int servo5Pin = 10; const int servo6Pin = 11; const int servo7Pin = 12; PWMServo servo1; //Create servo objects to control a servo or ESC with PWM PWMServo servo2; PWMServo servo3; PWMServo servo4; PWMServo servo5; PWMServo servo6; PWMServo servo7; //========================================================================================================================// //DECLARE GLOBAL VARIABLES //General stuff float dt; unsigned long current_time, prev_time; unsigned long print_counter, serial_counter; unsigned long blink_counter, blink_delay; bool blinkAlternate; //Radio communication: unsigned long channel_1_pwm, channel_2_pwm, channel_3_pwm, channel_4_pwm, channel_5_pwm, channel_6_pwm; unsigned long channel_1_pwm_prev, channel_2_pwm_prev, channel_3_pwm_prev, channel_4_pwm_prev; #if defined USE_SBUS_RX SBUS sbus(Serial5); uint16_t sbusChannels[16]; bool sbusFailSafe; bool sbusLostFrame; #endif #if defined USE_DSM_RX DSM1024 DSM; #endif //IMU: float AccX, AccY, AccZ; float AccX_prev, AccY_prev, AccZ_prev; float GyroX, GyroY, GyroZ; float GyroX_prev, GyroY_prev, GyroZ_prev; float MagX, MagY, MagZ; float MagX_prev, MagY_prev, MagZ_prev; float roll_IMU, pitch_IMU, yaw_IMU; float roll_IMU_prev, pitch_IMU_prev; float q0 = 1.0f; //Initialize quaternion for madgwick filter float q1 = 0.0f; float q2 = 0.0f; float q3 = 0.0f; //Normalized desired state: float thro_des, roll_des, pitch_des, yaw_des; float roll_passthru, pitch_passthru, yaw_passthru; //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, m5_command_scaled, m6_command_scaled; int m1_command_PWM, m2_command_PWM, m3_command_PWM, m4_command_PWM, m5_command_PWM, m6_command_PWM; float s1_command_scaled, s2_command_scaled, s3_command_scaled, s4_command_scaled, s5_command_scaled, s6_command_scaled, s7_command_scaled; int s1_command_PWM, s2_command_PWM, s3_command_PWM, s4_command_PWM, s5_command_PWM, s6_command_PWM, s7_command_PWM; //Flight status bool armedFly = false; //========================================================================================================================// // VOID SETUP // //========================================================================================================================// void setup() { Serial.begin(500000); //USB serial delay(500); //Initialize all pins pinMode(13, OUTPUT); //Pin 13 LED blinker on board, do not modify pinMode(m1Pin, OUTPUT); pinMode(m2Pin, OUTPUT); pinMode(m3Pin, OUTPUT); pinMode(m4Pin, OUTPUT); pinMode(m5Pin, OUTPUT); pinMode(m6Pin, OUTPUT); servo1.attach(servo1Pin, 900, 2100); //Pin, min PWM value, max PWM value servo2.attach(servo2Pin, 900, 2100); servo3.attach(servo3Pin, 900, 2100); servo4.attach(servo4Pin, 900, 2100); servo5.attach(servo5Pin, 900, 2100); servo6.attach(servo6Pin, 900, 2100); servo7.attach(servo7Pin, 900, 2100); //Set built in LED to turn on to signal startup digitalWrite(13, HIGH); delay(5); //Initialize radio communication radioSetup(); //Set radio channels to default (safe) values before entering main loop channel_1_pwm = channel_1_fs; channel_2_pwm = channel_2_fs; channel_3_pwm = channel_3_fs; channel_4_pwm = channel_4_fs; channel_5_pwm = channel_5_fs; channel_6_pwm = channel_6_fs; //Initialize IMU communication IMUinit(); delay(5); //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. //Arm servo channels servo1.write(0); //Command servo angle from 0-180 degrees (1000 to 2000 PWM) servo2.write(0); //Set these to 90 for servos if you do not want them to briefly max out on startup servo3.write(0); //Keep these at 0 if you are using servo outputs for motors servo4.write(0); servo5.write(0); servo6.write(0); servo7.write(0); delay(5); //calibrateESCs(); //PROPS OFF. Uncomment this to calibrate your ESCs by setting throttle stick to max, powering on, and lowering throttle to zero after the beeps //Code will not proceed past here if this function is uncommented! //Arm OneShot125 motors m1_command_PWM = 125; //Command OneShot125 ESC from 125 to 250us pulse length m2_command_PWM = 125; m3_command_PWM = 125; m4_command_PWM = 125; m5_command_PWM = 125; m6_command_PWM = 125; armMotors(); //Loop over commandMotors() until ESCs happily arm //Indicate entering main loop with 3 quick blinks setupBlink(3,160,70); //numBlinks, upTime (ms), downTime (ms) //If using MPU9250 IMU, uncomment for one-time magnetometer calibration (may need to repeat for new locations) //calibrateMagnetometer(); //Generates magentometer error and scale factors to be pasted in user-specified variables section } //========================================================================================================================// // MAIN LOOP // //========================================================================================================================// void loop() { //Keep track of what time it is and how much time has elapsed since the last loop prev_time = current_time; current_time = micros(); dt = (current_time - prev_time)/1000000.0; loopBlink(); //Indicate we are in main loop with short blink every 1.5 seconds //Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE: //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) //printMagData(); //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300) //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: 120 to 250) //printServoCommands(); //Prints the values being written to the servos (expected: 0 to 180) //printLoopRate(); //Prints the time between loops in microseconds (expected: microseconds between loop iterations) // Get arming status armedStatus(); //Check if the throttle cut is off and throttle is low. //Get vehicle state getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU angle estimates (degrees) //Compute desired state getDesState(); //Convert raw commands to normalized values based on saturated control limits //PID Controller - SELECT ONE: controlANGLE(); //Stabilize on angle setpoint //controlANGLE2(); //Stabilize on angle setpoint using cascaded method. Rate controller must be tuned well first! //controlRATE(); //Stabilize on rate setpoint //Actuator mixing and scaling to PWM values controlMixer(); //Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here scaleCommands(); //Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) //Throttle cut check throttleCut(); //Directly sets motor commands to low based on state of ch5 //Command actuators commandMotors(); //Sends command pulses to each motor pin using OneShot125 protocol servo1.write(s1_command_PWM); //Writes PWM value to servo object servo2.write(s2_command_PWM); servo3.write(s3_command_PWM); servo4.write(s4_command_PWM); servo5.write(s5_command_PWM); servo6.write(s6_command_PWM); servo7.write(s7_command_PWM); //Get vehicle commands for next loop iteration getCommands(); //Pulls current available radio commands failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup //Regulate loop rate loopRate(2000); //Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default } //========================================================================================================================// // 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. Can also apply direct unstabilized commands from the transmitter with * roll_passthru, pitch_passthru, and yaw_passthu. mX_command_scaled and sX_command scaled variables are used in scaleCommands() * in preparation to be sent to the motor ESCs and servos. * *Relevant variables: *thro_des - direct thottle control *roll_PID, pitch_PID, yaw_PID - stabilized axis variables *roll_passthru, pitch_passthru, yaw_passthru - direct unstabilized command passthrough *channel_6_pwm - free auxillary channel, can be used to toggle things with an 'if' statement */ //Quad mixing - EXAMPLE m1_command_scaled = thro_des - pitch_PID + roll_PID + yaw_PID; //Front Left m2_command_scaled = thro_des - pitch_PID - roll_PID - yaw_PID; //Front Right m3_command_scaled = thro_des + pitch_PID - roll_PID + yaw_PID; //Back Right m4_command_scaled = thro_des + pitch_PID + roll_PID - yaw_PID; //Back Left m5_command_scaled = 0; m6_command_scaled = 0; //0.5 is centered servo, 0.0 is zero throttle if connecting to ESC for conventional PWM, 1.0 is max throttle s1_command_scaled = 0; s2_command_scaled = 0; s3_command_scaled = 0; s4_command_scaled = 0; s5_command_scaled = 0; s6_command_scaled = 0; s7_command_scaled = 0; } void armedStatus() { //DESCRIPTION: Check if the throttle cut is off and the throttle input is low to prepare for flight. if ((channel_5_pwm < 1500) && (channel_1_pwm < 1050)) { armedFly = true; } } void IMUinit() { //DESCRIPTION: Initialize IMU /* * Don't worry about how this works. */ #if defined USE_MPU6050_I2C Wire.begin(); Wire.setClock(1000000); //Note this is 2.5 times the spec sheet 400 kHz max... mpu6050.initialize(); if (mpu6050.testConnection() == false) { Serial.println("MPU6050 initialization unsuccessful"); Serial.println("Check MPU6050 wiring or try cycling power"); while(1) {} } //From the reset state all registers should be 0x00, so we should be at //max sample rate with digital low pass filter(s) off. All we need to //do is set the desired fullscale ranges mpu6050.setFullScaleGyroRange(GYRO_SCALE); mpu6050.setFullScaleAccelRange(ACCEL_SCALE); #elif defined USE_MPU9250_SPI int status = mpu9250.begin(); if (status < 0) { Serial.println("MPU9250 initialization unsuccessful"); Serial.println("Check MPU9250 wiring or try cycling power"); Serial.print("Status: "); Serial.println(status); while(1) {} } //From the reset state all registers should be 0x00, so we should be at //max sample rate with digital low pass filter(s) off. All we need to //do is set the desired fullscale ranges mpu9250.setGyroRange(GYRO_SCALE); mpu9250.setAccelRange(ACCEL_SCALE); mpu9250.setMagCalX(MagErrorX, MagScaleX); mpu9250.setMagCalY(MagErrorY, MagScaleY); mpu9250.setMagCalZ(MagErrorZ, MagScaleZ); mpu9250.setSrd(0); //sets gyro and accel read to 1khz, magnetometer read to 100hz #endif } void getIMUdata() { //DESCRIPTION: Request full dataset from IMU and LP filter gyro, accelerometer, and magnetometer data /* * Reads accelerometer, gyro, and magnetometer data from IMU as AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, MagZ. * These values are scaled according to the IMU datasheet to put them into correct units of g's, deg/sec, and uT. A simple first-order * low-pass filter is used to get rid of high frequency noise in these raw signals. Generally you want to cut * off everything past 80Hz, but if your loop rate is not fast enough, the low pass filter will cause a lag in * the readings. The filter parameters B_gyro and B_accel are set to be good for a 2kHz loop rate. Finally, * the constant errors found in calculate_IMU_error() on startup are subtracted from the accelerometer and gyro readings. */ int16_t AcX,AcY,AcZ,GyX,GyY,GyZ,MgX,MgY,MgZ; #if defined USE_MPU6050_I2C mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); #elif defined USE_MPU9250_SPI mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); #endif //Accelerometer AccX = AcX / ACCEL_SCALE_FACTOR; //G's AccY = AcY / ACCEL_SCALE_FACTOR; AccZ = AcZ / ACCEL_SCALE_FACTOR; //Correct the outputs with the calculated error values AccX = AccX - AccErrorX; AccY = AccY - AccErrorY; AccZ = AccZ - AccErrorZ; //LP filter accelerometer data AccX = (1.0 - B_accel)*AccX_prev + B_accel*AccX; AccY = (1.0 - B_accel)*AccY_prev + B_accel*AccY; AccZ = (1.0 - B_accel)*AccZ_prev + B_accel*AccZ; AccX_prev = AccX; AccY_prev = AccY; AccZ_prev = AccZ; //Gyro GyroX = GyX / GYRO_SCALE_FACTOR; //deg/sec GyroY = GyY / GYRO_SCALE_FACTOR; GyroZ = GyZ / GYRO_SCALE_FACTOR; //Correct the outputs with the calculated error values GyroX = GyroX - GyroErrorX; GyroY = GyroY - GyroErrorY; GyroZ = GyroZ - GyroErrorZ; //LP filter gyro data GyroX = (1.0 - B_gyro)*GyroX_prev + B_gyro*GyroX; GyroY = (1.0 - B_gyro)*GyroY_prev + B_gyro*GyroY; GyroZ = (1.0 - B_gyro)*GyroZ_prev + B_gyro*GyroZ; GyroX_prev = GyroX; GyroY_prev = GyroY; GyroZ_prev = GyroZ; //Magnetometer MagX = MgX/6.0; //uT MagY = MgY/6.0; MagZ = MgZ/6.0; //Correct the outputs with the calculated error values MagX = (MagX - MagErrorX)*MagScaleX; MagY = (MagY - MagErrorY)*MagScaleY; MagZ = (MagZ - MagErrorZ)*MagScaleZ; //LP filter magnetometer data MagX = (1.0 - B_mag)*MagX_prev + B_mag*MagX; MagY = (1.0 - B_mag)*MagY_prev + B_mag*MagY; MagZ = (1.0 - B_mag)*MagZ_prev + B_mag*MagZ; MagX_prev = MagX; MagY_prev = MagY; MagZ_prev = MagZ; } void calculate_IMU_error() { //DESCRIPTION: Computes IMU accelerometer and gyro error on startup. Note: vehicle should be powered up on flat surface /* * Don't worry too much about what this is doing. The error values it computes are applied to the raw gyro and * accelerometer values AccX, AccY, AccZ, GyroX, GyroY, GyroZ in getIMUdata(). This eliminates drift in the * measurement. */ int16_t AcX,AcY,AcZ,GyX,GyY,GyZ,MgX,MgY,MgZ; AccErrorX = 0.0; AccErrorY = 0.0; AccErrorZ = 0.0; GyroErrorX = 0.0; GyroErrorY= 0.0; GyroErrorZ = 0.0; //Read IMU values 12000 times int c = 0; while (c < 12000) { #if defined USE_MPU6050_I2C mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); #elif defined USE_MPU9250_SPI mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); #endif AccX = AcX / ACCEL_SCALE_FACTOR; AccY = AcY / ACCEL_SCALE_FACTOR; AccZ = AcZ / ACCEL_SCALE_FACTOR; GyroX = GyX / GYRO_SCALE_FACTOR; GyroY = GyY / GYRO_SCALE_FACTOR; GyroZ = GyZ / GYRO_SCALE_FACTOR; //Sum all readings AccErrorX = AccErrorX + AccX; AccErrorY = AccErrorY + AccY; AccErrorZ = AccErrorZ + AccZ; GyroErrorX = GyroErrorX + GyroX; GyroErrorY = GyroErrorY + GyroY; GyroErrorZ = GyroErrorZ + GyroZ; 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."); } void calibrateAttitude() { //DESCRIPTION: Used to warm up the main loop to allow the madwick filter to converge before commands can be sent to the actuators //Assuming vehicle is powered up on level surface! /* * This function is used on startup to warm up the attitude estimation and is what causes startup to take a few seconds * to boot. */ //Warm up IMU and madgwick filter in simulated main loop for (int i = 0; i <= 10000; i++) { prev_time = current_time; current_time = micros(); dt = (current_time - prev_time)/1000000.0; getIMUdata(); Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); loopRate(2000); //do not exceed 2000Hz } } void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float invSampleFreq) { //DESCRIPTION: Attitude estimation through sensor fusion - 9DOF /* * This function fuses the accelerometer gyro, and magnetometer readings AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, and MagZ for attitude estimation. * Don't worry about the math. There is a tunable parameter B_madgwick in the user specified variable section which basically * adjusts the weight of gyro data in the state estimate. Higher beta leads to noisier estimate, lower * beta leads to slower to respond estimate. It is currently tuned for 2kHz loop rate. This function updates the roll_IMU, * pitch_IMU, and yaw_IMU variables which are in degrees. If magnetometer data is not available, this function calls Madgwick6DOF() instead. */ float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; float hx, hy; float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; //use 6DOF algorithm if MPU6050 is being used #if defined USE_MPU6050_I2C Madgwick6DOF(gx, gy, gz, ax, ay, az, invSampleFreq); return; #endif //Use 6DOF algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { Madgwick6DOF(gx, gy, gz, ax, ay, az, invSampleFreq); return; } //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; //Normalise magnetometer measurement recipNorm = invSqrt(mx * mx + my * my + mz * mz); mx *= recipNorm; my *= recipNorm; mz *= recipNorm; //Auxiliary variables to avoid repeated arithmetic _2q0mx = 2.0f * q0 * mx; _2q0my = 2.0f * q0 * my; _2q0mz = 2.0f * q0 * mz; _2q1mx = 2.0f * q1 * mx; _2q0 = 2.0f * q0; _2q1 = 2.0f * q1; _2q2 = 2.0f * q2; _2q3 = 2.0f * q3; _2q0q2 = 2.0f * q0 * q2; _2q2q3 = 2.0f * q2 * q3; q0q0 = q0 * q0; q0q1 = q0 * q1; q0q2 = q0 * q2; q0q3 = q0 * q3; q1q1 = q1 * q1; q1q2 = q1 * q2; q1q3 = q1 * q3; q2q2 = q2 * q2; q2q3 = q2 * q3; q3q3 = q3 * q3; //Reference direction of Earth's magnetic field hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; _2bx = sqrtf(hx * hx + hy * hy); _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; _4bx = 2.0f * _2bx; _4bz = 2.0f * _2bz; //Gradient decent algorithm corrective step s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); 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 * invSampleFreq; q1 += qDot2 * invSampleFreq; q2 += qDot3 * invSampleFreq; q3 += qDot4 * invSampleFreq; //Normalize quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; //compute angles - NWU roll_IMU = atan2(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)*57.29577951; //degrees pitch_IMU = -asin(constrain(-2.0f * (q1*q3 - q0*q2),-0.999999,0.999999))*57.29577951; //degrees yaw_IMU = -atan2(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3)*57.29577951; //degrees } void Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, float invSampleFreq) { //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 * invSampleFreq; q1 += qDot2 * invSampleFreq; q2 += qDot3 * invSampleFreq; q3 += qDot4 * invSampleFreq; //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(constrain(-2.0f * (q1*q3 - q0*q2),-0.999999,0.999999))*57.29577951; //degrees yaw_IMU = -atan2(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3)*57.29577951; //degrees } void getDesState() { //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 either degrees (angle mode) or degrees/sec * (rate mode). yaw_des is scaled to be within max yaw in degrees/sec. Also creates roll_passthru, pitch_passthru, and * yaw_passthru variables, to be used in commanding motors/servos with direct unstabilized commands in controlMixer(). */ thro_des = (channel_1_pwm - 1000.0)/1000.0; //Between 0 and 1 roll_des = (channel_2_pwm - 1500.0)/500.0; //Between -1 and 1 pitch_des = (channel_3_pwm - 1500.0)/500.0; //Between -1 and 1 yaw_des = (channel_4_pwm - 1500.0)/500.0; //Between -1 and 1 roll_passthru = roll_des/2.0; //Between -0.5 and 0.5 pitch_passthru = pitch_des/2.0; //Between -0.5 and 0.5 yaw_passthru = yaw_des/2.0; //Between -0.5 and 0.5 //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 roll_passthru = constrain(roll_passthru, -0.5, 0.5); pitch_passthru = constrain(pitch_passthru, -0.5, 0.5); yaw_passthru = constrain(yaw_passthru, -0.5, 0.5); } void controlANGLE() { //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 * getDesState(). 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(). */ //Roll error_roll = roll_des - roll_IMU; integral_roll = integral_roll_prev + error_roll*dt; if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low integral_roll = 0; } integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup derivative_roll = GyroX; roll_PID = 0.01*(Kp_roll_angle*error_roll + Ki_roll_angle*integral_roll - Kd_roll_angle*derivative_roll); //Scaled by .01 to bring within -1 to 1 range //Pitch error_pitch = pitch_des - pitch_IMU; integral_pitch = integral_pitch_prev + error_pitch*dt; if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low integral_pitch = 0; } integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup derivative_pitch = GyroY; pitch_PID = .01*(Kp_pitch_angle*error_pitch + Ki_pitch_angle*integral_pitch - Kd_pitch_angle*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range //Yaw, stablize on rate from GyroZ error_yaw = yaw_des - GyroZ; integral_yaw = integral_yaw_prev + error_yaw*dt; if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low integral_yaw = 0; } integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup derivative_yaw = (error_yaw - error_yaw_prev)/dt; yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range //Update roll variables integral_roll_prev = integral_roll; //Update pitch variables integral_pitch_prev = integral_pitch; //Update yaw variables error_yaw_prev = error_yaw; integral_yaw_prev = integral_yaw; } void controlANGLE2() { //DESCRIPTION: Computes control commands based on state error (angle) in cascaded scheme /* * Gives better performance than controlANGLE() but requires much more tuning. Not reccommended for first-time setup. * See the documentation for tuning this controller. */ //Outer loop - PID on angle float roll_des_ol, pitch_des_ol; //Roll error_roll = roll_des - roll_IMU; integral_roll_ol = integral_roll_prev_ol + error_roll*dt; if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low integral_roll_ol = 0; } integral_roll_ol = constrain(integral_roll_ol, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup derivative_roll = (roll_IMU - roll_IMU_prev)/dt; roll_des_ol = Kp_roll_angle*error_roll + Ki_roll_angle*integral_roll_ol;// - Kd_roll_angle*derivative_roll; //Pitch error_pitch = pitch_des - pitch_IMU; integral_pitch_ol = integral_pitch_prev_ol + error_pitch*dt; if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low integral_pitch_ol = 0; } integral_pitch_ol = constrain(integral_pitch_ol, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup derivative_pitch = (pitch_IMU - pitch_IMU_prev)/dt; pitch_des_ol = Kp_pitch_angle*error_pitch + Ki_pitch_angle*integral_pitch_ol;// - Kd_pitch_angle*derivative_pitch; //Apply loop gain, constrain, and LP filter for artificial damping float Kl = 30.0; roll_des_ol = Kl*roll_des_ol; pitch_des_ol = Kl*pitch_des_ol; roll_des_ol = constrain(roll_des_ol, -240.0, 240.0); pitch_des_ol = constrain(pitch_des_ol, -240.0, 240.0); roll_des_ol = (1.0 - B_loop_roll)*roll_des_prev + B_loop_roll*roll_des_ol; pitch_des_ol = (1.0 - B_loop_pitch)*pitch_des_prev + B_loop_pitch*pitch_des_ol; //Inner loop - PID on rate //Roll error_roll = roll_des_ol - GyroX; integral_roll_il = integral_roll_prev_il + error_roll*dt; if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low integral_roll_il = 0; } integral_roll_il = constrain(integral_roll_il, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup derivative_roll = (error_roll - error_roll_prev)/dt; roll_PID = .01*(Kp_roll_rate*error_roll + Ki_roll_rate*integral_roll_il + Kd_roll_rate*derivative_roll); //Scaled by .01 to bring within -1 to 1 range //Pitch error_pitch = pitch_des_ol - GyroY; integral_pitch_il = integral_pitch_prev_il + error_pitch*dt; if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low integral_pitch_il = 0; } integral_pitch_il = constrain(integral_pitch_il, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup derivative_pitch = (error_pitch - error_pitch_prev)/dt; pitch_PID = .01*(Kp_pitch_rate*error_pitch + Ki_pitch_rate*integral_pitch_il + Kd_pitch_rate*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range //Yaw error_yaw = yaw_des - GyroZ; integral_yaw = integral_yaw_prev + error_yaw*dt; if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low integral_yaw = 0; } integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup derivative_yaw = (error_yaw - error_yaw_prev)/dt; yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range //Update roll variables integral_roll_prev_ol = integral_roll_ol; integral_roll_prev_il = integral_roll_il; error_roll_prev = error_roll; roll_IMU_prev = roll_IMU; roll_des_prev = roll_des_ol; //Update pitch variables integral_pitch_prev_ol = integral_pitch_ol; integral_pitch_prev_il = integral_pitch_il; error_pitch_prev = error_pitch; pitch_IMU_prev = pitch_IMU; pitch_des_prev = pitch_des_ol; //Update yaw variables error_yaw_prev = error_yaw; integral_yaw_prev = integral_yaw; } void controlRATE() { //DESCRIPTION: Computes control commands based on state error (rate) /* * See explanation for controlANGLE(). Everything is the same here except the error is now the desired rate - raw gyro reading. */ //Roll error_roll = roll_des - GyroX; integral_roll = integral_roll_prev + error_roll*dt; if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low integral_roll = 0; } integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup derivative_roll = (error_roll - error_roll_prev)/dt; roll_PID = .01*(Kp_roll_rate*error_roll + Ki_roll_rate*integral_roll + Kd_roll_rate*derivative_roll); //Scaled by .01 to bring within -1 to 1 range //Pitch error_pitch = pitch_des - GyroY; integral_pitch = integral_pitch_prev + error_pitch*dt; if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low integral_pitch = 0; } integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup derivative_pitch = (error_pitch - error_pitch_prev)/dt; pitch_PID = .01*(Kp_pitch_rate*error_pitch + Ki_pitch_rate*integral_pitch + Kd_pitch_rate*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range //Yaw, stablize on rate from GyroZ error_yaw = yaw_des - GyroZ; integral_yaw = integral_yaw_prev + error_yaw*dt; if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low integral_yaw = 0; } integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup derivative_yaw = (error_yaw - error_yaw_prev)/dt; yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range //Update roll variables error_roll_prev = error_roll; integral_roll_prev = integral_roll; GyroX_prev = GyroX; //Update pitch variables error_pitch_prev = error_pitch; integral_pitch_prev = integral_pitch; GyroY_prev = GyroY; //Update yaw variables error_yaw_prev = error_yaw; integral_yaw_prev = integral_yaw; } void scaleCommands() { //DESCRIPTION: Scale normalized actuator commands to values for ESC/Servo protocol /* * mX_command_scaled variables from the mixer function are scaled to 125-250us for OneShot125 protocol. sX_command_scaled variables from * the mixer function are scaled to 0-180 for the servo library using standard PWM. * mX_command_PWM are updated here which are used to command the motors in commandMotors(). sX_command_PWM are updated * which are used to command the servos. */ //Scaled to 125us - 250us for oneshot125 protocol m1_command_PWM = m1_command_scaled*125 + 125; m2_command_PWM = m2_command_scaled*125 + 125; m3_command_PWM = m3_command_scaled*125 + 125; m4_command_PWM = m4_command_scaled*125 + 125; m5_command_PWM = m5_command_scaled*125 + 125; m6_command_PWM = m6_command_scaled*125 + 125; //Constrain commands to motors within oneshot125 bounds m1_command_PWM = constrain(m1_command_PWM, 125, 250); m2_command_PWM = constrain(m2_command_PWM, 125, 250); m3_command_PWM = constrain(m3_command_PWM, 125, 250); m4_command_PWM = constrain(m4_command_PWM, 125, 250); m5_command_PWM = constrain(m5_command_PWM, 125, 250); m6_command_PWM = constrain(m6_command_PWM, 125, 250); //Scaled to 0-180 for servo library s1_command_PWM = s1_command_scaled*180; s2_command_PWM = s2_command_scaled*180; s3_command_PWM = s3_command_scaled*180; s4_command_PWM = s4_command_scaled*180; s5_command_PWM = s5_command_scaled*180; s6_command_PWM = s6_command_scaled*180; s7_command_PWM = s7_command_scaled*180; //Constrain commands to servos within servo library bounds s1_command_PWM = constrain(s1_command_PWM, 0, 180); s2_command_PWM = constrain(s2_command_PWM, 0, 180); s3_command_PWM = constrain(s3_command_PWM, 0, 180); s4_command_PWM = constrain(s4_command_PWM, 0, 180); s5_command_PWM = constrain(s5_command_PWM, 0, 180); s6_command_PWM = constrain(s6_command_PWM, 0, 180); s7_command_PWM = constrain(s7_command_PWM, 0, 180); } void getCommands() { //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. If using a PWM or PPM receiver, the radio commands are retrieved from a function in the readPWM file separate from this one which * is running a bunch of interrupts to continuously update the radio readings. If using an SBUS receiver, the alues are pulled from the SBUS library directly. * The raw radio commands are filtered with a first order low-pass filter to eliminate any really high frequency noise. */ #if defined USE_PPM_RX || defined USE_PWM_RX channel_1_pwm = getRadioPWM(1); channel_2_pwm = getRadioPWM(2); channel_3_pwm = getRadioPWM(3); channel_4_pwm = getRadioPWM(4); channel_5_pwm = getRadioPWM(5); channel_6_pwm = getRadioPWM(6); #elif defined USE_SBUS_RX if (sbus.read(&sbusChannels[0], &sbusFailSafe, &sbusLostFrame)) { //sBus scaling below is for Taranis-Plus and X4R-SB float scale = 0.615; float bias = 895.0; channel_1_pwm = sbusChannels[0] * scale + bias; channel_2_pwm = sbusChannels[1] * scale + bias; channel_3_pwm = sbusChannels[2] * scale + bias; channel_4_pwm = sbusChannels[3] * scale + bias; channel_5_pwm = sbusChannels[4] * scale + bias; channel_6_pwm = sbusChannels[5] * scale + bias; } #elif defined USE_DSM_RX if (DSM.timedOut(micros())) { //Serial.println("*** DSM RX TIMED OUT ***"); } else if (DSM.gotNewFrame()) { uint16_t values[num_DSM_channels]; DSM.getChannelValues(values, num_DSM_channels); channel_1_pwm = values[0]; channel_2_pwm = values[1]; channel_3_pwm = values[2]; channel_4_pwm = values[3]; channel_5_pwm = values[4]; channel_6_pwm = values[5]; } #endif //Low-pass the critical commands and update previous values float b = 0.7; //Lower=slower, higher=noiser channel_1_pwm = (1.0 - b)*channel_1_pwm_prev + b*channel_1_pwm; channel_2_pwm = (1.0 - b)*channel_2_pwm_prev + b*channel_2_pwm; channel_3_pwm = (1.0 - b)*channel_3_pwm_prev + b*channel_3_pwm; channel_4_pwm = (1.0 - b)*channel_4_pwm_prev + b*channel_4_pwm; channel_1_pwm_prev = channel_1_pwm; channel_2_pwm_prev = channel_2_pwm; channel_3_pwm_prev = channel_3_pwm; channel_4_pwm_prev = channel_4_pwm; } void failSafe() { //DESCRIPTION: If radio gives garbage values, set all commands to default values /* * Radio connection failsafe used to check if the getCommands() 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. */ unsigned minVal = 800; unsigned maxVal = 2200; int check1 = 0; int check2 = 0; int check3 = 0; int check4 = 0; int check5 = 0; int check6 = 0; //Triggers for failure criteria if (channel_1_pwm > maxVal || channel_1_pwm < minVal) check1 = 1; if (channel_2_pwm > maxVal || channel_2_pwm < minVal) check2 = 1; if (channel_3_pwm > maxVal || channel_3_pwm < minVal) check3 = 1; if (channel_4_pwm > maxVal || channel_4_pwm < minVal) check4 = 1; if (channel_5_pwm > maxVal || channel_5_pwm < minVal) check5 = 1; if (channel_6_pwm > maxVal || channel_6_pwm < minVal) check6 = 1; //If any failures, set to default failsafe values if ((check1 + check2 + check3 + check4 + check5 + check6) > 0) { channel_1_pwm = channel_1_fs; channel_2_pwm = channel_2_fs; channel_3_pwm = channel_3_fs; channel_4_pwm = channel_4_fs; channel_5_pwm = channel_5_fs; channel_6_pwm = channel_6_fs; } } void commandMotors() { //DESCRIPTION: Send pulses to motor pins, oneshot125 protocol /* * My crude implimentation of OneShot125 protocol which sends 125 - 250us pulses to the ESCs (mXPin). The pulselengths being * sent are mX_command_PWM, computed in scaleCommands(). This may be replaced by something more efficient in the future. */ int wentLow = 0; int pulseStart, timer; int flagM1 = 0; int flagM2 = 0; int flagM3 = 0; int flagM4 = 0; int flagM5 = 0; int flagM6 = 0; //Write all motor pins high digitalWrite(m1Pin, HIGH); digitalWrite(m2Pin, HIGH); digitalWrite(m3Pin, HIGH); digitalWrite(m4Pin, HIGH); digitalWrite(m5Pin, HIGH); digitalWrite(m6Pin, HIGH); pulseStart = micros(); //Write each motor pin low as correct pulse length is reached while (wentLow < 6 ) { //Keep going until final (6th) pulse is finished, then done timer = micros(); if ((m1_command_PWM <= timer - pulseStart) && (flagM1==0)) { digitalWrite(m1Pin, LOW); wentLow = wentLow + 1; flagM1 = 1; } if ((m2_command_PWM <= timer - pulseStart) && (flagM2==0)) { digitalWrite(m2Pin, LOW); wentLow = wentLow + 1; flagM2 = 1; } if ((m3_command_PWM <= timer - pulseStart) && (flagM3==0)) { digitalWrite(m3Pin, LOW); wentLow = wentLow + 1; flagM3 = 1; } if ((m4_command_PWM <= timer - pulseStart) && (flagM4==0)) { digitalWrite(m4Pin, LOW); wentLow = wentLow + 1; flagM4 = 1; } if ((m5_command_PWM <= timer - pulseStart) && (flagM5==0)) { digitalWrite(m5Pin, LOW); wentLow = wentLow + 1; flagM5 = 1; } if ((m6_command_PWM <= timer - pulseStart) && (flagM6==0)) { digitalWrite(m6Pin, LOW); wentLow = wentLow + 1; flagM6 = 1; } } } void armMotors() { //DESCRIPTION: Sends many command pulses to the motors, to be used to arm motors in the void setup() /* * Loops over the commandMotors() function 50 times with a delay in between, simulating how the commandMotors() * function is used in the main loop. Ensures motors arm within the void setup() where there are some delays * for other processes that sometimes prevent motors from arming. */ for (int i = 0; i <= 50; i++) { commandMotors(); delay(2); } } 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. This function should only be * uncommented when performing an ESC calibration. */ while (true) { prev_time = current_time; current_time = micros(); dt = (current_time - prev_time)/1000000.0; digitalWrite(13, HIGH); //LED on to indicate we are not in main loop getCommands(); //Pulls current available radio commands failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup getDesState(); //Convert raw commands to normalized values based on saturated control limits getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU (degrees) getDesState(); //Convert raw commands to normalized values based on saturated control limits m1_command_scaled = thro_des; m2_command_scaled = thro_des; m3_command_scaled = thro_des; m4_command_scaled = thro_des; m5_command_scaled = thro_des; m6_command_scaled = thro_des; s1_command_scaled = thro_des; s2_command_scaled = thro_des; s3_command_scaled = thro_des; s4_command_scaled = thro_des; s5_command_scaled = thro_des; s6_command_scaled = thro_des; s7_command_scaled = thro_des; scaleCommands(); //Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) //throttleCut(); //Directly sets motor commands to low based on state of ch5 servo1.write(s1_command_PWM); servo2.write(s2_command_PWM); servo3.write(s3_command_PWM); servo4.write(s4_command_PWM); servo5.write(s5_command_PWM); servo6.write(s6_command_PWM); servo7.write(s7_command_PWM); commandMotors(); //Sends command pulses to each motor pin using OneShot125 protocol //printRadioData(); //Radio pwm values (expected: 1000 to 2000) loopRate(2000); //Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default } } float floatFaderLinear(float param, float param_min, float param_max, float fadeTime, int state, int loopFreq){ //DESCRIPTION: Linearly fades a float type variable between min and max bounds based on desired high or low state and time /* * Takes in a float variable, desired minimum and maximum bounds, fade time, high or low desired state, and the loop frequency * and linearly interpolates that param variable between the maximum and minimum bounds. This function can be called in controlMixer() * and high/low states can be determined by monitoring the state of an auxillarly radio channel. For example, if channel_6_pwm is being * monitored to switch between two dynamic configurations (hover and forward flight), this function can be called within the logical * statements in order to fade controller gains, for example between the two dynamic configurations. The 'state' (1 or 0) can be used * to designate the two final options for that control gain based on the dynamic configuration assignment to the auxillary radio channel. * */ float diffParam = (param_max - param_min)/(fadeTime*loopFreq); //Difference to add or subtract from param for each loop iteration for desired fadeTime if (state == 1) { //Maximum param bound desired, increase param by diffParam for each loop iteration param = param + diffParam; } else if (state == 0) { //Minimum param bound desired, decrease param by diffParam for each loop iteration param = param - diffParam; } param = constrain(param, param_min, param_max); //Constrain param within max bounds return param; } float floatFaderLinear2(float param, float param_des, float param_lower, float param_upper, float fadeTime_up, float fadeTime_down, int loopFreq){ //DESCRIPTION: Linearly fades a float type variable from its current value to the desired value, up or down /* * Takes in a float variable to be modified, desired new position, upper value, lower value, fade time, and the loop frequency * and linearly fades that param variable up or down to the desired value. This function can be called in controlMixer() * to fade up or down between flight modes monitored by an auxillary radio channel. For example, if channel_6_pwm is being * monitored to switch between two dynamic configurations (hover and forward flight), this function can be called within the logical * statements in order to fade controller gains, for example between the two dynamic configurations. * */ if (param > param_des) { //Need to fade down to get to desired float diffParam = (param_upper - param_des)/(fadeTime_down*loopFreq); param = param - diffParam; } else if (param < param_des) { //Need to fade up to get to desired float diffParam = (param_des - param_lower)/(fadeTime_up*loopFreq); param = param + diffParam; } param = constrain(param, param_lower, param_upper); //Constrain param within max bounds return param; } void switchRollYaw(int reverseRoll, int reverseYaw) { //DESCRIPTION: Switches roll_des and yaw_des variables for tailsitter-type configurations /* * Takes in two integers (either 1 or -1) corresponding to the desired reversing of the roll axis and yaw axis, respectively. * Reversing of the roll or yaw axis may be needed when switching between the two for some dynamic configurations. Inputs of 1, 1 does not * reverse either of them, while -1, 1 will reverse the output corresponding to the new roll axis. * This function may be replaced in the future by a function that switches the IMU data instead (so that angle can also be estimated with the * IMU tilted 90 degrees from default level). */ float switch_holder; switch_holder = yaw_des; yaw_des = reverseYaw*roll_des; roll_des = reverseRoll*switch_holder; } void throttleCut() { //DESCRIPTION: Directly set actuator outputs to minimum value if triggered /* Monitors the state of radio command channel_5_pwm and directly sets the mx_command_PWM values to minimum (120 is minimum for oneshot125 protocol, 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. channel_5_pwm is LOW then throttle cut is OFF and throttle value can change. (ThrottleCut is DEACTIVATED) channel_5_pwm is HIGH then throttle cut is ON and throttle value = 120 only. (ThrottleCut is ACTIVATED), (drone is DISARMED) */ if ((channel_5_pwm > 1500) || (armedFly == false)) { armedFly = false; m1_command_PWM = 120; m2_command_PWM = 120; m3_command_PWM = 120; m4_command_PWM = 120; m5_command_PWM = 120; m6_command_PWM = 120; //Uncomment if using servo PWM variables to control motor ESCs //s1_command_PWM = 0; //s2_command_PWM = 0; //s3_command_PWM = 0; //s4_command_PWM = 0; //s5_command_PWM = 0; //s6_command_PWM = 0; //s7_command_PWM = 0; } } void calibrateMagnetometer() { #if defined USE_MPU9250_SPI float success; Serial.println("Beginning magnetometer calibration in"); Serial.println("3..."); delay(1000); Serial.println("2..."); delay(1000); Serial.println("1..."); delay(1000); Serial.println("Rotate the IMU about all axes until complete."); Serial.println(" "); success = mpu9250.calibrateMag(); if(success) { Serial.println("Calibration Successful!"); Serial.println("Please comment out the calibrateMagnetometer() function and copy these values into the code:"); Serial.print("float MagErrorX = "); Serial.print(mpu9250.getMagBiasX_uT()); Serial.println(";"); Serial.print("float MagErrorY = "); Serial.print(mpu9250.getMagBiasY_uT()); Serial.println(";"); Serial.print("float MagErrorZ = "); Serial.print(mpu9250.getMagBiasZ_uT()); Serial.println(";"); Serial.print("float MagScaleX = "); Serial.print(mpu9250.getMagScaleFactorX()); Serial.println(";"); Serial.print("float MagScaleY = "); Serial.print(mpu9250.getMagScaleFactorY()); Serial.println(";"); Serial.print("float MagScaleZ = "); Serial.print(mpu9250.getMagScaleFactorZ()); Serial.println(";"); Serial.println(" "); Serial.println("If you are having trouble with your attitude estimate at a new flying location, repeat this process as needed."); } else { Serial.println("Calibration Unsuccessful. Please reset the board and try again."); } while(1); //Halt code so it won't enter main loop until this function commented out #endif Serial.println("Error: MPU9250 not selected. Cannot calibrate non-existent magnetometer."); while(1); //Halt code so it won't enter main loop until this function commented out } void loopRate(int freq) { //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. 2kHz is a good rate to * be at because the loop nominally will run between 2.8kHz - 4.2kHz. This lets us have a little room to add extra computations * and remain above 2kHz, without needing to retune all of our filtering parameters. */ float invFreq = 1.0/freq*1000000.0; unsigned long checker = micros(); //Sit in loop until appropriate time has passed while (invFreq > (checker - current_time)) { checker = micros(); } } void loopBlink() { //DESCRIPTION: Blink LED on board to indicate main loop is running /* * It looks cool. */ if (current_time - blink_counter > blink_delay) { blink_counter = micros(); digitalWrite(13, blinkAlternate); //Pin 13 is built in LED if (blinkAlternate == 1) { blinkAlternate = 0; blink_delay = 100000; } else if (blinkAlternate == 0) { blinkAlternate = 1; blink_delay = 2000000; } } } void setupBlink(int numBlinks,int upTime, int downTime) { //DESCRIPTION: Simple function to make LED on board blink as desired for (int j = 1; j<= numBlinks; j++) { digitalWrite(13, LOW); delay(downTime); digitalWrite(13, HIGH); delay(upTime); } } void printRadioData() { if (current_time - print_counter > 10000) { print_counter = micros(); Serial.print(F(" CH1:")); Serial.print(channel_1_pwm); Serial.print(F(" CH2:")); Serial.print(channel_2_pwm); Serial.print(F(" CH3:")); Serial.print(channel_3_pwm); Serial.print(F(" CH4:")); Serial.print(channel_4_pwm); Serial.print(F(" CH5:")); Serial.print(channel_5_pwm); Serial.print(F(" CH6:")); Serial.println(channel_6_pwm); } } 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 printMagData() { if (current_time - print_counter > 10000) { print_counter = micros(); Serial.print(F("MagX:")); Serial.print(MagX); Serial.print(F(" MagY:")); Serial.print(MagY); Serial.print(F(" MagZ:")); Serial.println(MagZ); } } 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.print(m4_command_PWM); Serial.print(F(" m5_command:")); Serial.print(m5_command_PWM); Serial.print(F(" m6_command:")); Serial.println(m6_command_PWM); } } void printServoCommands() { if (current_time - print_counter > 10000) { print_counter = micros(); Serial.print(F("s1_command:")); Serial.print(s1_command_PWM); Serial.print(F(" s2_command:")); Serial.print(s2_command_PWM); Serial.print(F(" s3_command:")); Serial.print(s3_command_PWM); Serial.print(F(" s4_command:")); Serial.print(s4_command_PWM); Serial.print(F(" s5_command:")); Serial.print(s5_command_PWM); Serial.print(F(" s6_command:")); Serial.print(s6_command_PWM); Serial.print(F(" s7_command:")); Serial.println(s7_command_PWM); } } void printLoopRate() { if (current_time - print_counter > 10000) { print_counter = micros(); Serial.print(F("dt:")); Serial.println(dt*1000000.0); } } //=========================================================================================// //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 }