For the next part of my project I decided to work on the code. I wrote the code for this using Arduino's development software.
IMU
One of the first things I tried to implement was the IMU. I figured getting all the motors to run at the same time and at the same speed might be difficult. Any variance in the motor speeds would put the drone off balance. Also if there was any wind it would also knock the drone out of position. So we needed a way for the drone to correct itself. To do this we decided to buy an IMU from sparkfun. We went with the 9 degrees of freedom board which has a three axis accelerometer, gyroscope, and magnetometer. At the time is seemed like all three sets of sensors would be really useful (and in the future they might be) but as of right now, I only implemented the accelerometer into the code.
The IMU was pretty easy to set up, it just required some basic soldering (which I still managed to screw up the first time, but that's what solder wick is for). The sparkfun website has a very clear tutorial which made it very easy.
I downloaded sparkfun's arduino library for the IMU which contained example code on the different functions that could be used with the IMU and it they even coded in the math equations to calculate pitch and roll. This was the main piece of code that I needed so I just copied their functions over to my program.
Radio Controller
The next thing we needed was a controller to guide the drone. We decided radio frequency would be the best direction to go with here since the technology was fairly simple and cheap. We bought a goolRC 6 channel controller/transmitter and reciever. The transmitter obviously sends a signal to the reciever but I wasn't sure what kind of signal this was and the manual that came with the controller wasn't any help. The reciever has seven groups of three pins, six are channel outputs with a signal, power, and ground pin, and the last group is the input power and ground. These pin grouping are designed for servo motors to be directly attached to the reciever. However, for a drone set up this doesn't work because an input, such as increasing the throttle, would need to change the rpm on all four motors not one. What I needed instead was to receive the signal for the four inputs (throttle, pitch, roll, and yaw), process it, and send the modified signal to the ESCs.
In order to do this I used the pulled the reading from the servo pin with the Arduino and then wrote some code to process how fast the motors needed to spin based off of the read inputs. Each motor would receive a base level of power depending on what the throttle input was and then each motor would have a value added or subtracted from it depending on if there was any input to adjust the pitch, roll, or yaw.
With this implemented we were only using four of our six channels. The radio controller has four switches and two dials that you can choose from to send signal through the last two channels with. Since the Arduino runs on a loop one of the switches needed to be used as a kill power switch, sending signals of 0 to each motor when it was activated. This would allow us to plug in the drone and get things set up without any of the motors turning on. The last channel was attached to another switch that a hover funtion was attached to.
Another key element was the remapping after adding in the pitch, roll, and yaw calculations. If a motor was receiving a value of 2000 (which is the upper limit for these ESC's) and suddenly there was a roll input that added 200 to this value, even though the motor would be receiving the value of 2200 it would be spinning any faster than it was before because it was already at the upper limit of it's angular velocity. The same scenario could happen if a motor was told to drop below a value of 1100. Anything below that would not be spinning at all.
This would mess up the flight pattern quite a bit if the Arduino needed a motor to spin faster or slower and it wasn't able to. In order to counteract this problem I wrote a remapping function that detected if any of the motors ever go above 2000 or dip below 1100 it will remap all four motor values so that they all still have the same ratio yet will all be within the allowed range of inputs.
I have posted the code I wrote below for reference.
//Valkyrie drone control and autopilot code
//created and built by Jake Rosqvist and Caleb Warner
//Utah State University
//Summer and Fall 2017 project
#include <Servo.h>
#include <Wire.h>
#include <SPI.h>
#include <SparkFunLSM9DS1.h>
LSM9DS1 imu;
#define LSM9DS1_M 0x1E;
#define LSM9DS1_AG 0x6B;
#define DECLINATION -8.58
int value = 0; // set values you need to zero
int ch1, ch2, ch3, ch4, ch5, ch6, throttle, pitch, roll, yaw, aux1, aux2; //Initialize the variables to recieve input from the controller
int input1, input2, input3, input4; //Initialize values to be passed to the esc
int p1, p2, r1, r2, greatest, least; //Initialize values to correct motor speeds
float imuPitch;
Servo esc1;
Servo esc2;
Servo esc3;
Servo esc4;
//Servo esc5;//Create the objects for the ESCs
void setup() {
Serial.begin(115200); // start serial at 115200 baud for the IMU
esc1.attach(4); //Attach each esc object to a pin
esc2.attach(5);
esc3.attach(6);
esc4.attach(7);
//esc5.attach(3);
pinMode(8, INPUT); //Set up the pins attached to the reciever as inputs
pinMode(9, INPUT);
pinMode(10, INPUT);
pinMode(11, INPUT);
pinMode(12, INPUT);
pinMode(13, INPUT);
imu.settings.device.commInterface = IMU_MODE_I2C;
imu.settings.device.mAddress = LSM9DS1_M;
imu.settings.device.agAddress = LSM9DS1_AG;
if (!imu.begin())
{
Serial.println("Failed to communicate with LSM9DS1.");
Serial.println("Double-check wiring.");
Serial.println("Default settings in this sketch will " \
"work for an out of the box LSM9DS1 " \
"Breakout, but may need to be modified " \
"if the board jumpers are.");
while (1)
;
}
}
void loop() {
if (imu.accelAvailable()) {
imu.readAccel();
}
//For some reason the pitch won't calculate unless run through a function
imuPitch = printAttitude(imu.ax, imu.ay, imu.az);
Serial.println();
float imuRoll = atan2(imu.ay, imu.az);
// Convert everything radians to degrees
imuRoll *= (180.0 / PI);
ch1 = pulseIn(8, HIGH); //pass in the original signal to the variable
ch2 = pulseIn(9, HIGH);
ch3 = pulseIn(10, HIGH);
ch4 = pulseIn(11, HIGH);
ch5 = pulseIn(12, HIGH);
ch6 = pulseIn(13, HIGH);
if(ch5 < 1500) {
input1 = 0;
input2 = 0;
input3 = 0;
input4 = 0;
}
else {
if(ch6 < 1250) {
throttle = map(ch3, 1000, 2000, 0, 700); //remap the values to fit the range for the esc
pitch = map(ch2, 1000, 2000, -35, 35);
roll = map(ch1, 1000, 2000, 35, -35);
yaw = map(ch4, 1000, 2000, -75, 75);
aux1 = map(ch5, 1000, 2000, 1200, 1800);
aux2 = map(ch6, 1000, 2000, 1200, 1800);
if(imuPitch > pitch) {
p1 = (imuPitch-pitch)*5;
p2 = (imuPitch-pitch) * 5 * -1;
}
else if(imuPitch < pitch) { //correct pitch inbalance
p1 = (pitch - imuPitch) * 5 * -1;
p2 = (pitch - imuPitch) * 5;
}
else {
p1 = 0;
p2 = 0;
}
if(imuRoll > roll){
r1 = (imuRoll - roll) *5;
r2 = (imuRoll - roll) *5*-1;
}
else if(imuRoll < roll) { //correct roll inbalance
r1 = (roll - imuRoll) * 5 * -1;
r2 = (roll - imuRoll) * 5;
}
else {
r1 = 0;
r2 = 0;
}
yaw*=3;
}
else {
if(imuPitch > 0) {
p1 = (imuPitch-pitch)*5;
p2 = (imuPitch-pitch) * 5 * -1;
}
else if(imuPitch < 0) { //correct pitch inbalance
p1 = (pitch - imuPitch) * 5 * -1;
p2 = (pitch - imuPitch) * 5;
}
else {
p1 = 0;
p2 = 0;
}
if(imuRoll > 0){
r1 = (imuRoll - roll) *5;
r2 = (imuRoll - roll) *5*-1;
}
else if(imuRoll < 0) { //correct roll inbalance
r1 = (roll - imuRoll) * 5 * -1;
r2 = (roll - imuRoll) * 5;
}
else {
r1 = 0;
r2 = 0;
}
yaw = 0;
throttle = 350;
}
input1 = 1100 + throttle + yaw + p1 + r2;
input2 = 1100 + throttle - yaw + p2 + r2;
input3 = 1100 + throttle + yaw + p2 + r1;
input4 = 1100 + throttle - yaw + p1 + r1;
greatest = input1;
least = input1;
if(greatest < input2) {
greatest = input2;
}
if(least > input2) {
least = input2;
}
if(greatest < input3) {
greatest = input3;
}
if(least > input3) {
least = input3;
}
if(greatest < input4) {
greatest = input4;
}
if(least > input4) {
least = input4;
}
if(greatest > 2000 && least > 1100) {
input1 = map(input1, 1100, greatest, 1100, 2000);
input2 = map(input2, 1100, greatest, 1100, 2000);
input3 = map(input3, 1100, greatest, 1100, 2000);
input4 = map(input4, 1100, greatest, 1100, 2000);
}
else if(least < 1100 && greatest < 2000) {
input1 = map(input1, least, 2000, 1100, 2000);
input2 = map(input2, least, 2000, 1100, 2000);
input3 = map(input3, least, 2000, 1100, 2000);
input4 = map(input4, least, 2000, 1100, 2000);
}
else if(greatest > 2000 && least < 1100) {
input1 = map(input1, least, greatest, 1100, 2000);
input2 = map(input2, least, greatest, 1100, 2000);
input3 = map(input3, least, greatest, 1100, 2000);
input4 = map(input4, least, greatest, 1100, 2000);
}
else{
}
}
Serial.print(input1);
Serial.print(", ");
Serial.print(input2);
Serial.print(", ");
Serial.print(input3);
Serial.print(", ");
Serial.println(input4);
//Serial.println(throttle);
esc1.writeMicroseconds(input1); //spin up the motors to the correct speed
esc2.writeMicroseconds(input2);
esc3.writeMicroseconds(input3);
esc4.writeMicroseconds(input4);
//esc5.writeMicroseconds(input2);
}
float printAttitude(float ax, float ay, float az)
{
float pitch = atan2(-ax, sqrt(ay * ay + az * az));
pitch *= 180.0 / PI;
return pitch;
}
