Cole Purtzer
Published © GPL3+

Advanced Rocket Flight Computer

This will keep your rocket upright and on course!

AdvancedWork in progress37,660
Advanced Rocket Flight Computer

Things used in this project

Hardware components

Teensy 3.6
Teensy 3.6
×1
Arduino UNO
Arduino UNO
×1

Software apps and online services

Arduino IDE
Arduino IDE

Hand tools and fabrication machines

Soldering iron (generic)
Soldering iron (generic)
Solder Flux, Soldering
Solder Flux, Soldering
Soldering Gun Kit, Instant Heat
Soldering Gun Kit, Instant Heat

Story

Read more

Custom parts and enclosures

gerber_boardoutline_nfeZPaERQ1.GKO

gerber_topsilklayer_B65K2qOteS.GTO

gerber_bottomlayer_Mnp3NuN8aY.GBL

gerber_bottompastemasklayer_vM8SdpWS8r.GBP

gerber_bottomsilklayer_qmijTbiPer.GBO

gerber_bottomsoldermasklayer_Da65enU9fY.GBS

gerber_drill_npth_Z1JX8U4DOj.DRL

gerber_drill_pth_N77NwzoHLQ.DRL

gerber_toplayer_Dk145N7EUm.GTL

gerber_toppastemasklayer_P1YAtQ6sp3.GTP

gerber_topsoldermasklayer_4As8yQ2tmM.GTS

omega_flight_computer_mount_v2_v6_0bl8Rpgbwe.stl

Schematics

omega_v4_dev_iH8wTGMZul.png

Code

OmegaSoft-1.052.ino

C/C++
// 2020 Delta Space Systems //
 float PIDX, PIDY, errorX, errorY, previous_errorX, previous_errorY, pwmX, pwmY;
#include <Wire.h>
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
#include <Servo.h>
#define RESTRICT_PITCH // Comment out to restrict roll to 90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
Servo servoY;
Servo servoX;
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int desired_angleX = 193;//servoY
int desired_angleY = -18; // servoX
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
int ledblu=2;
int ledgrn=5;
int ledred=6;
int buzzer=21;
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
float pidX_p=0;
float pidX_i=0;
float pidX_d=0;
float pidY_p=0;
float pidY_d=0;
/////////////////PID CONSTANTS/////////////////
double kp=0.16;//3.55
double ki=0;//2.05
double kd=0.045;//2.05
///////////////////////////////////////////////
int State = 0;
void setup() {
  pinMode(ledblu, OUTPUT);
  pinMode(ledgrn, OUTPUT);
  pinMode(ledred, OUTPUT);
  pinMode(buzzer, OUTPUT);
 
  servoY.attach(30);
   servoX.attach(29);
  Serial.begin(115200);
  Wire.begin();
#if ARDUINO >= 157
  Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
  TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to 250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to 2g
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }
  delay(100); // Wait for sensor to stabilize
  while (i2cRead(0x3B, i2cData, 6));
  accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);

#ifdef RESTRICT_PITCH // Eq. 25 and 26
  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
  kalmanX.setAngle(roll); // Set starting angle
  kalmanY.setAngle(pitch);
  gyroXangle = roll;
  gyroYangle = pitch;
 
  timer = micros();
  digitalWrite(ledgrn, HIGH);
  servoX.write(93);
  servoY.write(103);
  delay(1000);
  digitalWrite(ledgrn, LOW);
  digitalWrite(buzzer, HIGH);
  digitalWrite(ledred, HIGH);
  servoX.write(107);
  delay(200);
  digitalWrite(buzzer, LOW);
  servoY.write(123);
  delay(200);
  digitalWrite(ledred, LOW);
  digitalWrite(buzzer, HIGH);
  digitalWrite(ledblu, HIGH);
  servoX.write(93);
  delay(200);
  digitalWrite(buzzer, LOW);
  servoY.write(103);
  delay(200);
  digitalWrite(ledblu, LOW);
  digitalWrite(buzzer, HIGH);
  digitalWrite(ledgrn, HIGH);
  servoX.write(80);
  delay(200);
  digitalWrite(buzzer, LOW);
  
  servoY.write(83);
  delay(200);
  servoX.write(93);
  delay(200);
  servoY.write(103);
  delay(500);
}

