Hello everyone,
I've been developing a thrust vectoring controlled rocket since 7 months.
(To share thrust vectoring control code is illegal, therefore i'm not going to give you this part of the code)
I'm pretty advanced in my project, however my flight computer (image, PCB,
schematic below) encounters an issue with the data logging. In fact, the code is compiled and everything is working on the rocket except the SD Card.
Something that caught my attention is that it's working in the setup but not in the loop.
Here is the censured code:
/*
Flight Software for Sprite's first launch
Specifications: TIP120, 7.4v S-erc lipo battery
Designed for Sprite's first flight computer
Code written by Ethan D.
No use permitted
*/
// --- Inertial unit and Accelerometer --- //
#include <Arduino_LSM6DS3.h>
#include "Wire.h"
float accelX, accelY, accelZ, // units m/s/s i.e. accelZ if often 9.8 (gravity)
gyroX, gyroY, gyroZ, // units dps (degrees per second)
gyroDriftX, gyroDriftY, gyroDriftZ, // units dps
gyroRoll, gyroPitch, gyroYaw, // units degrees (expect major drift)
gyroCorrectedRoll, gyroCorrectedPitch, gyroCorrectedYaw, // units degrees (expect minor drift)
accRoll, accPitch, accYaw, // units degrees (roll and pitch noisy, yaw not possible)
complementaryRoll, complementaryPitch, complementaryYaw; // units degrees (excellent roll, pitch, yaw minor drift)
long lastTime;
long lastInterval;
// --- Bluetooth Low Energy unit --- //
#include <ArduinoBLE.h>
BLEService ledService("180A");
BLEByteCharacteristic switchCharacteristic("2A57", BLERead | BLEWrite);
// --- Barometer and temperature sensor --- //
#include <Adafruit_BMP085.h>
Adafruit_BMP085 bmp;
#define seaLevelPressure_hPa 1013.25
float altitude;
float temp;
float FirstAltitude;
float CurrentAltitude;
// --- Sd card and datalogging --- //
#include <SPI.h>
#include <SD.h>
//File dataFile;
char fileName[] = "data.txt";
const char Presets[] = "Roll, Pitch, Yaw, PIDX, PIDY, Altitude, Temperature, Flstate";
// --- Leds and RGB --- //
const int Red = A2, Green = A1, Blue = A0;
boolean sthwrong;
bool blinkl = LOW;
// --- Servomotors --- //
#include <Servo.h>
Servo AxeX;
Servo AxeY;
Servo Para;
float SmoothDecalX;
float SmoothDecalY;
float SmoothPrevX;
float SmoothPrevY;
// --- Timing, Pyro, Flight States --- //
unsigned long currentTime = 0;
unsigned long previousTime = 0;
unsigned long crt;
unsigned long prt;
const int Pyro_Channel = 2;
boolean Apogee;
boolean TVC_enabled;
int FlState;
int SDwrong;
//_______________________________________________SETUP_______________________________________________//
void setup(){
analogWrite(Red, 255);
Serial.begin(10000000);
Serial.println("| --- Initialization --- |");
Serial.println("");
Serial.println("First, let's see if there is a component missing ! ");
Serial.println("");
if (!bmp.begin()) {
Serial.println("Fatal error with the barometer !");
sthwrong = 1;
} else {
Serial.println("Barometer is functionnal");
}
Serial.println("");
if (!IMU.begin()) {
Serial.println("Fatal error with the Inertial unit !");
sthwrong = 1;
} else {
Serial.println("Inertial Unit is functionnal");
}
Serial.println("");
if (!BLE.begin()) {
Serial.println("Fatal error with the BLE !");
} else {
Serial.println("Bluetooth enabled !");
}
/* Servos */
AxeX.attach(5);
AxeY.attach(6);
Para.attach(3);
/* IMU */
analogWrite(Blue, 255);
analogWrite(Red, 0);
Serial.println("");
Serial.println("Now let's calibrate the Inertial Measurement Unit !, 5s long");
lastTime = micros();
calibrateIMU(1500, 3500);
Serial.println("");
analogWrite(Blue, 0);
analogWrite(Red, 255);
/* BLE */
BLE.setLocalName("V8Software");
BLE.setAdvertisedService(ledService);
ledService.addCharacteristic(switchCharacteristic);
BLE.addService(ledService);
switchCharacteristic.writeValue(0);
BLE.advertise();
Serial.println("Bluetooth configuration done !");
Serial.println("");
/* DATA */
Serial.println("Set reference altitude :");
FirstAltitude = bmp.readAltitude(seaLevelPressure_hPa * 100);
if (FirstAltitude == 30344.60) {
Serial.println("No correct altitude available...");
} else {
Serial.println(FirstAltitude);
}
Serial.println("");
/* SD CARD */
//pinMode(4, OUTPUT);
//digitalWrite(4,HIGH);
Serial.print("Initializing SD card...");
if (!SD.begin(4)) {
Serial.print("");
Serial.println("...Initialization failed!");
sthwrong = 1;
SDwrong = 1;
} else {
Serial.print("");
Serial.println("...Initialization done.");
}
Serial.println("");
Serial.println("Deleting old files...");
SD.remove(fileName);
Serial.println("Done");
Serial.println("");
File dataFile = SD.open(fileName, FILE_WRITE);
if (!dataFile) {
Serial.println("Fatal error with the Sd card, data recording not possible");
sthwrong = 1;
} else {
Serial.println("Sd card connected !");
}
Serial.println("");
Serial.println("Writing Assets on it if the card is present : ");
dataFile.println();
dataFile.println(Presets);
dataFile.close();
Serial.println("Finished");
Serial.println("Now, let's move to the Loop function, rocket ready for flight ! (or not) ");
analogWrite(Red, 0);
delay(1000);
/* PYROpin */
pinMode(Pyro_Channel, OUTPUT);
pinMode(LED_BUILTIN, OUTPUT);
}
//_______________________________________________IMU Configuration_______________________________________________//
void calibrateIMU(int delayMillis, int calibrationMillis) {
int calibrationCount = 0;
delay(delayMillis);
float sumX, sumY, sumZ;
int startTime = millis();
while (millis() < startTime + calibrationMillis) {
if (readIMU()) {
sumX += gyroX;
sumY += gyroY;
sumZ += gyroZ;
calibrationCount++;
}
}
if (calibrationCount == 0) {
Serial.println("SERIOUS PROBLEMS AHEAD");
} else {
Serial.println("Inertial measurement Unit calibrated");
}
gyroDriftX = sumX / calibrationCount;
gyroDriftY = sumY / calibrationCount;
gyroDriftZ = sumZ / calibrationCount;
}
bool readIMU() {
if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable() ) {
IMU.readAcceleration(accelX, accelY, accelZ);
IMU.readGyroscope(gyroX, gyroY, gyroZ);
return true;
}
return false;
}
//_______________________________________________LOOP_______________________________________________//
void loop(){
/* LEDBlink */
currentTime = millis();
if ((currentTime - previousTime) > 500) {
blinkl = !blinkl;
previousTime = currentTime;
if (blinkl == HIGH) {
if (sthwrong == 1) {
analogWrite(Green, 255);
analogWrite(Red, 255);
} else {
analogWrite(Green, 255);
}
} else {
analogWrite(Green, 0);
analogWrite(Red, 0);
}
}
/* BLUETOOTH */
BLEDevice central = BLE.central();
if (central) {
if (switchCharacteristic.written()) {
switch (switchCharacteristic.value()) {
case 01:
Serial.println("BLE 1 activated");
ReadyForLaunch();
break;
case 02:
Serial.println("BLE 2 activated");
if (SDwrong == 1) {
switchCharacteristic.writeValue(5);
} else {
switchCharacteristic.writeValue(3);
}
break;
}
}
}
/* BAROMETER */
altitude = bmp.readAltitude(seaLevelPressure_hPa * 100);
temp = bmp.readTemperature();
CurrentAltitude = altitude - FirstAltitude;
/* IMUloop */
if (readIMU()) {
long currentTime = micros();
lastInterval = currentTime - lastTime; // expecting t-his to be ~104Hz +- 4%
lastTime = currentTime;
doCalculations();
}
/* VOIDS */
ThrustVectoringControl();
FindFFFS();
/* SD Card */
if(FlState >= 1){
String dataString = "";
dataString += String(complementaryRoll);
dataString += String(",");
dataString += String(complementaryYaw);
dataString += String(",");
dataString += String(complementaryPitch);
dataString += String(",");
dataString += String(PIDX);
dataString += String(",");
dataString += String(PIDY);
dataString += String(",");
dataString += String(CurrentAltitude);
dataString += String(",");
dataString += String(temp);
dataString += String(",");
dataString += String(FlState);
crt = currentTime;
File dataFile = SD.open(fileName, FILE_WRITE);
if (crt >= (prt + 100)) {
if (dataFile) {
dataFile.print(dataString);
dataFile.println(",");
Serial.println("Writing...");
}
prt = crt;
}
dataFile.close();
}
}
//_______________________________________________VOIDS_______________________________________________//
void doCalculations() {
accRoll = atan2(accelY, accelZ) * 180 / M_PI;
accPitch = atan2(-accelX, sqrt(accelY * accelY + accelZ * accelZ)) * 180 / M_PI;
float lastFrequency = (float) 1000000.0 / lastInterval;
gyroRoll = gyroRoll + (gyroX / lastFrequency);
gyroPitch = gyroPitch + (gyroY / lastFrequency);
gyroYaw = gyroYaw + (gyroZ / lastFrequency);
gyroCorrectedRoll = gyroCorrectedRoll + ((gyroX - gyroDriftX) / lastFrequency);
gyroCorrectedPitch = gyroCorrectedPitch + ((gyroY - gyroDriftY) / lastFrequency);
gyroCorrectedYaw = gyroCorrectedYaw + ((gyroZ - gyroDriftZ) / lastFrequency);
complementaryRoll = complementaryRoll + ((gyroX - gyroDriftX) / lastFrequency);
complementaryPitch = complementaryPitch + ((gyroY - gyroDriftY) / lastFrequency);
complementaryYaw = complementaryYaw + ((gyroZ - gyroDriftZ) / lastFrequency);
complementaryRoll = 0.98 * complementaryRoll + 0.02 * accRoll;
complementaryPitch = 0.98 * complementaryPitch + 0.02 * accPitch;
}
void ReadyForLaunch() {
analogWrite(Red, 255);
analogWrite(Green, 255);
analogWrite(Blue, 255);
Serial.println("Thrust Vectoring Mount activated !");
delay(500);
TVC_enabled = 1;
FlState = 1;
}
void FindFFFS() {
if (FlState >= 1) {
if (complementaryYaw < -80 || complementaryYaw > 80 || complementaryPitch < 0) {
if (FlState < 2) {
FlState = 2; // APOGEE !
}
ParachuteSequence();
}
}
}
void ParachuteSequence() {
boolean markup;
if (markup == 0) {
if (CurrentAltitude >= 80) {
delay(2000);
analogWrite(Red, 255);
analogWrite(Green, 0);
analogWrite(Blue, 0);
digitalWrite(LED_BUILTIN, HIGH);
digitalWrite(Pyro_Channel, LOW);
delay(2000);
analogWrite(Red, 0);
digitalWrite(LED_BUILTIN, LOW);
digitalWrite(Pyro_Channel, HIGH);
} else {
analogWrite(Red, 255);
analogWrite(Green, 0);
analogWrite(Blue, 0);
digitalWrite(LED_BUILTIN, HIGH);
digitalWrite(Pyro_Channel, LOW);
delay(2000);
analogWrite(Red, 0);
digitalWrite(LED_BUILTIN, LOW);
digitalWrite(Pyro_Channel, HIGH);
}
markup = 1;
FlState = 3; // PARACHUTE !
}
}
I can give more informations about the rocket and my project to help you.
Thanks in advance,
Ethan D.
by the way i'm only 15yo.
Please answer me to use my work (actually this one isn't working).