kopia lustrzana https://github.com/DanInvents/Rockit
Porównaj commity
2 Commity
4944a89ad6
...
fec78541e7
Autor | SHA1 | Data |
---|---|---|
DanInvents | fec78541e7 | |
DanInvents | 0b84537a2e |
|
@ -0,0 +1,24 @@
|
||||||
|
void SDstartup(){
|
||||||
|
// This program checks if the card is present and can be initialized:
|
||||||
|
if (!SD.begin(17)) {
|
||||||
|
digitalWrite(statusLED, HIGH); //The blue LED turns on if the card cannot be initialized
|
||||||
|
while(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
char filename[] = "00.CSV"; //File name
|
||||||
|
for (uint8_t i = 0; i < 100; i++) { //The SD card can store up to 100 files
|
||||||
|
filename[0] = i/10 + '0';
|
||||||
|
filename[1] = i%10 + '0';
|
||||||
|
if (! SD.exists(filename)) {
|
||||||
|
dataFile = SD.open(filename, O_CREAT | O_WRITE); //Only open a new file if it doesn't exist
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
else if (SD.exists(F("99.CSV"))){
|
||||||
|
while(1){
|
||||||
|
digitalWrite(statusLED, HIGH); //If there are 100 files, the blue LED turns on
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
dataFile.println(F("Time (ms), Altitude (m), Filtered altitude (m), Acceleration (g), Perpendicular acceleration (g), Temperature (C)")); //File header
|
||||||
|
dataFile.flush(); //Writes data to the SD card
|
||||||
|
}
|
|
@ -0,0 +1,8 @@
|
||||||
|
void batteryStatus(){
|
||||||
|
if ((2*analogRead(29)*3.3/(pow(2,12)-1)) < 3.8){
|
||||||
|
digitalWrite(batLED, HIGH);
|
||||||
|
}
|
||||||
|
else if ((2*analogRead(29)*3.3/(pow(2,12)-1)) > 3.8){
|
||||||
|
digitalWrite(batLED, LOW);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,76 @@
|
||||||
|
void beepnblink(){
|
||||||
|
if (initVar == true && overtime == false){
|
||||||
|
|
||||||
|
if (p<30){
|
||||||
|
digitalWrite(statusLED, HIGH);
|
||||||
|
if (piezoEnable == true){
|
||||||
|
analogWrite(piezo, 50); //Turn the piezo on for 300ms
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (p == 30){
|
||||||
|
digitalWrite(statusLED, LOW);
|
||||||
|
if (piezoEnable == true){
|
||||||
|
analogWrite(piezo, 0); //Turn the piezo off
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (p == 400){
|
||||||
|
p = 0;
|
||||||
|
}
|
||||||
|
p++;
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (initVar == false && overtime == false){ //This is necesary because if p<30 after launch is detected the LED and the piezo will be stuck in ON mode.
|
||||||
|
digitalWrite(statusLED, LOW);
|
||||||
|
if (piezoEnable == false){
|
||||||
|
analogWrite(piezo, 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (initVar == false && overtime == true){ //After timeout blink and beep the maximum altitude in meters
|
||||||
|
altToDigits(); //Convert the maximum altitude to digits
|
||||||
|
while(1){
|
||||||
|
for (int i=3; i>-1; i--){
|
||||||
|
blinknbeep(altMaxDig[i]); //Beep and blink the altitude digits
|
||||||
|
delay(700); //0.5s pause in between the beeps and blinks
|
||||||
|
if (i == 0){
|
||||||
|
sleep_ms(10000); //Sleep for 10 seconds to save battery
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void altToDigits(){ //Convert altitude to digits
|
||||||
|
for (int i=0; i<4; i++){
|
||||||
|
rmnd = altMax % 10; //This is the remainder
|
||||||
|
altMaxDig[i] = rmnd;
|
||||||
|
altMax = altMax / 10;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void blinkLED(int n){ //Blinks the blue LED every 200 ms
|
||||||
|
for (int i=0; i<=n; i++){
|
||||||
|
digitalWrite(statusLED, HIGH);
|
||||||
|
delay(200);
|
||||||
|
digitalWrite(statusLED, LOW);
|
||||||
|
delay(200);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void blinknbeep(int n){ //Blinks the blue LED every 300 ms and makes the buzzer beep
|
||||||
|
for (int i=0; i<n; i++){
|
||||||
|
digitalWrite(statusLED, HIGH);
|
||||||
|
if (piezoEnable == true){
|
||||||
|
analogWrite(piezo, 50);
|
||||||
|
}
|
||||||
|
delay(250);
|
||||||
|
|
||||||
|
digitalWrite(statusLED, LOW);
|
||||||
|
if (piezoEnable == true){
|
||||||
|
analogWrite(piezo, 0);
|
||||||
|
}
|
||||||
|
delay(250);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,251 @@
|
||||||
|
// Firmware version 2.1. Release date: 07.08.2022 //This version of the firmware features a complementary-coded rotary switch.
|
||||||
|
|
||||||
|
// In no respect shall DanInvents be accountable for any liabilities, claims, demands, damages or suits resulting from the use of
|
||||||
|
// the flight controller and/or this software. By using this software, you assume all risks associated with this product and
|
||||||
|
// its associated features. While the circuitry and software have been tested, they should be considered experimental and handled
|
||||||
|
// with caution.
|
||||||
|
|
||||||
|
// Before uploading this code make sure that you have downloaded the latest ADXL343 (Adafruit) and MS5637 (Sparkfun) libraries.
|
||||||
|
// You will also need the Circular Buffer library by Roberto Lo Giacco.
|
||||||
|
// Thanks to Adafruit, Sparkfun and Roberto for the open source libraries and also to Homemade Multibody Dynamics for a guide into how to log data fast.
|
||||||
|
// Thanks to MartinMcC for showing how to use a rotary encoder with a microcontroller.
|
||||||
|
// Special thanks to Barun Basnet for the exceptional work on Kalman filters.
|
||||||
|
// Special thanks to Earle Philhower for providing the support that allows using the Arduino libraries and IDE with the RP2040.
|
||||||
|
|
||||||
|
//Firmware improvements of version 2.1 over version 2:
|
||||||
|
|
||||||
|
// Fixed a bug that made the piezo buzzer and the blue LED stay on once launch was detected.
|
||||||
|
// Selecting the position 3 of the rotary switch allows you to test the servo motors. Servo motor 1 moves from its initial to its final position followed by servo 2 3 seconds later.
|
||||||
|
// By using position 3 to test the servos you avoid creating unnecessary files in your microSD card.
|
||||||
|
// Wait for 10 seconds to allow the rocketeer to prepare for launch before the flight computer is armed this avoids creating unnecesary files in your microSD card.
|
||||||
|
|
||||||
|
|
||||||
|
// Firmware improvements of version 2 over version 1.2:
|
||||||
|
|
||||||
|
// Changed the sign of the longitudinal acceleration. Now positive acceleration is pointing downwards and negative upwards.
|
||||||
|
// Changed the way that launch is detected. Now the altitude must be greater than 10 m and the acceleration higher than 2 gs for over 100 ms.
|
||||||
|
// Modified the Kalman filter parameters. Now the filtered data closely follows the measured values but featuring lower noise. This guarantees accurate apogee detection.
|
||||||
|
// Modified the frequency at which the flight computer beeps, now it beeps less frequently before launch.
|
||||||
|
// Now the flight computer goes silent once launch is detected. After 5 minutes, the flight computer beeps and flashes the altitude.
|
||||||
|
// For example, 5 beeps/flashes followed by 7 beeps/flashes means 57 meters.
|
||||||
|
// Now the flight computer can rotate a servo 180 degrees (not yet tested).
|
||||||
|
|
||||||
|
#include <Wire.h>
|
||||||
|
#include "SparkFun_MS5637_Arduino_Library.h"
|
||||||
|
#include <Adafruit_Sensor.h>
|
||||||
|
#include <Adafruit_ADXL343.h>
|
||||||
|
#include <SPI.h>
|
||||||
|
#include <SD.h>
|
||||||
|
#include <Servo.h>
|
||||||
|
#include <EEPROM.h>
|
||||||
|
#include <CircularBuffer.h>
|
||||||
|
#include "pico/stdlib.h"
|
||||||
|
|
||||||
|
CircularBuffer <float,100> FilteredAltitudes;
|
||||||
|
CircularBuffer <float,100> altitudes;
|
||||||
|
CircularBuffer <float,100> accelerations;
|
||||||
|
CircularBuffer <long,100> times;
|
||||||
|
|
||||||
|
//Initialization of Kalman Variables
|
||||||
|
float R = 0.3; //R = measurement noise covariance. Larger R means large measurement uncertainty
|
||||||
|
float Q = 0.3*1e-2; //Q = process noise covariance. Larger Q means larger estimation uncertainty. Thus increasing Q corrects more
|
||||||
|
double Xpe0; // Xpe0 = prior estimation of signal X at time t=0 (current state)
|
||||||
|
double Xe1; //Xe1 = estimation of X at time t=1 (previous state)
|
||||||
|
double Ppe0; //Ppe0 = prior estimation of "error covariance" at t=0
|
||||||
|
double P1,P0; //P1 = error covariance at t=1, P0 = error covariance at t=0
|
||||||
|
double K, Xe0, Z; //K = Kalman gain, Xe0 = estimation of signal at t=0, Z = measured signal at t=0
|
||||||
|
|
||||||
|
//Physical magnitudes
|
||||||
|
float altold; //Baseline pressure
|
||||||
|
int altMax; //Rounded maximum altitude
|
||||||
|
int altMaxDig[4] = {}; //Max altitude digits
|
||||||
|
int rmnd; //Dummy variable remainder
|
||||||
|
int dvsr; //Dummy variable for beeping/flasing the altitude
|
||||||
|
float temp;
|
||||||
|
float currentPressure;
|
||||||
|
float altitudeDelta;
|
||||||
|
float altThreshold = 10; //Altitude threshold for launchd detection in meters
|
||||||
|
float accelThreshold = 2.0; //Acceleration threshold for launch detection in gs.
|
||||||
|
float filteredAltitudeDelta;
|
||||||
|
float rocketAccel;
|
||||||
|
float startingPressure = 0.0;
|
||||||
|
|
||||||
|
//Definition of time and auxiliary integers
|
||||||
|
int tconfig, n, q, p = 0, r = 0;
|
||||||
|
int deltat; //Time step of every loop iteration
|
||||||
|
long int t1; //Time variables
|
||||||
|
long int t4, tout = 300000; //Here tout is the timeout variable tout = 300000 equals 5 min of data logging time
|
||||||
|
|
||||||
|
/* Assign a unique ID to this sensor at the same time */
|
||||||
|
Adafruit_ADXL343 accel = Adafruit_ADXL343(12345, &Wire1);
|
||||||
|
|
||||||
|
char filename[] = "00.CSV"; //Dummy file name to store flight data.
|
||||||
|
|
||||||
|
//Config. rotary switch. This configuration is for the real-coded rotary switch
|
||||||
|
byte switchPins[4] = {15, 13, 14, 16}; //Digital pins assigned to the rotary switch
|
||||||
|
byte rotValue = B0000; // Variable for printing value over serial debug
|
||||||
|
byte switchPos; // Variable for storing the current switch possition
|
||||||
|
byte previousValue; //Variable for storing the previous switch possition
|
||||||
|
|
||||||
|
//Boolean variables defining the state of the program
|
||||||
|
bool initVar = true;
|
||||||
|
bool launchCondition1 = false;
|
||||||
|
bool launchCondition2 = false;
|
||||||
|
bool deploy = false;
|
||||||
|
bool automatic = false;
|
||||||
|
bool timer = false;
|
||||||
|
bool overtime = false;
|
||||||
|
bool piezoEnable = true;
|
||||||
|
|
||||||
|
//LEDs
|
||||||
|
int batLED = 2; //Battery indicator LED
|
||||||
|
int statusLED = 26; //Status LED
|
||||||
|
|
||||||
|
//Servos
|
||||||
|
int servo1pin = 28;
|
||||||
|
int servo2pin = 27;
|
||||||
|
|
||||||
|
//Piezo
|
||||||
|
int piezo = 12;
|
||||||
|
|
||||||
|
MS5637 barometricSensor; //Creates a barometricSensor object
|
||||||
|
File dataFile; //Creates a dataFile object
|
||||||
|
Servo servo1; //Creates a servo1 object
|
||||||
|
Servo servo2; //Creates a servo2 object
|
||||||
|
|
||||||
|
void setup() {
|
||||||
|
//Serial.begin(9600); //For debugging purposes only
|
||||||
|
EEPROM.begin(512); //Emulates EEPROM by allocating 512 kB from the flash memory
|
||||||
|
|
||||||
|
//Declaration of the I2C pins
|
||||||
|
Wire1.setSDA(10);
|
||||||
|
Wire1.setSCL(11);
|
||||||
|
|
||||||
|
//Declaration of the SPI pins
|
||||||
|
SPI.setRX(20);
|
||||||
|
SPI.setTX(19);
|
||||||
|
SPI.setSCK(18);
|
||||||
|
SPI.setCS(17);
|
||||||
|
|
||||||
|
//Declaration of the pins for the battery indicator, and status LED as well as the pin for the buzzer
|
||||||
|
pinMode(batLED, OUTPUT); //Low battery LED
|
||||||
|
pinMode(statusLED, OUTPUT); //Status LED
|
||||||
|
pinMode(piezo, OUTPUT); //Piezo buzzer
|
||||||
|
|
||||||
|
//Piezo buzzer PWM settings
|
||||||
|
analogWriteFreq(4000); //Set the piezo frequency to 4kHz
|
||||||
|
analogWriteRange(100); //Set the dynamic range of the piezo
|
||||||
|
|
||||||
|
for (int i = 0; i < 4; i = i + 1){
|
||||||
|
pinMode(switchPins[i], INPUT_PULLUP);
|
||||||
|
}
|
||||||
|
|
||||||
|
barometricSensor.begin(Wire1);
|
||||||
|
barometricSensor.setResolution(ms5637_resolution_osr_1024);
|
||||||
|
|
||||||
|
//Take 16 readings and average them
|
||||||
|
startingPressure = 0.0;
|
||||||
|
for (int x = 0 ; x < 16 ; x++)
|
||||||
|
startingPressure += barometricSensor.getPressure();
|
||||||
|
startingPressure /= (float)16;
|
||||||
|
|
||||||
|
accel.begin();
|
||||||
|
accel.setRange(ADXL343_RANGE_16_G);
|
||||||
|
accel.setDataRate(ADXL343_DATARATE_400_HZ);
|
||||||
|
switchStartup();
|
||||||
|
delay(10000); //Wait for 10 seconds to allow the rocketeer to prepare for launch before the flight computer is armed.
|
||||||
|
preLaunch(); //Here I store the first second of data into the circular buffers
|
||||||
|
SDstartup();
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
batteryStatus(); //Check the battery level
|
||||||
|
|
||||||
|
if (overtime == false){
|
||||||
|
currentPressure = barometricSensor.getPressure();
|
||||||
|
temp = barometricSensor.getTemperature();
|
||||||
|
sensors_event_t event;
|
||||||
|
accel.getEvent(&event);
|
||||||
|
rocketAccel = -((event.acceleration.y/9.81)-(event.acceleration.x/9.81))/sqrt(2);
|
||||||
|
altitudeDelta = barometricSensor.altitudeChange(currentPressure, startingPressure);
|
||||||
|
filteredAltitudeDelta = kalmanFilter(altitudeDelta);
|
||||||
|
|
||||||
|
|
||||||
|
if (altitudeDelta > altThreshold && launchCondition1 == false){ //Threshold condition set to 10 m
|
||||||
|
launchCondition1 = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (rocketAccel > accelThreshold && launchCondition2 == false){
|
||||||
|
q++;
|
||||||
|
if (q > 10){ //launcCondition2 stablishes the requirement that to detect launch there should be at least an acceleration of 2g for 100 ms
|
||||||
|
launchCondition2 = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rocketAccel < accelThreshold && launchCondition2 == false){
|
||||||
|
q = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (initVar == true){ //Store data to the circular buffer
|
||||||
|
accelerations.push(rocketAccel);
|
||||||
|
altitudes.push(altitudeDelta);
|
||||||
|
FilteredAltitudes.push(filteredAltitudeDelta);
|
||||||
|
times.push(millis()-t4); //Circular buffer for time
|
||||||
|
|
||||||
|
if (launchCondition1 == true && launchCondition2 == true){
|
||||||
|
initVar = false;
|
||||||
|
|
||||||
|
for (int i = 0; i<=99; i++){ //Saving the buffer allows me to store the data measured before launch.
|
||||||
|
dataFile.print(times[i]-times[0]); //Here times[0] sets the time zero for the time variable
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(altitudes.shift());
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(FilteredAltitudes.shift());
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(accelerations.shift());
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(event.acceleration.z/9.81);
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.println(temp, 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
dataFile.flush(); //Store data of the 908 ms before launch
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (initVar == false){
|
||||||
|
t1 = millis() - t4 - times[0];
|
||||||
|
recovery();
|
||||||
|
dataFile.print(t1);
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(altitudeDelta);
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(filteredAltitudeDelta);
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(rocketAccel);
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(event.acceleration.z/9.81);
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.println(temp, 1);
|
||||||
|
|
||||||
|
if (altitudeDelta > altold){ //Here is where I store the maximum altitude value
|
||||||
|
altMax = round(altitudeDelta);
|
||||||
|
altold = altMax;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (r == 200 && overtime == false){ //Here I set the rate at which I send data to the uSD card
|
||||||
|
r = 0;
|
||||||
|
dataFile.flush();
|
||||||
|
}
|
||||||
|
r++;
|
||||||
|
|
||||||
|
|
||||||
|
if (t1 >= tout){
|
||||||
|
overtime = true;
|
||||||
|
dataFile.flush();
|
||||||
|
dataFile.close(); //After timeout flush the data to the microSD card and close the file
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
beepnblink();
|
||||||
|
}
|
Plik binarny nie jest wyświetlany.
|
@ -0,0 +1,20 @@
|
||||||
|
// This program performs a Kalman filter of the flight data. It smoothens the data and ignores transitory events.
|
||||||
|
|
||||||
|
// Q = process noise covariance
|
||||||
|
// R = measurement noise covariance. Larger R means large measurement uncertainty. Larger Q means larger estimation uncertainty. Thus increasing Q corrects more.
|
||||||
|
// Xpe0 = prior estimation of signal X at time t=0 (current state)
|
||||||
|
// Xe1 = estimation of X at time t=1 (previous state)
|
||||||
|
// Ppe0 = prior estimation of "error covariance" at t=0,
|
||||||
|
// P1 = error covariance at t=1, P0 = error covariance at t=0
|
||||||
|
// K = Kalman gain, Xe0 = estimation of signal at t=0, Z = measured signal at t=0;
|
||||||
|
|
||||||
|
float kalmanFilter(float Z){
|
||||||
|
Xpe0 = Xe1; // Assumption of prediction 1
|
||||||
|
Ppe0 = P1 + Q; // Update of prior estimation of "error covariance"
|
||||||
|
K = Ppe0/(Ppe0 + R); // Measurement update or correction of "Kalman gain"
|
||||||
|
Xe0 = Xpe0 + K * (Z - Xpe0); // Measurement update or correction of "estimated signal"
|
||||||
|
P0 = (1 - K) * Ppe0; // Measurement update or correction of "error covariance";
|
||||||
|
Xe1 = Xe0;
|
||||||
|
P1 = P0;
|
||||||
|
return Xe0;
|
||||||
|
}
|
|
@ -0,0 +1,15 @@
|
||||||
|
void preLaunch(){ //This code works great
|
||||||
|
t4 = millis();
|
||||||
|
|
||||||
|
for (int i = 0; i<=99; i++){
|
||||||
|
currentPressure = barometricSensor.getPressure();
|
||||||
|
sensors_event_t event;
|
||||||
|
accel.getEvent(&event);
|
||||||
|
rocketAccel = ((event.acceleration.y/9.81+0.01)-(event.acceleration.x/9.81-0.04))/sqrt(2);
|
||||||
|
accelerations.push(rocketAccel);
|
||||||
|
altitudeDelta = barometricSensor.altitudeChange(currentPressure, startingPressure)+0.6;
|
||||||
|
altitudes.push(altitudeDelta);
|
||||||
|
FilteredAltitudes.push(kalmanFilter(altitudeDelta));
|
||||||
|
times.push(millis()-t4);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,52 @@
|
||||||
|
void recovery(){
|
||||||
|
|
||||||
|
if (timer == true && t1 >= (1000*EEPROM.read(1)+908)){ //Here the 908 ms correspond to the time covered by the circular buffer
|
||||||
|
servo1.write(EEPROM.read(3)); //Move servo1 to the final position EEPROM.read(3);
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
if (timer == true && t1 >= (1000*EEPROM.read(1)+908+2000)){ //We disable the servo after 2 seconds to save power
|
||||||
|
servo1.detach();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (t1 >= (1000*EEPROM.read(1) + 500*EEPROM.read(6) + 908 + 100)){ //The additional 100 ms is to prevent both servos from moving simultaneously.
|
||||||
|
servo2.write(EEPROM.read(5));
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
timer = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (t1 >= (1000*EEPROM.read(1) + 500*EEPROM.read(6) + 908 + 100 + 2000)){ //We disable the servo after 2 seconds to save power
|
||||||
|
servo2.detach();
|
||||||
|
timer = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (automatic == true){
|
||||||
|
if ((filteredAltitudeDelta - altold) < -0.01){
|
||||||
|
n++;
|
||||||
|
if (n == 4 && deploy == false){
|
||||||
|
deploy = true;
|
||||||
|
tconfig = t1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if ((filteredAltitudeDelta - altold) >= 0 && deploy == false){
|
||||||
|
n = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (deploy == true && (t1-tconfig) >= 500*EEPROM.read(0)){
|
||||||
|
servo1.write(EEPROM.read(3));
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
if (deploy == true && (t1-tconfig) >= (500*EEPROM.read(0) + 2000)){ //We disable the servo after 2 seconds to save power
|
||||||
|
servo1.detach();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (deploy == true && (t1-tconfig) >= (500*(EEPROM.read(0) + EEPROM.read(6)))){
|
||||||
|
servo2.write(EEPROM.read(5));
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
deploy = false;
|
||||||
|
if (deploy == true && (t1-tconfig) >= (500*(EEPROM.read(0) + EEPROM.read(6)) + 2000)){ //We disable the servo after 2 seconds to save power
|
||||||
|
servo2.detach();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,179 @@
|
||||||
|
// This program reads the rotary switch.
|
||||||
|
|
||||||
|
void readRotSwitch(){
|
||||||
|
for (int k = 0; k < 4; k++){
|
||||||
|
if (digitalRead(switchPins[k]) == LOW) {
|
||||||
|
bitClear(rotValue, k); //sets bit k to 1
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
bitSet(rotValue, k); //sets bit k to 0
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void switchStartup(){
|
||||||
|
readRotSwitch();
|
||||||
|
|
||||||
|
if (rotValue == 10){ //A Automatic mode
|
||||||
|
servo1.write(EEPROM.read(2)); //EEPROM.read(2)
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
|
||||||
|
delay(100); //It is important to have a delay to reduce the current spike drawn by the motors
|
||||||
|
servo2.write(EEPROM.read(4)); //EEPROM.read(4)
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
|
||||||
|
automatic = true;
|
||||||
|
delay(300);
|
||||||
|
blinkLED(EEPROM.read(0));
|
||||||
|
delay(500);
|
||||||
|
blinkLED(EEPROM.read(6));
|
||||||
|
servo1.detach(); //I detach the servos to save power
|
||||||
|
servo2.detach();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 11){ //B Timer mode
|
||||||
|
servo1.write(EEPROM.read(2)); //EEPROM.read(2)
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
|
||||||
|
delay(100); //It is important to have a delay to reduce the current spike drawn by the motors
|
||||||
|
servo2.write(EEPROM.read(4)); //EEPROM.read(4)
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
|
||||||
|
timer = true;
|
||||||
|
delay(300);
|
||||||
|
blinkLED(EEPROM.read(1));
|
||||||
|
delay(500);
|
||||||
|
blinkLED(EEPROM.read(6));
|
||||||
|
servo1.detach(); //I detach the servos to save power
|
||||||
|
servo2.detach();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 12){ //C, Configure the time for parachute deployment on automatic mode
|
||||||
|
while(1){
|
||||||
|
readRotSwitch();
|
||||||
|
blinkLED(1);
|
||||||
|
if (previousValue != rotValue){
|
||||||
|
EEPROM.write(0, rotValue);
|
||||||
|
EEPROM.commit();
|
||||||
|
previousValue = rotValue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 13){ //D, Configure the time for parachute deployment on timer mode.
|
||||||
|
while(1){
|
||||||
|
readRotSwitch();
|
||||||
|
blinkLED(1);
|
||||||
|
if (previousValue != rotValue){
|
||||||
|
EEPROM.write(1, rotValue);
|
||||||
|
EEPROM.commit();
|
||||||
|
previousValue = rotValue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
else if (rotValue == 14){ //E, Adjust servo's 1 initial possition
|
||||||
|
while(1){
|
||||||
|
readRotSwitch();
|
||||||
|
servo1.write(180*rotValue/15);
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
blinkLED(1);
|
||||||
|
if (previousValue != rotValue){
|
||||||
|
EEPROM.write(2, 180*rotValue/15);
|
||||||
|
EEPROM.commit();
|
||||||
|
}
|
||||||
|
previousValue == rotValue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 15){ //F, Adjust servo's 1 final possition
|
||||||
|
while(1){
|
||||||
|
readRotSwitch();
|
||||||
|
servo1.write(180*rotValue/15);
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
|
||||||
|
blinkLED(1);
|
||||||
|
if (previousValue != rotValue){
|
||||||
|
EEPROM.write(3, 180*rotValue/15);
|
||||||
|
EEPROM.commit();
|
||||||
|
}
|
||||||
|
previousValue == rotValue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 0){ //0, Adjust the servo's 2 initial possition
|
||||||
|
while(1){
|
||||||
|
readRotSwitch();
|
||||||
|
servo2.write(180*rotValue/15);
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
|
||||||
|
blinkLED(1);
|
||||||
|
if (previousValue != rotValue){
|
||||||
|
EEPROM.write(4, 180*rotValue/15);
|
||||||
|
EEPROM.commit();
|
||||||
|
}
|
||||||
|
previousValue == rotValue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 1){ //1, Adjust the servo's 2 final possition
|
||||||
|
while(1){
|
||||||
|
readRotSwitch();
|
||||||
|
servo2.write(180*rotValue/15);
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
|
||||||
|
blinkLED(1);
|
||||||
|
if (previousValue != rotValue){
|
||||||
|
EEPROM.write(5, 180*rotValue/15);
|
||||||
|
EEPROM.commit();
|
||||||
|
}
|
||||||
|
previousValue == rotValue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 2){ //2, Adjust the deploy time for servo 2 after servo 1
|
||||||
|
while(1){
|
||||||
|
readRotSwitch();
|
||||||
|
blinkLED(1);
|
||||||
|
if (previousValue != rotValue){
|
||||||
|
EEPROM.write(6, rotValue);
|
||||||
|
EEPROM.commit();
|
||||||
|
previousValue = rotValue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 3){ //3, Test the servo motors
|
||||||
|
servo1.write(EEPROM.read(2)); //EEPROM.read(2)
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
delay(500); //It is important to have a delay to reduce the current spike drawn by the motors
|
||||||
|
servo1.detach();
|
||||||
|
servo2.write(EEPROM.read(4)); //EEPROM.read(4)
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
delay(500);
|
||||||
|
servo2.detach();
|
||||||
|
blinkLED(2);
|
||||||
|
delay(2000);
|
||||||
|
servo1.write(EEPROM.read(3)); //Move servo1 to the final position EEPROM.read(3);
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
blinkLED(2);
|
||||||
|
delay(2000);
|
||||||
|
servo1.detach();
|
||||||
|
servo2.write(EEPROM.read(5));
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
delay(500);
|
||||||
|
servo2.detach();
|
||||||
|
while(1){
|
||||||
|
sleep_ms(100000);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else {
|
||||||
|
while (true){
|
||||||
|
sleep_ms(10000);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,24 @@
|
||||||
|
void SDstartup(){
|
||||||
|
// This program checks if the card is present and can be initialized:
|
||||||
|
if (!SD.begin(17)) {
|
||||||
|
digitalWrite(statusLED, HIGH); //The blue LED turns on if the card cannot be initialized
|
||||||
|
while(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
char filename[] = "00.CSV"; //File name
|
||||||
|
for (uint8_t i = 0; i < 100; i++) { //The SD card can store up to 100 files
|
||||||
|
filename[0] = i/10 + '0';
|
||||||
|
filename[1] = i%10 + '0';
|
||||||
|
if (! SD.exists(filename)) {
|
||||||
|
dataFile = SD.open(filename, O_CREAT | O_WRITE); //Only open a new file if it doesn't exist
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
else if (SD.exists(F("99.CSV"))){
|
||||||
|
while(1){
|
||||||
|
digitalWrite(statusLED, HIGH); //If there are 100 files, the blue LED turns on
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
dataFile.println(F("Time (ms), Altitude (m), Filtered altitude (m), Acceleration (g), Perpendicular acceleration (g), Temperature (C)")); //File header
|
||||||
|
dataFile.flush(); //Writes data to the SD card
|
||||||
|
}
|
|
@ -0,0 +1,8 @@
|
||||||
|
void batteryStatus(){
|
||||||
|
if ((2*analogRead(29)*3.3/(pow(2,12)-1)) < 3.8){
|
||||||
|
digitalWrite(batLED, HIGH);
|
||||||
|
}
|
||||||
|
else if ((2*analogRead(29)*3.3/(pow(2,12)-1)) > 3.8){
|
||||||
|
digitalWrite(batLED, LOW);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,76 @@
|
||||||
|
void beepnblink(){
|
||||||
|
if (initVar == true && overtime == false){
|
||||||
|
|
||||||
|
if (p<30){
|
||||||
|
digitalWrite(statusLED, HIGH);
|
||||||
|
if (piezoEnable == true){
|
||||||
|
analogWrite(piezo, 50); //Turn the piezo on for 300ms
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (p == 30){
|
||||||
|
digitalWrite(statusLED, LOW);
|
||||||
|
if (piezoEnable == true){
|
||||||
|
analogWrite(piezo, 0); //Turn the piezo off
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (p == 400){
|
||||||
|
p = 0;
|
||||||
|
}
|
||||||
|
p++;
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (initVar == false && overtime == false){ //This is necesary because if p<30 after launch is detected the LED and the piezo will be stuck in ON mode.
|
||||||
|
digitalWrite(statusLED, LOW);
|
||||||
|
if (piezoEnable == false){
|
||||||
|
analogWrite(piezo, 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (initVar == false && overtime == true){ //After timeout blink and beep the maximum altitude in meters
|
||||||
|
altToDigits(); //Convert the maximum altitude to digits
|
||||||
|
while(1){
|
||||||
|
for (int i=3; i>-1; i--){
|
||||||
|
blinknbeep(altMaxDig[i]); //Beep and blink the altitude digits
|
||||||
|
delay(700); //0.5s pause in between the beeps and blinks
|
||||||
|
if (i == 0){
|
||||||
|
sleep_ms(10000); //Sleep for 10 seconds to save battery
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void altToDigits(){ //Convert altitude to digits
|
||||||
|
for (int i=0; i<4; i++){
|
||||||
|
rmnd = altMax % 10; //This is the remainder
|
||||||
|
altMaxDig[i] = rmnd;
|
||||||
|
altMax = altMax / 10;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void blinkLED(int n){ //Blinks the blue LED every 200 ms
|
||||||
|
for (int i=0; i<=n; i++){
|
||||||
|
digitalWrite(statusLED, HIGH);
|
||||||
|
delay(200);
|
||||||
|
digitalWrite(statusLED, LOW);
|
||||||
|
delay(200);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void blinknbeep(int n){ //Blinks the blue LED every 300 ms and makes the buzzer beep
|
||||||
|
for (int i=0; i<n; i++){
|
||||||
|
digitalWrite(statusLED, HIGH);
|
||||||
|
if (piezoEnable == true){
|
||||||
|
analogWrite(piezo, 50);
|
||||||
|
}
|
||||||
|
delay(250);
|
||||||
|
|
||||||
|
digitalWrite(statusLED, LOW);
|
||||||
|
if (piezoEnable == true){
|
||||||
|
analogWrite(piezo, 0);
|
||||||
|
}
|
||||||
|
delay(250);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,251 @@
|
||||||
|
// Firmware version 2.1. Release date: 07.08.2022 //This version of the firmware features a real-coded rotary switch.
|
||||||
|
|
||||||
|
// In no respect shall DanInvents be accountable for any liabilities, claims, demands, damages or suits resulting from the use of
|
||||||
|
// the flight controller and/or this software. By using this software, you assume all risks associated with this product and
|
||||||
|
// its associated features. While the circuitry and software have been tested, they should be considered experimental and handled
|
||||||
|
// with caution.
|
||||||
|
|
||||||
|
// Before uploading this code make sure that you have downloaded the latest ADXL343 (Adafruit) and MS5637 (Sparkfun) libraries.
|
||||||
|
// You will also need the Circular Buffer library by Roberto Lo Giacco.
|
||||||
|
// Thanks to Adafruit, Sparkfun and Roberto for the open source libraries and also to Homemade Multibody Dynamics for a guide into how to log data fast.
|
||||||
|
// Thanks to MartinMcC for showing how to use a rotary encoder with a microcontroller.
|
||||||
|
// Special thanks to Barun Basnet for the exceptional work on Kalman filters.
|
||||||
|
// Special thanks to Earle Philhower for providing the support that allows using the Arduino libraries and IDE with the RP2040.
|
||||||
|
|
||||||
|
//Firmware improvements of version 2.1 over version 2:
|
||||||
|
|
||||||
|
// Fixed a bug that made the piezo buzzer and the blue LED stay on once launch was detected.
|
||||||
|
// Selecting the position 3 of the rotary switch allows you to test the servo motors. Servo motor 1 moves from its initial to its final position followed by servo 2 3 seconds later.
|
||||||
|
// By using position 3 to test the servos you avoid creating unnecessary files in your microSD card.
|
||||||
|
// Wait for 10 seconds to allow the rocketeer to prepare for launch before the flight computer is armed this avoids creating unnecesary files in your microSD card.
|
||||||
|
|
||||||
|
|
||||||
|
// Firmware improvements of version 2 over version 1.2:
|
||||||
|
|
||||||
|
// Changed the sign of the longitudinal acceleration. Now positive acceleration is pointing downwards and negative upwards.
|
||||||
|
// Changed the way that launch is detected. Now the altitude must be greater than 10 m and the acceleration higher than 2 gs for over 100 ms.
|
||||||
|
// Modified the Kalman filter parameters. Now the filtered data closely follows the measured values but featuring lower noise. This guarantees accurate apogee detection.
|
||||||
|
// Modified the frequency at which the flight computer beeps, now it beeps less frequently before launch.
|
||||||
|
// Now the flight computer goes silent once launch is detected. After 5 minutes, the flight computer beeps and flashes the altitude.
|
||||||
|
// For example, 5 beeps/flashes followed by 7 beeps/flashes means 57 meters.
|
||||||
|
// Now the flight computer can rotate a servo 180 degrees (not yet tested).
|
||||||
|
|
||||||
|
#include <Wire.h>
|
||||||
|
#include "SparkFun_MS5637_Arduino_Library.h"
|
||||||
|
#include <Adafruit_Sensor.h>
|
||||||
|
#include <Adafruit_ADXL343.h>
|
||||||
|
#include <SPI.h>
|
||||||
|
#include <SD.h>
|
||||||
|
#include <Servo.h>
|
||||||
|
#include <EEPROM.h>
|
||||||
|
#include <CircularBuffer.h>
|
||||||
|
#include "pico/stdlib.h"
|
||||||
|
|
||||||
|
CircularBuffer <float,100> FilteredAltitudes;
|
||||||
|
CircularBuffer <float,100> altitudes;
|
||||||
|
CircularBuffer <float,100> accelerations;
|
||||||
|
CircularBuffer <long,100> times;
|
||||||
|
|
||||||
|
//Initialization of Kalman Variables
|
||||||
|
float R = 0.3; //R = measurement noise covariance. Larger R means large measurement uncertainty
|
||||||
|
float Q = 0.3*1e-2; //Q = process noise covariance. Larger Q means larger estimation uncertainty. Thus increasing Q corrects more
|
||||||
|
double Xpe0; // Xpe0 = prior estimation of signal X at time t=0 (current state)
|
||||||
|
double Xe1; //Xe1 = estimation of X at time t=1 (previous state)
|
||||||
|
double Ppe0; //Ppe0 = prior estimation of "error covariance" at t=0
|
||||||
|
double P1,P0; //P1 = error covariance at t=1, P0 = error covariance at t=0
|
||||||
|
double K, Xe0, Z; //K = Kalman gain, Xe0 = estimation of signal at t=0, Z = measured signal at t=0
|
||||||
|
|
||||||
|
//Physical magnitudes
|
||||||
|
float altold; //Baseline pressure
|
||||||
|
int altMax; //Rounded maximum altitude
|
||||||
|
int altMaxDig[4] = {}; //Max altitude digits
|
||||||
|
int rmnd; //Dummy variable remainder
|
||||||
|
int dvsr; //Dummy variable for beeping/flasing the altitude
|
||||||
|
float temp;
|
||||||
|
float currentPressure;
|
||||||
|
float altitudeDelta;
|
||||||
|
float altThreshold = 10; //Altitude threshold for launchd detection in meters
|
||||||
|
float accelThreshold = 2.0; //Acceleration threshold for launch detection in gs.
|
||||||
|
float filteredAltitudeDelta;
|
||||||
|
float rocketAccel;
|
||||||
|
float startingPressure = 0.0;
|
||||||
|
|
||||||
|
//Definition of time and auxiliary integers
|
||||||
|
int tconfig, n, q, p = 0, r = 0;
|
||||||
|
int deltat; //Time step of every loop iteration
|
||||||
|
long int t1; //Time variables
|
||||||
|
long int t4, tout = 300000; //Here tout is the timeout variable tout = 300000 equals 5 min of data logging time
|
||||||
|
|
||||||
|
/* Assign a unique ID to this sensor at the same time */
|
||||||
|
Adafruit_ADXL343 accel = Adafruit_ADXL343(12345, &Wire1);
|
||||||
|
|
||||||
|
char filename[] = "00.CSV"; //Dummy file name to store flight data.
|
||||||
|
|
||||||
|
//Config. rotary switch. This configuration is for the real-coded rotary switch
|
||||||
|
byte switchPins[4] = {15, 13, 14, 16}; //Digital pins assigned to the rotary switch
|
||||||
|
byte rotValue = B0000; // Variable for printing value over serial debug
|
||||||
|
byte switchPos; // Variable for storing the current switch possition
|
||||||
|
byte previousValue; //Variable for storing the previous switch possition
|
||||||
|
|
||||||
|
//Boolean variables defining the state of the program
|
||||||
|
bool initVar = true;
|
||||||
|
bool launchCondition1 = false;
|
||||||
|
bool launchCondition2 = false;
|
||||||
|
bool deploy = false;
|
||||||
|
bool automatic = false;
|
||||||
|
bool timer = false;
|
||||||
|
bool overtime = false;
|
||||||
|
bool piezoEnable = true;
|
||||||
|
|
||||||
|
//LEDs
|
||||||
|
int batLED = 2; //Battery indicator LED
|
||||||
|
int statusLED = 26; //Status LED
|
||||||
|
|
||||||
|
//Servos
|
||||||
|
int servo1pin = 28;
|
||||||
|
int servo2pin = 27;
|
||||||
|
|
||||||
|
//Piezo
|
||||||
|
int piezo = 12;
|
||||||
|
|
||||||
|
MS5637 barometricSensor; //Creates a barometricSensor object
|
||||||
|
File dataFile; //Creates a dataFile object
|
||||||
|
Servo servo1; //Creates a servo1 object
|
||||||
|
Servo servo2; //Creates a servo2 object
|
||||||
|
|
||||||
|
void setup() {
|
||||||
|
//Serial.begin(9600); //For debugging purposes only
|
||||||
|
EEPROM.begin(512); //Emulates EEPROM by allocating 512 kB from the flash memory
|
||||||
|
|
||||||
|
//Declaration of the I2C pins
|
||||||
|
Wire1.setSDA(10);
|
||||||
|
Wire1.setSCL(11);
|
||||||
|
|
||||||
|
//Declaration of the SPI pins
|
||||||
|
SPI.setRX(20);
|
||||||
|
SPI.setTX(19);
|
||||||
|
SPI.setSCK(18);
|
||||||
|
SPI.setCS(17);
|
||||||
|
|
||||||
|
//Declaration of the pins for the battery indicator, and status LED as well as the pin for the buzzer
|
||||||
|
pinMode(batLED, OUTPUT); //Low battery LED
|
||||||
|
pinMode(statusLED, OUTPUT); //Status LED
|
||||||
|
pinMode(piezo, OUTPUT); //Piezo buzzer
|
||||||
|
|
||||||
|
//Piezo buzzer PWM settings
|
||||||
|
analogWriteFreq(4000); //Set the piezo frequency to 4kHz
|
||||||
|
analogWriteRange(100); //Set the dynamic range of the piezo
|
||||||
|
|
||||||
|
for (int i = 0; i < 4; i = i + 1){
|
||||||
|
pinMode(switchPins[i], INPUT_PULLUP);
|
||||||
|
}
|
||||||
|
|
||||||
|
barometricSensor.begin(Wire1);
|
||||||
|
barometricSensor.setResolution(ms5637_resolution_osr_1024);
|
||||||
|
|
||||||
|
//Take 16 readings and average them
|
||||||
|
startingPressure = 0.0;
|
||||||
|
for (int x = 0 ; x < 16 ; x++)
|
||||||
|
startingPressure += barometricSensor.getPressure();
|
||||||
|
startingPressure /= (float)16;
|
||||||
|
|
||||||
|
accel.begin();
|
||||||
|
accel.setRange(ADXL343_RANGE_16_G);
|
||||||
|
accel.setDataRate(ADXL343_DATARATE_400_HZ);
|
||||||
|
switchStartup();
|
||||||
|
delay(10000); //Wait for 10 seconds to allow the rocketeer to prepare for launch before the flight computer is armed.
|
||||||
|
preLaunch(); //Here I store the first second of data into the circular buffers
|
||||||
|
SDstartup();
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
batteryStatus(); //Check the battery level
|
||||||
|
|
||||||
|
if (overtime == false){
|
||||||
|
currentPressure = barometricSensor.getPressure();
|
||||||
|
temp = barometricSensor.getTemperature();
|
||||||
|
sensors_event_t event;
|
||||||
|
accel.getEvent(&event);
|
||||||
|
rocketAccel = -((event.acceleration.y/9.81)-(event.acceleration.x/9.81))/sqrt(2);
|
||||||
|
altitudeDelta = barometricSensor.altitudeChange(currentPressure, startingPressure);
|
||||||
|
filteredAltitudeDelta = kalmanFilter(altitudeDelta);
|
||||||
|
|
||||||
|
|
||||||
|
if (altitudeDelta > altThreshold && launchCondition1 == false){ //Threshold condition set to 10 m
|
||||||
|
launchCondition1 = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (rocketAccel > accelThreshold && launchCondition2 == false){
|
||||||
|
q++;
|
||||||
|
if (q > 10){ //launcCondition2 stablishes the requirement that to detect launch there should be at least an acceleration of 2g for 100 ms
|
||||||
|
launchCondition2 = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rocketAccel < accelThreshold && launchCondition2 == false){
|
||||||
|
q = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (initVar == true){ //Store data to the circular buffer
|
||||||
|
accelerations.push(rocketAccel);
|
||||||
|
altitudes.push(altitudeDelta);
|
||||||
|
FilteredAltitudes.push(filteredAltitudeDelta);
|
||||||
|
times.push(millis()-t4); //Circular buffer for time
|
||||||
|
|
||||||
|
if (launchCondition1 == true && launchCondition2 == true){
|
||||||
|
initVar = false;
|
||||||
|
|
||||||
|
for (int i = 0; i<=99; i++){ //Saving the buffer allows me to store the data measured before launch.
|
||||||
|
dataFile.print(times[i]-times[0]); //Here times[0] sets the time zero for the time variable
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(altitudes.shift());
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(FilteredAltitudes.shift());
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(accelerations.shift());
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(event.acceleration.z/9.81);
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.println(temp, 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
dataFile.flush(); //Store data of the 908 ms before launch
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (initVar == false){
|
||||||
|
t1 = millis() - t4 - times[0];
|
||||||
|
recovery();
|
||||||
|
dataFile.print(t1);
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(altitudeDelta);
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(filteredAltitudeDelta);
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(rocketAccel);
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.print(event.acceleration.z/9.81);
|
||||||
|
dataFile.print(',');
|
||||||
|
dataFile.println(temp, 1);
|
||||||
|
|
||||||
|
if (altitudeDelta > altold){ //Here is where I store the maximum altitude value
|
||||||
|
altMax = round(altitudeDelta);
|
||||||
|
altold = altMax;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (r == 200 && overtime == false){ //Here I set the rate at which I send data to the uSD card
|
||||||
|
r = 0;
|
||||||
|
dataFile.flush();
|
||||||
|
}
|
||||||
|
r++;
|
||||||
|
|
||||||
|
|
||||||
|
if (t1 >= tout){
|
||||||
|
overtime = true;
|
||||||
|
dataFile.flush();
|
||||||
|
dataFile.close(); //After timeout flush the data to the microSD card and close the file
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
beepnblink();
|
||||||
|
}
|
Plik binarny nie jest wyświetlany.
|
@ -0,0 +1,20 @@
|
||||||
|
// This program performs a Kalman filter of the flight data. It smoothens the data and ignores transitory events.
|
||||||
|
|
||||||
|
// Q = process noise covariance
|
||||||
|
// R = measurement noise covariance. Larger R means large measurement uncertainty. Larger Q means larger estimation uncertainty. Thus increasing Q corrects more.
|
||||||
|
// Xpe0 = prior estimation of signal X at time t=0 (current state)
|
||||||
|
// Xe1 = estimation of X at time t=1 (previous state)
|
||||||
|
// Ppe0 = prior estimation of "error covariance" at t=0,
|
||||||
|
// P1 = error covariance at t=1, P0 = error covariance at t=0
|
||||||
|
// K = Kalman gain, Xe0 = estimation of signal at t=0, Z = measured signal at t=0;
|
||||||
|
|
||||||
|
float kalmanFilter(float Z){
|
||||||
|
Xpe0 = Xe1; // Assumption of prediction 1
|
||||||
|
Ppe0 = P1 + Q; // Update of prior estimation of "error covariance"
|
||||||
|
K = Ppe0/(Ppe0 + R); // Measurement update or correction of "Kalman gain"
|
||||||
|
Xe0 = Xpe0 + K * (Z - Xpe0); // Measurement update or correction of "estimated signal"
|
||||||
|
P0 = (1 - K) * Ppe0; // Measurement update or correction of "error covariance";
|
||||||
|
Xe1 = Xe0;
|
||||||
|
P1 = P0;
|
||||||
|
return Xe0;
|
||||||
|
}
|
|
@ -0,0 +1,15 @@
|
||||||
|
void preLaunch(){ //This code works great
|
||||||
|
t4 = millis();
|
||||||
|
|
||||||
|
for (int i = 0; i<=99; i++){
|
||||||
|
currentPressure = barometricSensor.getPressure();
|
||||||
|
sensors_event_t event;
|
||||||
|
accel.getEvent(&event);
|
||||||
|
rocketAccel = ((event.acceleration.y/9.81+0.01)-(event.acceleration.x/9.81-0.04))/sqrt(2);
|
||||||
|
accelerations.push(rocketAccel);
|
||||||
|
altitudeDelta = barometricSensor.altitudeChange(currentPressure, startingPressure)+0.6;
|
||||||
|
altitudes.push(altitudeDelta);
|
||||||
|
FilteredAltitudes.push(kalmanFilter(altitudeDelta));
|
||||||
|
times.push(millis()-t4);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,52 @@
|
||||||
|
void recovery(){
|
||||||
|
|
||||||
|
if (timer == true && t1 >= (1000*EEPROM.read(1)+908)){ //Here the 908 ms correspond to the time covered by the circular buffer
|
||||||
|
servo1.write(EEPROM.read(3)); //Move servo1 to the final position EEPROM.read(3);
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
if (timer == true && t1 >= (1000*EEPROM.read(1)+908+2000)){ //We disable the servo after 2 seconds to save power
|
||||||
|
servo1.detach();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (t1 >= (1000*EEPROM.read(1) + 500*EEPROM.read(6) + 908 + 100)){ //The additional 100 ms is to prevent both servos from moving simultaneously.
|
||||||
|
servo2.write(EEPROM.read(5));
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
timer = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (t1 >= (1000*EEPROM.read(1) + 500*EEPROM.read(6) + 908 + 100 + 2000)){ //We disable the servo after 2 seconds to save power
|
||||||
|
servo2.detach();
|
||||||
|
timer = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (automatic == true){
|
||||||
|
if ((filteredAltitudeDelta - altold) < -0.01){
|
||||||
|
n++;
|
||||||
|
if (n == 4 && deploy == false){
|
||||||
|
deploy = true;
|
||||||
|
tconfig = t1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if ((filteredAltitudeDelta - altold) >= 0 && deploy == false){
|
||||||
|
n = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (deploy == true && (t1-tconfig) >= 500*EEPROM.read(0)){
|
||||||
|
servo1.write(EEPROM.read(3));
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
if (deploy == true && (t1-tconfig) >= (500*EEPROM.read(0) + 2000)){ //We disable the servo after 2 seconds to save power
|
||||||
|
servo1.detach();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (deploy == true && (t1-tconfig) >= (500*(EEPROM.read(0) + EEPROM.read(6)))){
|
||||||
|
servo2.write(EEPROM.read(5));
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
deploy = false;
|
||||||
|
if (deploy == true && (t1-tconfig) >= (500*(EEPROM.read(0) + EEPROM.read(6)) + 2000)){ //We disable the servo after 2 seconds to save power
|
||||||
|
servo2.detach();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,179 @@
|
||||||
|
// This program reads the rotary switch.
|
||||||
|
|
||||||
|
void readRotSwitch(){
|
||||||
|
for (int k = 0; k < 4; k++){
|
||||||
|
if (digitalRead(switchPins[k]) == LOW) {
|
||||||
|
bitSet(rotValue, k); //sets bit k to 1
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
bitClear(rotValue, k); //sets bit k to 0
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void switchStartup(){
|
||||||
|
readRotSwitch();
|
||||||
|
|
||||||
|
if (rotValue == 10){ //A Automatic mode
|
||||||
|
servo1.write(EEPROM.read(2)); //EEPROM.read(2)
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
|
||||||
|
delay(100); //It is important to have a delay to reduce the current spike drawn by the motors
|
||||||
|
servo2.write(EEPROM.read(4)); //EEPROM.read(4)
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
|
||||||
|
automatic = true;
|
||||||
|
delay(300);
|
||||||
|
blinkLED(EEPROM.read(0));
|
||||||
|
delay(500);
|
||||||
|
blinkLED(EEPROM.read(6));
|
||||||
|
servo1.detach(); //I detach the servos to save power
|
||||||
|
servo2.detach();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 11){ //B Timer mode
|
||||||
|
servo1.write(EEPROM.read(2)); //EEPROM.read(2)
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
|
||||||
|
delay(100); //It is important to have a delay to reduce the current spike drawn by the motors
|
||||||
|
servo2.write(EEPROM.read(4)); //EEPROM.read(4)
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
|
||||||
|
timer = true;
|
||||||
|
delay(300);
|
||||||
|
blinkLED(EEPROM.read(1));
|
||||||
|
delay(500);
|
||||||
|
blinkLED(EEPROM.read(6));
|
||||||
|
servo1.detach(); //I detach the servos to save power
|
||||||
|
servo2.detach();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 12){ //C, Configure the time for parachute deployment on automatic mode
|
||||||
|
while(1){
|
||||||
|
readRotSwitch();
|
||||||
|
blinkLED(1);
|
||||||
|
if (previousValue != rotValue){
|
||||||
|
EEPROM.write(0, rotValue);
|
||||||
|
EEPROM.commit();
|
||||||
|
previousValue = rotValue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 13){ //D, Configure the time for parachute deployment on timer mode.
|
||||||
|
while(1){
|
||||||
|
readRotSwitch();
|
||||||
|
blinkLED(1);
|
||||||
|
if (previousValue != rotValue){
|
||||||
|
EEPROM.write(1, rotValue);
|
||||||
|
EEPROM.commit();
|
||||||
|
previousValue = rotValue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
else if (rotValue == 14){ //E, Adjust servo's 1 initial possition
|
||||||
|
while(1){
|
||||||
|
readRotSwitch();
|
||||||
|
servo1.write(180*rotValue/15);
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
blinkLED(1);
|
||||||
|
if (previousValue != rotValue){
|
||||||
|
EEPROM.write(2, 180*rotValue/15);
|
||||||
|
EEPROM.commit();
|
||||||
|
}
|
||||||
|
previousValue == rotValue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 15){ //F, Adjust servo's 1 final possition
|
||||||
|
while(1){
|
||||||
|
readRotSwitch();
|
||||||
|
servo1.write(180*rotValue/15);
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
|
||||||
|
blinkLED(1);
|
||||||
|
if (previousValue != rotValue){
|
||||||
|
EEPROM.write(3, 180*rotValue/15);
|
||||||
|
EEPROM.commit();
|
||||||
|
}
|
||||||
|
previousValue == rotValue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 0){ //0, Adjust the servo's 2 initial possition
|
||||||
|
while(1){
|
||||||
|
readRotSwitch();
|
||||||
|
servo2.write(180*rotValue/15);
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
|
||||||
|
blinkLED(1);
|
||||||
|
if (previousValue != rotValue){
|
||||||
|
EEPROM.write(4, 180*rotValue/15);
|
||||||
|
EEPROM.commit();
|
||||||
|
}
|
||||||
|
previousValue == rotValue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 1){ //1, Adjust the servo's 2 final possition
|
||||||
|
while(1){
|
||||||
|
readRotSwitch();
|
||||||
|
servo2.write(180*rotValue/15);
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
|
||||||
|
blinkLED(1);
|
||||||
|
if (previousValue != rotValue){
|
||||||
|
EEPROM.write(5, 180*rotValue/15);
|
||||||
|
EEPROM.commit();
|
||||||
|
}
|
||||||
|
previousValue == rotValue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 2){ //2, Adjust the deploy time for servo 2 after servo 1
|
||||||
|
while(1){
|
||||||
|
readRotSwitch();
|
||||||
|
blinkLED(1);
|
||||||
|
if (previousValue != rotValue){
|
||||||
|
EEPROM.write(6, rotValue);
|
||||||
|
EEPROM.commit();
|
||||||
|
previousValue = rotValue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (rotValue == 3){ //3, Test the servo motors
|
||||||
|
servo1.write(EEPROM.read(2)); //EEPROM.read(2)
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
delay(500); //It is important to have a delay to reduce the current spike drawn by the motors
|
||||||
|
servo1.detach();
|
||||||
|
servo2.write(EEPROM.read(4)); //EEPROM.read(4)
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
delay(500);
|
||||||
|
servo2.detach();
|
||||||
|
blinkLED(2);
|
||||||
|
delay(2000);
|
||||||
|
servo1.write(EEPROM.read(3)); //Move servo1 to the final position EEPROM.read(3);
|
||||||
|
servo1.attach(servo1pin);
|
||||||
|
blinkLED(2);
|
||||||
|
delay(2000);
|
||||||
|
servo1.detach();
|
||||||
|
servo2.write(EEPROM.read(5));
|
||||||
|
servo2.attach(servo2pin);
|
||||||
|
delay(500);
|
||||||
|
servo2.detach();
|
||||||
|
while(1){
|
||||||
|
sleep_ms(100000);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
else {
|
||||||
|
while (true){
|
||||||
|
sleep_ms(10000);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
Ładowanie…
Reference in New Issue