├── .gitattributes
├── .gitignore
├── LICENSE
├── README.md
├── TVC-Flight-Code
    ├── BMP.cpp
    ├── BMP.h
    ├── Buzzer.cpp
    ├── Buzzer.h
    ├── IMU.cpp
    ├── IMU.h
    ├── LED.cpp
    ├── LED.h
    ├── PID.cpp
    ├── PID.h
    ├── Rocket.cpp
    ├── Rocket.h
    ├── SDCard.cpp
    ├── SDCard.h
    ├── StateMachine.cpp
    ├── StateMachine.h
    ├── TVC-Flight-Code.ino
    ├── VectorDefinition.h
    ├── VoltageDivider.cpp
    └── VoltageDivider.h
└── flight.gif


/.gitattributes:
--------------------------------------------------------------------------------
1 | # Auto detect text files and perform LF normalization
2 | * text=auto
3 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | .DS_Store


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2022 Tom Kuttler
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | <div align="center">
 2 | 
 3 | # TVC-Rocket-Flight-Code
 4 | 
 5 | <a href="https://www.arduino.cc">
 6 |     <img alt="arduino" src="https://img.shields.io/badge/-Arduino-00979D?style=for-the-badge&logo=Arduino&logoColor=white" /></a>
 7 | <a href="https://www.iso.org/standard/74528.html">
 8 |     <img alt="C" src="https://img.shields.io/badge/c-%2300599C.svg?style=for-the-badge&logo=c&logoColor=white" /></a>
 9 | <a href="https://isocpp.org">
