Files
Drone-FabAcademy-2025/src/drone/drone.ino

1735 lines
70 KiB
C++

//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 <Wire.h> //I2c communication
#include <SPI.h> //SPI communication
#include <PWMServo.h> //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
}