void loop() {
 
  /* Update all the values */
  while (i2cRead(0x3B, i2cData, 14));
  accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
  gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
  gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
  gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;
  systemstateabort();
  pidcompute();
  
}
 void pidcompute () {
  double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
  timer = micros();
#ifdef RESTRICT_PITCH // Eq. 25 and 26
  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

  double gyroXrate = gyroX / 131.0; // Convert to deg/s
  double gyroYrate = gyroY / 131.0; // Convert to deg/s

#ifdef RESTRICT_PITCH
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
    kalmanX.setAngle(roll);
    kalAngleX = roll;
    gyroXangle = roll;
  } else
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleX) > 90)
    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
    kalmanY.setAngle(pitch);
   
    kalAngleY = pitch;
    gyroYangle = pitch;
  } else
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleY) > 90)
    gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
  kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
 
pwmX = max(pwmX, (desired_angleX - 20)); 
pwmY = max(pwmY, (desired_angleY - 20)); 
pwmX = min(pwmX, (desired_angleX + 20)); 
pwmY = min(pwmY, (desired_angleY + 20)); 

errorX = kalAngleX - desired_angleX;
errorY = kalAngleY - desired_angleY;
pidX_p = kp*errorX;
pidX_d = kd*((errorX - previous_errorX)/dt);
pidY_p = kp*errorY;
pidY_d = kd*((errorY - previous_errorY)/dt);
pidX_i = ki*errorX*dt;
previous_errorX = errorX;
previous_errorY = errorY;
PIDX = pidX_p + pidX_i + pidX_d;
PIDY = pidY_p + pidY_d;
pwmX = -1*((PIDX * 5.8) + 90);
pwmY = ((PIDY * 5.8) + 90);
servowrite();
 }

void servowrite () {
 servoX.write(pwmX); 
 servoY.write(pwmY); 

}

 void systemstateabort() {
 if (kalAngleX > 40 || kalAngleY > 40){
  digitalWrite(ledgrn, LOW);
  digitalWrite(ledblu, HIGH);
  //digitalWrite(buzzer, HIGH);
  
  }
 }

I2C.ino

C/C++
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.

 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").

 Contact information
 -------------------

 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
 */

const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication

uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
  return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
}

uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data, length);
  uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
  if (rcode) {
    Serial.print(F("i2cWrite failed: "));
    Serial.println(rcode);
  }
  return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}

uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
  uint32_t timeOutTimer;
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
  if (rcode) {
    Serial.print(F("i2cRead failed: "));
    Serial.println(rcode);
    return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
  }
  Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
  for (uint8_t i = 0; i < nbytes; i++) {
    if (Wire.available())
      data[i] = Wire.read();
    else {
      timeOutTimer = micros();
      while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
      if (Wire.available())
        data[i] = Wire.read();
      else {
        Serial.println(F("i2cRead timeout"));
        return 5; // This error value is not already taken by endTransmission
      }
    }
  }
  return 0; // Success
} 

OmegaSoft-1.2.ino

C/C++
Yay, after about 2 weeks i finished up my new flagship software. It now features the full PID controller as apposed to just PD. Also it has a launch detection mode so it knows when launch has occurred, and it is using a raw gyro so no more nasty accelerometer readings!
/* Delta Space Systems
      Version 1.2
    May, 8th 2020*/

#include <SD.h>
#include <SPI.h>
#include <Servo.h>
#include<Wire.h>

//#include <KalmanFilter.h>
const int MPU=0x68; 
int16_t AcX,AcY,AcZ,Tmp,GyX,GyroY,GyroZ;
 double PIDX, PIDY, errorX, errorY, previous_errorX, previous_errorY, pwmX, pwmY, gyroXX, gyroYY;
 
int desired_angleX = 0;//servoY
int desired_angleY = 0;//servoX

int servoX_offset = 100;
int servoY_offset = 100;

int servoXstart = 80;
int servoYstart = 80;

float servo_gear_ratio = 5.8;


double accAngleX;
double accAngleY;
double yaw;
double GyroX;
double gyroAngleX;
double gyroAngleY;
double pitch;

int ledblu = 2;    // LED connected to digital pin 9
int ledgrn = 5;    // LED connected to digital pin 9
int ledred = 6;    // LED connected to digital pin 9
int mosfet = 24;

Servo servoX;
Servo servoY;

int pyro1 = 24;
int buzzer = 21;

double dt, currentTime, previousTime;

float pidX_p = 0;
float pidX_i = 0;
float pidX_d = 0;
float pidY_p = 0;
float pidY_i = 0;
float pidY_d = 0;

int pos;

double kp = 0.15;
double ki = 0.0;
double kd = 0.0;

int state = 0;
//KalmanFilter kalman(0.001, 0.003, 0.03);