10 |     <img alt="C++" src="https://img.shields.io/badge/c++-%2300599C.svg?style=for-the-badge&logo=c%2B%2B&logoColor=white" /></a>
11 | <br>
12 | <br>
13 | 
14 | This repository contains the source code for a software implementation of thrust vector control (TVC) for model rockets. The software consists of a state machine and PID control loops to adjust the orientation of the rocket during its powered ascent.
15 | 
16 | <img alt="flight video" src="https://github.com/tomkuttler/TVC-Rocket-Flight-Code/blob/main/flight.gif" width="400" />
17 | 
18 | </div>
19 | 
20 | <details>
21 | <summary>Table of Contents</summary>
22 | 
23 | - [Installation](#installation)
24 | - [Usage](#usage)
25 | - [Features](#features)
26 |     - [State Machine](#state-machine)
27 |     - [PID Control Loop](#pid-control-loop)
28 |     - [Parachute Ejection](#parachute-ejection)
29 | - [Contributing](#contributing)
30 | - [License](#license)
31 | </details>
32 | 
33 | ## Installation
34 | 
35 | To use this software, you will need a Teensy 4.1 board and the Arduino IDE with the Teensy Loader Application. Follow these steps to install the software:
36 | 
37 | 1. Clone the repository to your local machine.
38 | 2. Open the `TVC-Flight-Code.ino` file in the Arduino IDE.
39 | 3. Upload the code to your Teensy.
40 | 
41 | ## Usage
42 | 
43 | To use the TVC system with this software, you'll need a flight controller and a motor mount which can be rotated with two servos. The parameters in the `Rocket.h` and `StateMachine.h` file need to be adjusted for your specific rocket. After evaluating the correct parameters upload the code to the Teensy. Do not forget to power on the flight controller before launching the rocket. The Software automatically detects the launch and the TVC system will automatically adjust the orientation of the rocket during its powered ascent based on the PID control loop. Note that it's important to follow all safety guidelines when testing or launching your rocket, and to always conduct tests in a safe and legal manner.
44 | 
45 | ## Features
46 | 
47 | ### State Machine
48 | 
49 | The state machine is controlling the model rocket based on the current state of the flight. The state machine has four states:
50 | 
51 | - `LAUNCH_PAD_IDLE`: The initialization state where the TVC system is initialized and the rocket is waiting for launch.
52 | - `ASCENT`: The ascent state where the TVC system controls the orientation of the rocket during its powered ascent.
53 | - `DESCENT`: The descent state where the TVC system is deactivated and the rocket is falling down.
54 | - `LANDED`: The landed state where the rocket is waiting for recovery.
55 | 
56 | ### PID Control Loop
57 | 
58 | The PID control loop is responsible for adjusting the orientation of the motor mount based on the current orientation of the rocket and the desired orientation. The TVC system uses two separate PID control loops, one for the orientation in the x-direction and one for the orientation in the y-direction.
59 | 
60 | Each PID control loop has three components:
61 | 
62 | - Proportional: The proportional component adjusts the orientation of the rocket based on the difference between the current orientation and the desired orientation.
63 | - Integral: The integral component adjusts the orientation of the rocket based on the cumulative error between the current orientation and the desired orientation.
64 | - Derivative: The derivative component adjusts the orientation of the rocket based on the rate of change of the error between the current orientation and the desired orientation.
65 | 
66 | By using two separate PID control loops, the TVC system is able to independently adjust the orientation of the rocket in both the x-direction and the y-direction, by using two servos which gimbal the motor mount.
67 | 
68 | ### Parachute Ejection
69 | 
70 | While the TVC system controls the orientation of the rocket during ascent, a separate mechanical parachute ejection system is used to deploy the parachute. This system consists of springs and a servo that is triggered by a signal from the state machine when the rocket switches from the ascent state to the descent state. By ejecting the parachute during the switch from ascent to descent, the rocket is able to safely decelerate and return to the ground without sustaining damage.
71 | 
72 | ## Contributing
73 | 
74 | If you would like to contribute to this project, please follow these steps:
75 | 
76 | 1. Fork the repository to your own account.
77 | 2. Create a new branch with your changes.
78 | 3. Submit a pull request with your changes.
79 | 
80 | ## License
81 | 
82 | This project is licensed under the [MIT License](https://opensource.org/licenses/MIT). You are free to use, modify, and distribute this software for both personal and commercial purposes as long as you give attribution to the original author. For more information about the license, please see the `LICENSE` file.
83 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/BMP.cpp:
--------------------------------------------------------------------------------
 1 | #include "BMP.h"
 2 | 
 3 | BMP::BMP() {
 4 |   if (!bmp.begin()) {
 5 |     Serial.println("ERROR: BMP280 initialisation failed!");
 6 |   }
 7 | 
 8 |   // Default settings from datasheet https://cdn-shop.adafruit.com/datasheets/BST-BMP280-DS001-11.pdf
 9 |   bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,     /* Operating Mode. */
10 |                   Adafruit_BMP280::SAMPLING_X2,     /* Temp. oversampling */
11 |                   Adafruit_BMP280::SAMPLING_X16,    /* Pressure oversampling */
12 |                   Adafruit_BMP280::FILTER_X16,      /* Filtering. */
13 |                   Adafruit_BMP280::STANDBY_MS_500); /* Standby time. */
14 | }
15 | 
16 | float BMP::readTemperature() {
17 |   return bmp.readTemperature();
18 | }
19 | 
20 | float BMP::readPressure() {
21 |   return bmp.readPressure();
22 | }
23 | 
24 | float BMP::readAltitude(float seaLevelhPa) {
25 |   return bmp.readAltitude(seaLevelhPa);
26 | }
27 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/BMP.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Barometric Pressure & Altitude Sensor BMP280
 3 |  * https://www.adafruit.com/product/2651
 4 |  * 
 5 |  * Outputs the following data: temperature, athmospheric pressure and altitude (which is calculated with the pressure at sea level)
 6 |  */
 7 | 
 8 | #include <Adafruit_BMP280.h>
 9 | #include <Wire.h>
10 | 
11 | class BMP {
12 |   public:
13 |     BMP();
14 |     
15 |     float readTemperature();
16 |     float readPressure();
17 |     float readAltitude(float seaLevelhPa);
18 | 
19 |   private:
20 |     Adafruit_BMP280 bmp;
21 | };
22 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/Buzzer.cpp:
--------------------------------------------------------------------------------
 1 | #include "Buzzer.h"
 2 | 
 3 | Buzzer::Buzzer() {
 4 |   
 5 | }
 6 | 
 7 | void Buzzer::noSDCardAlert() {
 8 |   int slide = 5000;
 9 |   for (int i = 0; i <= 120; i++) {
10 |     tone(BUZZER_PIN, slide); delay(10); noTone(BUZZER_PIN);
11 |     slide = slide - 40;
12 |   }
13 |   noTone(BUZZER_PIN);
14 | }
15 | 
16 | void Buzzer::tooLowOnBoardVoltageAlert() {
17 |   for(int i = 0; i < 1000; i++) {    
18 |     tone(BUZZER_PIN, 2000);
19 |     delayMicroseconds(i);
20 |     noTone(BUZZER_PIN);
21 |     delayMicroseconds(i);    
22 |   }
23 |   noTone(BUZZER_PIN);
24 | }
25 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/Buzzer.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Piezo buzzer
 3 |  * 
 4 |  * Plays an alarm if no SD-Card is inserted into the Flight Controller or the on-board voltage is too low
 5 |  */
 6 | 
 7 | #include <Arduino.h>
 8 | 
 9 | #define BUZZER_PIN 36
10 | 
11 | class Buzzer {
12 |   public:
13 |     Buzzer();
14 | 
15 |     void noSDCardAlert();
16 |     void tooLowOnBoardVoltageAlert();
17 | };
18 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/IMU.cpp:
--------------------------------------------------------------------------------
 1 | #include "IMU.h"
 2 | 
 3 | IMU::IMU() {
 4 |   Wire.begin();
 5 |   Wire.setClock(400000);
 6 |   if (!bno.begin()) {
 7 |     Serial.println("ERROR: BNO055 initialisation failed!");
 8 |   }
 9 | }
10 | 
11 | Vector3 IMU::getGyros() {
12 |   bno.getEvent(&angVelocityData, Adafruit_BNO055::VECTOR_GYROSCOPE);
13 |   return angVelocityData.gyro;
14 | }
15 | 
16 | Vector3 IMU::getAcceleration() {
17 |   bno.getEvent(&linearAccelerationData, Adafruit_BNO055::VECTOR_LINEARACCEL);
18 |   Vector3 Vector(linearAccelerationData.acceleration.x, linearAccelerationData.acceleration.y, linearAccelerationData.acceleration.z);
19 |   return Vector;
20 | }
21 | 
22 | Vector3 IMU::getOrientation() {
23 |   bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
24 |   return orientationData.orientation;
25 | }
26 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/IMU.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Absolute Orientation Sensor BNO055
 3 |  * https://www.adafruit.com/product/4646
 4 |  * 
 5 |  * IMU = inertial measurement unit
 6 |  * 
 7 |  * Outputs the following data: angular velocity, acceleration and absolute orientation
 8 |  */
 9 | 
10 | #include <Adafruit_BNO055.h>
11 | #include <Adafruit_Sensor.h>
12 | #include <Wire.h>
13 | #include <Arduino.h>
14 | #include "VectorDefinition.h"
15 | 
16 | class IMU {
17 |   public:
18 |     IMU();
19 |     Vector3 getGyros();
20 |     Vector3 getAcceleration();
21 |     Vector3 getOrientation();
22 | 
23 |   private:
24 |     Adafruit_BNO055 bno = Adafruit_BNO055(-1, 0x28, &Wire1);
25 |     sensors_event_t angVelocityData;
26 |     sensors_event_t linearAccelerationData;
27 |     sensors_event_t orientationData;
28 | };
29 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/LED.cpp:
--------------------------------------------------------------------------------
 1 | #include "LED.h"
 2 | 
 3 | LED::LED() {
 4 |   // Set system led pins as outputs
 5 |   pinMode(R_LED, OUTPUT);
 6 |   pinMode(G_LED, OUTPUT);
 7 |   pinMode(B_LED, OUTPUT);
 8 | 
 9 |   // The computer is using a common anode LED -> write LOW to turn it on, and HIGH to turn it off
10 |   digitalWrite(R_LED, HIGH);
11 |   digitalWrite(G_LED, HIGH);
12 |   digitalWrite(B_LED, HIGH);
13 | }
14 | 
15 | void LED::red() {
16 |   digitalWrite(R_LED, LOW);
17 |   digitalWrite(G_LED, HIGH);
18 |   digitalWrite(B_LED, HIGH);
19 | }
20 | 
21 | void LED::green() {
22 |   digitalWrite(R_LED, HIGH);
23 |   digitalWrite(G_LED, LOW);
24 |   digitalWrite(B_LED, HIGH);
25 | }
26 | 
27 | void LED::blue() {
28 |   digitalWrite(R_LED, HIGH);
29 |   digitalWrite(G_LED, HIGH);
30 |   digitalWrite(B_LED, LOW);
31 | }
32 | 
33 | void LED::purple() {
34 |   digitalWrite(R_LED, LOW);
35 |   digitalWrite(G_LED, HIGH);
36 |   digitalWrite(B_LED, LOW);
37 | }
38 | 
39 | void LED::off() {
40 |   digitalWrite(R_LED, HIGH);
41 |   digitalWrite(G_LED, HIGH);
42 |   digitalWrite(B_LED, HIGH);
43 | }
44 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/LED.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Common Anode LED
 3 |  * 
 4 |  * Changes color according to the state the Flight Controller is currently in
 5 |  * 
 6 |  * STATE           | COLOR
 7 |  * ----------------|--------
 8 |  * LAUNCH_PAD_IDLE | red
 9 |  * ASCENT          | green
10 |  * DESCENT         | blue
11 |  * LANDED          | purple
12 |  */
13 | 
14 | #include <Arduino.h>
15 | 
16 | #define R_LED 41
17 | #define G_LED 40
18 | #define B_LED 39
19 | 
20 | class LED {
21 |   public:
22 |     LED();
23 |     void red();
24 |     void green();
25 |     void blue();
26 |     void purple();
27 |     void off();     
28 | };
29 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/PID.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * PID Controller, inspired by https://gist.github.com/bradley219/5373998
 3 |  */
 4 | 
 5 | #include "PID.h"
 6 | 
 7 | PID::PID(float Kp, float Ki, float Kd, float saturation) {
 8 |   this->Kp = Kp;
 9 |   this->Ki = Ki;
10 |   this->Kd = Kd;
11 |   this->saturation = saturation;
12 | }
13 | 
14 | float PID::update(float input) {
15 |   // Calculate delata time
16 |   currentTime = micros();
17 |   deltaTime = (currentTime - previousTime) / 1000000.0f;
18 | 
19 |   // Calculate error
20 |   error = setpoint - input;
21 | 
22 |   // Integral term
23 |   errorIntegral += error * deltaTime;
24 | 
25 |   // Derivative term
26 |   errorDerivative = (error - errorLastCycle) / deltaTime;
27 | 
28 |   // Calculate total output
29 |   output = Kp * error + Ki * errorIntegral + Kd * errorDerivative;
30 | 
31 |   // Save current error and time for next cycle
32 |   errorLastCycle = error;
33 |   previousTime = currentTime;
34 | 
35 |   // Check if output is in saturation
36 |   if (output > saturation) {
37 |     output = saturation;
38 |   } else if (output < -saturation) {
39 |     output = -saturation;
40 |   }
41 | 
42 |   return output;
43 | }
44 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/PID.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * PID-Controller
 3 |  * 
 4 |  * The orientation of the motor mount is calculated with two separate PID-Controllers
 5 |  */
 6 | 
 7 | #include <Wire.h>
 8 | #include <Arduino.h>
 9 | 
10 | class PID {
11 |   public:
12 |     PID(float Kp, float Ki, float Kd, float saturation);
13 |     float update(float input);
14 | 
15 |   private:
16 |     // Proportional gain
17 |     float Kp = 0.0f;
18 |     // Integral gain
19 |     float Ki = 0.0f;
20 |     // Derivative gain
21 |     float Kd = 0.0f;
22 | 
23 |     // Loop interval time
24 |     float deltaTime = 0.0f;
25 |     float error = 0.0f;
26 |     float setpoint = 0.0f;
27 |     float errorIntegral = 0.0f;
28 |     float errorDerivative = 0.0f;
29 |     float errorLastCycle = 0.0f;
30 | 
31 |     // Maximum value of manipulated variable (absolute value)
32 |     float saturation = 0.0f;
33 |     float output = 0.0f;
34 | 
35 |     unsigned long currentTime = 0.0;
36 |     unsigned long previousTime = 0.0;
37 | };
38 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/Rocket.cpp:
--------------------------------------------------------------------------------
  1 | #include "Rocket.h"
  2 | 
  3 | Rocket::Rocket() {
  4 |   // Attach and initialize servos
  5 |   yServo.attach(Y_SERVO_PIN);
  6 |   zServo.attach(Z_SERVO_PIN);
  7 |   parachuteServo.attach(PARACHUTE_SERVO_PIN);
  8 | 
  9 |   yServo.write(Y_SERVO_MIDDLE);
 10 |   zServo.write(Z_SERVO_MIDDLE);
 11 |   parachuteServo.write(PARACHUTE_CLOSED);
 12 | 
 13 |   // Check on-board voltage
 14 |   if(voltageDivider.getBoardVoltage() < MIN_VOLTAGE) {
 15 |     Serial.println("Error: on-board voltage too low");
 16 |     while (true) {
 17 |       // on-board voltage too low, so do not do anything more - stay stuck here and play alarm
 18 |       sdCard.buzzer.tooLowOnBoardVoltageAlert();
 19 |       
 20 |       sdCard.led.red();
 21 |       delay(500);
 22 |       sdCard.led.off();
 23 |     }
 24 |   }
 25 | }
 26 | 
 27 | void Rocket::padIdle() {
 28 |   // Update acceleration for lift off check
 29 |   acceleration = imu.getAcceleration();
 30 | }
 31 | 
 32 | void Rocket::ascent() {
 33 |   // Calculate flight time in s
 34 |   flightTime = (millis() - flightStartTime) / 1000.0f;
 35 | 
 36 |   // ----- TVC ALGORITHM -----
 37 |   // Calculate delta time
 38 |   currentTime = micros();
 39 |   deltaTime = (currentTime - previousTime) / 1000000.0f;
 40 | 
 41 |   // Update gyro values
 42 |   gyros = imu.getGyros();
 43 | 
 44 |   // Integrate angular velocity for relative orientation (roll, pitch and yaw) in rad
 45 |   relativeOrientation += gyros * deltaTime;
 46 | 
 47 |   // Send the yaw and pitch to their respective PID Controller and recieve output
 48 |   float yPIDoutput = yPID.update(relativeOrientation.y);
 49 |   float zPIDoutput = zPID.update(relativeOrientation.z);
 50 | 
 51 |   // Convert the relative orientation in rad to deg
 52 |   yPIDoutput = yPIDoutput * RAD2DEG;
 53 |   zPIDoutput = zPIDoutput * RAD2DEG;
 54 | 
 55 |   // Convert the PID output (orientation of motor mount) to servo position (relative orientation of servo horn)
 56 |   float yServoRelPosition = yPIDoutput * SERVO_RATIO;
 57 |   float zServoRelPosition = zPIDoutput * SERVO_RATIO;
 58 | 
 59 |   // Check if the servo relative positions are greater than the max motor mount rotation
 60 |   // The motor mount can only be rotated 10 deg => the servos are allowed to rotate 30 deg
 61 |   if(yServoRelPosition > MAX_SERVO_ROTATION) {
 62 |     yServoRelPosition = MAX_SERVO_ROTATION;
 63 |   }
 64 |   else if(yServoRelPosition < -MAX_SERVO_ROTATION) {
 65 |     yServoRelPosition = -MAX_SERVO_ROTATION;
 66 |   }
 67 | 
 68 |   if(zServoRelPosition > MAX_SERVO_ROTATION) {
 69 |     zServoRelPosition = MAX_SERVO_ROTATION;
 70 |   }
 71 |   else if(zServoRelPosition < -MAX_SERVO_ROTATION) {
 72 |     zServoRelPosition = -MAX_SERVO_ROTATION;
 73 |   }
 74 | 
 75 |   // Set the position of the servos
 76 |   // To get the absolute position of the servo horns, the middle servo horn position is added to the relative orientation
 77 |   yServo.write(yServoRelPosition + Y_SERVO_MIDDLE);
 78 |   zServo.write(zServoRelPosition + Z_SERVO_MIDDLE);
 79 | 
 80 |   // Save current time for next cycle
 81 |   previousTime = currentTime;
 82 | 
 83 |   // ----- DATA LOGGING -----
 84 |   acceleration = imu.getAcceleration();
 85 |   orientation = imu.getOrientation();
 86 | 
 87 |   float temperature = bmp.readTemperature();
 88 |   float pressure = bmp.readPressure();
 89 |   float altitude = bmp.readAltitude(SEA_LEVEL_PRESSURE);
 90 | 
 91 |   float voltage = voltageDivider.getBoardVoltage();
 92 |   
 93 |   sdCard.logData(flightTime, gyros.x, gyros.y, gyros.z, acceleration.x, acceleration.y, acceleration.z, orientation.x, orientation.y, orientation.z, temperature, pressure, altitude, yServoRelPosition + Y_SERVO_MIDDLE, zServoRelPosition + Z_SERVO_MIDDLE, voltage, "ASCENT");
 94 | }
 95 | 
 96 | void Rocket::maxApogee() {
 97 |   // Eject Parachute
 98 |   parachuteServo.write(PARACHUTE_EJECT);
 99 | 
100 |   // Set tvc servos to middle position
101 |   yServo.write(Y_SERVO_MIDDLE);
102 |   zServo.write(Z_SERVO_MIDDLE);
103 | }
104 | 
105 | void Rocket::descent() {
106 |   // Calculate flight time in s
107 |   flightTime = (millis() - flightStartTime) / 1000.0f;
108 | 
109 |   // ----- DATA LOGGING -----
110 |   acceleration = imu.getAcceleration();
111 |   orientation = imu.getOrientation();
112 | 
113 |   float temperature = bmp.readTemperature();
114 |   float pressure = bmp.readPressure();
115 |   float altitude = bmp.readAltitude(SEA_LEVEL_PRESSURE);
116 | 
117 |   float voltage = voltageDivider.getBoardVoltage();
118 |   
119 |   sdCard.logData(flightTime, gyros.x, gyros.y, gyros.z, acceleration.x, acceleration.y, acceleration.z, orientation.x, orientation.y, orientation.z, temperature, pressure, altitude, Y_SERVO_MIDDLE, Z_SERVO_MIDDLE, voltage, "DESCENT");
120 | 
121 | }
122 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/Rocket.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Rocket code file
 3 |  * 
 4 |  * Contains code that will be run in a specific state
 5 |  * Functions are called in StateMachine.cpp
 6 |  * Header file contains all constants that are specific to the model rocket
 7 |  */
 8 | 
 9 | #include <Wire.h>
10 | #include <Arduino.h>
11 | #include <Servo.h>
12 | #include "IMU.h"
13 | #include "BMP.h"
14 | #include "PID.h"
15 | #include "SDCard.h"
16 | #include "VoltageDivider.h"
17 | 
18 | #define Y_SERVO_PIN 3               // Y Servo (y axis labeled on IMU) = Y Servo (labeled on pcb)
19 | #define Z_SERVO_PIN 2               // Z Servo (z axis labeled on IMU) = X Servo (labeled on pcb)
20 | 
21 | #define Y_SERVO_MIDDLE 78           // Y Servo horn vertical position (motor mount is vertical)
22 | #define Z_SERVO_MIDDLE 87           // Z Servo horn vertical position (motor mount is vertical)
23 | 
24 | // Servo ratio is calculated by making multiple images of different servo horn angles and comparing the orientation of the horn and the motor mount
25 | #define SERVO_RATIO 3.1             // Servo horn to motor mount ratio (if horn is turned 1 deg, motor mount will turn 3 deg)
26 | #define MAX_SERVO_ROTATION 30       // Motor mount can only be rotated 10 deg => the servos are allowed to rotate 30 deg
27 | 
28 | #define PARACHUTE_SERVO_PIN 5       // Parachute Servo = Servo 3 (labeled on pcb)
29 | #define PARACHUTE_CLOSED 0          // Position of the parachute servo before ejection (in deg)
30 | #define PARACHUTE_EJECT 175         // Position of the parachute servo after ejection (in deg)
31 | 
32 | #define DEG2RAD 0.01745329251f      // Convert degrees to radians by multiplying with this number
33 | #define RAD2DEG 57.2957795131f      // Convert radians to degrees by multiplying with this number
34 | 
35 | #define MIN_VOLTAGE 4               // Minimum board voltage to complete startup (in V)
36 | 
37 | #define SEA_LEVEL_PRESSURE 1013.25  // Standard atmospheric pressure at sea level (in hPa)
38 | 
39 | class Rocket {
40 |   public:
41 |     Rocket();
42 | 
43 |     SDCard sdCard;
44 | 
45 |     // Angular Velocity Vector
46 |     // Three axis of rotation speed in radians per second (rad/s)
47 |     Vector3 gyros;
48 | 
49 |     // Acceleration Vector
50 |     // Three axis of acceleration (gravity + linear motion) in m/s^2
51 |     Vector3 acceleration;
52 | 
53 |     // Absolute Orientation Vector
54 |     // Three axis orientation data based on a 360° sphere (Euler Vector in deg)
55 |     Vector3 orientation;
56 | 
57 |     // Relative Orientation Vector
58 |     // Three axis orientation data (roll, pitch and yaw) in rad
59 |     Vector3 relativeOrientation;
60 | 
61 |     float flightTime = 0.0f; // in s
62 |     float flightStartTime = 0.0f; // in ms
63 |     
64 |     void padIdle();
65 |     void ascent();
66 |     void maxApogee();
67 |     void descent();
68 |     void landed();
69 | 
70 |   private:
71 |     IMU imu;
72 |     BMP bmp;
73 |     VoltageDivider voltageDivider;
74 |     
75 |     // PID Values were tuned with a Simulink simulation and verified with AeroVECTOR https://github.com/GuidodiPasquo/AeroVECTOR
76 |     // PID-Controller for calculation of motor mount orientation on y-axis
77 |     PID yPID = PID(0.6f, 0.05f, 0.125f, 10.0f);
78 |     // PID-Controller for calculation of motor mount orientation on z-axis
79 |     PID zPID = PID(0.6f, 0.05f, 0.125f, 10.0f);
80 | 
81 |     Servo yServo;
82 |     Servo zServo;
83 |     Servo parachuteServo;
84 |     
85 |     unsigned long currentTime = 0.0;
86 |     unsigned long previousTime = 0.0;
87 |     float deltaTime = 0.0f;
88 | };
89 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/SDCard.cpp:
--------------------------------------------------------------------------------
 1 | #include "SDCard.h"
 2 | 
 3 | SDCard::SDCard() {
 4 |   if(!SD.begin(chipSelect)) {
 5 |     Serial.println("Error: The SD card failed to initialize, or is not present!");
 6 |     while (true) {
 7 |       // No SD card, so do not do anything more - stay stuck here and play alarm
 8 |       buzzer.noSDCardAlert();
 9 |       
10 |       led.red();
11 |       delay(1000);
12 |       led.off();
13 |     }
14 |   }
15 | 
16 |   // Create file (or open it if it already exists)
17 |   File dataFile = SD.open("datalog.CSV", FILE_WRITE);
18 | 
19 |   // Write top description line
20 |   dataFile.println("flightTime, gyros.x, gyros.y, gyros.z, acceleration.x, acceleration.y, acceleration.z, orientation.x, orientation.y, orientation.z, temperature, pressure, altitude, yServo orientation, zServo orientation, voltage, state");
21 |   dataFile.close();
22 | }
23 | 
24 | void SDCard::logData(float flightTime, float gyroX, float gyroY, float gyroZ, float accelerationX, float accelerationY, float accelerationZ, float orientationX, float orientationY, float orientationZ, float temperature, float pressure, float altitude, float yServo, float zServo, float voltage, String state) {
25 |   // Open the data logging file
26 |   File dataFile = SD.open("datalog.CSV", FILE_WRITE);
27 | 
28 |   // If the file is available, write to it
29 |   if (dataFile) {
30 |     dataFile.print(flightTime);
31 |     dataFile.print(",");
32 |     dataFile.print(gyroX);
33 |     dataFile.print(",");
34 |     dataFile.print(gyroY);
35 |     dataFile.print(",");
36 |     dataFile.print(gyroZ);
37 |     dataFile.print(",");
38 |     dataFile.print(accelerationX);
39 |     dataFile.print(",");
40 |     dataFile.print(accelerationY);
41 |     dataFile.print(",");
42 |     dataFile.print(accelerationZ);
43 |     dataFile.print(",");
44 |     dataFile.print(orientationX);
45 |     dataFile.print(",");
46 |     dataFile.print(orientationY);
47 |     dataFile.print(",");
48 |     dataFile.print(orientationZ);
49 |     dataFile.print(",");
50 |     dataFile.print(temperature);
51 |     dataFile.print(",");
52 |     dataFile.print(pressure);
53 |     dataFile.print(",");
54 |     dataFile.print(altitude);
55 |     dataFile.print(",");
56 |     dataFile.print(yServo);
57 |     dataFile.print(",");
58 |     dataFile.print(zServo);
59 |     dataFile.print(",");
60 |     dataFile.print(voltage);
61 |     dataFile.print(",");
62 |     dataFile.println(state);
63 |     
64 |     dataFile.close();
65 |   } else {
66 |     // If the file can not be opened, pop up an error message
67 |     Serial.println("Error: Can not open datalog.txt");
68 |   }
69 | }
70 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/SDCard.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * SD-Card
 3 |  * 
 4 |  * During the ascent and descent states important flight data is saved on the SD-Card
 5 |  */
 6 | 
 7 | #include <SD.h>
 8 | #include <SPI.h>
 9 | #include "LED.h"
10 | #include "Buzzer.h"
11 | 
12 | class SDCard {
13 |   public:
14 |     SDCard();
15 | 
16 |     LED led;
17 |     Buzzer buzzer;
18 |     
19 |     void logData(float flightTime, float gyroX, float gyroY, float gyroZ, float accelerationX, float accelerationY, float accelerationZ, float orientationX, float orientationY, float orientationZ, float temperature, float pressure, float altitude, float yServo, float zServo, float voltage, String state);
20 |     
21 |   private:
22 |     // Use the built in SD-Card slot of the Teensy 4.1
23 |     const int chipSelect = BUILTIN_SDCARD;
24 | };
25 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/StateMachine.cpp:
--------------------------------------------------------------------------------
 1 | #include "StateMachine.h"
 2 | 
 3 | StateMachine::StateMachine() {
 4 |   activeState = LAUNCH_PAD_IDLE; // Default state
 5 | 
 6 |   rocket.sdCard.led.red();
 7 | }
 8 | 
 9 | void StateMachine::stateMachineLoop() {
10 |   switch (activeState) {
11 |     case LAUNCH_PAD_IDLE:
12 |       rocket.padIdle();
13 |       // Detect lift off -> Ascent flight
14 |       if (liftOffCheck()) {
15 |         // Save the time of lift off
16 |         rocket.flightStartTime = millis();
17 |         activeState = ASCENT;
18 |         rocket.sdCard.led.green();
19 |       }
20 |       break;
21 |     case ASCENT:
22 |       // TVC active
23 |       // Log Data
24 |       rocket.ascent();
25 |       // Detect max apogee -> Descent flight and eject Parachute
26 |       if (maxApogeeCheck()) {
27 |         rocket.maxApogee();
28 |         activeState = DESCENT;
29 |         rocket.sdCard.led.blue();
30 |       }
31 |       break;
32 |     case DESCENT:
33 |       rocket.descent();
34 |       // Log Data
35 |       // Detect landing
36 |       if (landedCheck()) {
37 |         activeState = LANDED;
38 |         rocket.sdCard.led.purple();
39 |       }
40 |       break;
41 |     case LANDED:
42 |       // Do nothing
43 |       break;
44 |   }
45 | }
46 | 
47 | // Check if rocket has launched by looking at the acceleration on vertical axis (x-axis)
48 | bool StateMachine::liftOffCheck() {
49 |   if (rocket.acceleration.x < LIFT_THRESHOLD) {
50 |     return true;
51 |   }
52 |   return false;
53 | }
54 | 
55 | // Check if rocket has reached apogee by looking at the time it takes to reach apogee
56 | bool StateMachine::maxApogeeCheck() {
57 |   if (rocket.flightTime > TIME_TO_APOGEE) {
58 |     return true;
59 |   }
60 |   return false;
61 | }
62 | 
63 | // Check if rocket has landed by looking at the time it takes to land
64 | bool StateMachine::landedCheck() {
65 |   if (rocket.flightTime > TIME_TO_LANDING) {
66 |     return true;
67 |   }
68 |   return false;
69 | }
70 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/StateMachine.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * State Machine
 3 |  * 
 4 |  * The code is divided into four states:
 5 |  * 
 6 |  * LAUNCH_PAD_IDLE
 7 |  *   Check the acceleration of the rocket on vertical axis
 8 |  * ASCENT
 9 |  *   Thrust vector controle is active
10 |  *   Flight data is logged to SD-Card    ‾\
11 |  * DESCENT                                 | Eject parachute
12 |  *   Flight data is logged to SD-Card   <-/
13 |  * LANDED
14 |  *   No active code (computer waiting to be turned off)
15 |  */
16 | 
17 | #include <Arduino.h>
18 | #include <Wire.h>
19 | #include "Rocket.h"
20 | 
21 | #define LIFT_THRESHOLD -2   // Acceleration on vertical axis/x-axis (negative value = acceleration upwards) (in m/s^2)
22 | 
23 | // Calculated with a flight simulation in AeroVECTOR https://github.com/GuidodiPasquo/AeroVECTOR
24 | #define TIME_TO_APOGEE 4     // Time from start until rocket reaches max apogee and parachute will be ejected (in s)
25 | #define TIME_TO_LANDING 30   // Time from start until rocket lands (in s)
26 | 
27 | enum State {
28 |   LAUNCH_PAD_IDLE,
29 |   ASCENT,
30 |   DESCENT,
31 |   LANDED
32 | };
33 | 
34 | class StateMachine {
35 |   public:
36 |     StateMachine();
37 |     void stateMachineLoop();
38 |     bool liftOffCheck();
39 |     bool maxApogeeCheck();
40 |     bool landedCheck();
41 | 
42 |   private:
43 |     Rocket rocket;
44 |     State activeState;
45 | };
46 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/TVC-Flight-Code.ino:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * TVC-Flight-Code
 3 |  * 
 4 |  * This is the source code for a software implementation of thrust vector control (TVC) for model rockets. 
 5 |  * The software consists of a state machine and PID control loops to adjust the orientation of the rocket during its powered ascent.
 6 |  * 
 7 |  * This version 1.0 was used for the first test flight of the rocket on Sep 03 2022.
 8 |  * 
 9 |  * @version 1.0
10 |  * @author Tom Kuttler
11 |  * @GitHub https://github.com/tomkuttler/TVC-Rocket-Flight-Code
12 |  */
13 | 
14 | #include "StateMachine.h"
15 | 
16 | StateMachine *stateMachine;
17 | 
18 | void setup() {
19 |   stateMachine = new StateMachine();
20 | }
21 | 
22 | void loop() {
23 |   stateMachine->stateMachineLoop();
24 | }
25 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/VectorDefinition.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Vector definitions
 3 |  * 
 4 |  * Standard maths definitions
 5 |  */
 6 | 
 7 | struct Vector3 {
 8 |   float x, y, z;
 9 |   Vector3(float x, float y, float z): x{x}, y{y}, z{z} {}
10 |   Vector3(): x(0), y(0), z(0) {}
11 |   Vector3(sensors_vec_t vec): x{vec.x}, y{vec.y}, z{vec.z} {};
12 | 
13 |   // Multiply vector with float
14 |   Vector3 operator * (float d) {
15 |     Vector3 P;
16 |     P.x = x * d;
17 |     P.y = y * d;
18 |     P.z = z * d;
19 |     return P;
20 |   }
21 | 
22 |   // Integrate vector
23 |   Vector3 * operator += (Vector3 Vector) {
24 |     x += Vector.x;
25 |     y += Vector.y;
26 |     z += Vector.z;
27 |     return this;
28 |   }
29 | };
30 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/VoltageDivider.cpp:
--------------------------------------------------------------------------------
 1 | #include "VoltageDivider.h"
 2 | 
 3 | VoltageDivider::VoltageDivider() {
 4 |   
 5 | }
 6 | 
 7 | float VoltageDivider::getBoardVoltage() {
 8 |   int voltageDividerValue = analogRead(VOLTAGEDIVIDER_PIN);
 9 | 
10 |   //        R1 + R2
11 |   // Vin = --------- * Vout
12 |   //          R2
13 |              
14 |   float voltage = ((1000000 + 89000)/89000)*(voltageDividerValue * (5.0 / 1023.0));
15 |   return voltage;
16 | }
17 | 


--------------------------------------------------------------------------------
/TVC-Flight-Code/VoltageDivider.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Voltage divider
 3 |  * 
 4 |  * Calculates the on-board voltage by using a voltage divider connected to an analoge input pin of the Teensy 4.1
 5 |  */
 6 | 
 7 | #include <Arduino.h>
 8 | 
 9 | #define VOLTAGEDIVIDER_PIN 38
10 | 
11 | class VoltageDivider {
12 |   public:
13 |     VoltageDivider();
14 |     float getBoardVoltage();
15 | };
16 | 


--------------------------------------------------------------------------------
/flight.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/tomkuttler/TVC-Rocket-Flight-Code/199b901097098ee33ccf38209613cc0033cfa55f/flight.gif


--------------------------------------------------------------------------------