diff --git a/src/drone/drone.ino b/src/drone/drone.ino index dbde5c9..19d9233 100644 --- a/src/drone/drone.ino +++ b/src/drone/drone.ino @@ -1,32 +1,1735 @@ -#include "conf.h" -#include -#include -#include -#include -#include -#include -#include +//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() { - // Setup all ESC -ledcAttach(MOTOR1, PWMFREQ, PWMRESOLUTION); -ledcAttach(MOTOR2, PWMFREQ, PWMRESOLUTION); -ledcAttach(MOTOR3, PWMFREQ, PWMRESOLUTION); -ledcAttach(MOTOR4, PWMFREQ, PWMRESOLUTION); + 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() { - // put your main code here, to run repeatedly: + //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 } -void calibrateESC() { -ledcWrite(MOTOR1, 1100); -ledcWrite(MOTOR2, 1100); -ledcWrite(MOTOR3, 1100); -ledcWrite(MOTOR4, 1100); + +//========================================================================================================================// +// 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; + } -//https://randomnerdtutorials.com/esp32-pwm-arduino-ide/ \ No newline at end of file +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 +} \ No newline at end of file