void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); 
  Wire.write(0);    
  Wire.endTransmission(true);
  Serial.begin(9600);
  servoX.attach(29);
  servoY.attach(30);
  pinMode(mosfet, OUTPUT);
   pinMode(ledblu, OUTPUT);
  pinMode(ledgrn, OUTPUT);
  pinMode(ledred, OUTPUT);
  pinMode(buzzer, OUTPUT);
  pinMode(pyro1, OUTPUT);
 startup();

 
}
void loop() {
  

  previousTime = currentTime;        
  currentTime = millis();            
  dt = (currentTime - previousTime) / 1000; 
 
  accAngleX = (atan(AcY / sqrt(pow(AcX, 2) + pow(AcZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
  accAngleY = (atan(-1 * AcX / sqrt(pow(AcY, 2) + pow(AcZ, 2))) * 180 / PI) + 1.58; // 
 
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,12,true);
  AcX=Wire.read()<<8|Wire.read();    
  AcY=Wire.read()<<8|Wire.read();  
  AcZ=Wire.read()<<8|Wire.read();  
  GyroX=Wire.read()<<8|Wire.read();  
  GyroY=Wire.read()<<8|Wire.read();  
  GyroZ=Wire.read()<<8|Wire.read();  
  datadump();
  launchdetect();
  
}
void accel_degrees () {
  double prevgyroX = GyroZ;
  double prevgyroY = GyroY;
  
  //converting angular acceleration to degrees
  gyroAngleX +=  (((GyroZ + prevgyroX) / 2)* dt); // deg/s * s = deg
  gyroAngleY +=  (((GyroY + prevgyroY) / 2)* dt);
  
  //complimentary filter
  double OrientationX = 0.94 * gyroAngleX + 0.06 * accAngleX;
  double OrientationY = 0.94 * gyroAngleY + 0.06 * accAngleY;
  
  //divided by 32.8 as recommended by the datasheet
  pitch = OrientationX / 32.8;
  yaw = OrientationY / 32.8;
  pidcompute();
}

void servowrite() {

 servoX.write(pwmX);
 servoY.write(pwmY); 

}
void pidcompute () {
  
previous_errorX = errorX;
previous_errorY = errorY; 

errorX = pitch - desired_angleX;
errorY = yaw - desired_angleY;

//Defining "P" 
pidX_p = kp*errorX;
pidY_p = kp*errorY;

//Defining "D"
pidX_d = kd*((errorX - previous_errorX)/dt);
pidY_d = kd*((errorY - previous_errorY)/dt);

//Defining "I"
pidX_i = ki * (pidX_i + errorX * dt);
pidY_i = ki * (pidY_i + errorY * dt);

PIDX = pidX_p + pidX_i + pidX_d;
PIDY = pidY_p + pidY_i + pidY_d;

pwmX = ((PIDX * servo_gear_ratio) + servoX_offset);
pwmY = ((PIDY * servo_gear_ratio) + servoY_offset);
servowrite();

}
void startup () {
  digitalWrite(ledgrn, HIGH);
  servoX.write(servoXstart);
  servoY.write(servoYstart);
  delay(1000);
  digitalWrite(ledgrn, LOW);
  digitalWrite(buzzer, HIGH);
  digitalWrite(ledred, HIGH);
  servoX.write(servoXstart + 20);
  delay(200);
  digitalWrite(buzzer, LOW);
  servoY.write(servoYstart + 20);
  delay(200);
  digitalWrite(ledred, LOW);
  digitalWrite(buzzer, HIGH);
  digitalWrite(ledblu, HIGH);
  servoX.write(servoXstart);
  delay(200);
  digitalWrite(buzzer, LOW);
  servoY.write(servoYstart);
  delay(200);
  digitalWrite(ledblu, LOW);
  digitalWrite(buzzer, HIGH);
  digitalWrite(ledgrn, HIGH);
  servoX.write(servoXstart - 20);
  delay(200);
  digitalWrite(buzzer, LOW);
  
  servoY.write(servoYstart - 20);
  delay(200);
  servoX.write(servoXstart);
  delay(200);
  servoY.write(servoYstart);
  delay(500);
 }
void launchdetect () {
   if (AcX > 17000) {
  state = 1;
 }
 if (state == 1) {
  accel_degrees();
 }
}
void datadump () {
Serial.println(AcX);
Serial.print(AcZ);
}

Credits

Cole Purtzer

Cole Purtzer

16 projects • 210 followers
Hey I am Cole, the lead engineer at Delta Space Systems! Youtube Channel: https://www.youtube.com/channel/UC7Nhgj_PVCtroPXHMhdku-g

Comments