Skip to content

Commit

Permalink
Launch Ready Code
Browse files Browse the repository at this point in the history
  • Loading branch information
@DrewBrandt
DrewBrandt committed Apr 21, 2024
1 parent 505ac7c commit a0d998f
Show file tree
Hide file tree
Showing 3 changed files with 52 additions and 24 deletions.
2 changes: 1 addition & 1 deletion Spaceport/23-24/Code/Teensy-Based Avionics/src/MAX_M10S.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ void MAX_M10S::update()
origin.y() = pos.y();
origin.z() = altitude;

for (int i = 0; i < 20; i++)
for (int i = 0; i < 11; i++)
{
prevReadings[i] = origin;
}
Expand Down
37 changes: 23 additions & 14 deletions Spaceport/23-24/Code/Teensy-Based Avionics/src/State.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,7 @@ bool State::init()
radio = nullptr;
}
setCsvHeader();
if(useKF)
if (useKF)
kfilter = initializeFilter();
numSensors = good;

Expand Down Expand Up @@ -138,7 +138,7 @@ void State::updateSensors()

void State::updateState(double newTimeAbsolute)
{
if (stageNumber > 4 && landingCounter > 50) // if landed and waited 5 seconds, don't update sensors.
if (stageNumber > 4 && landingCounter > 300) // if landed and waited 5 seconds, don't update sensors.
return;

lastTimeAbsolute = timeAbsolute;
Expand All @@ -150,8 +150,8 @@ void State::updateState(double newTimeAbsolute)
updateSensors();
if (kfilter && sensorOK(gps) && gps->getHasFirstFix() && stageNumber > 0)
{
measurements = new double[3]{0, 0, 0};
inputs = new double[3]{0, 0, 0};
measurements = new double[3]{0, 0, 0};
inputs = new double[3]{0, 0, 0};
// gps x y z barometer z
measurements[0] = gps->getDisplace().x();
measurements[1] = gps->getDisplace().y();
Expand Down Expand Up @@ -211,7 +211,12 @@ void State::updateState(double newTimeAbsolute)
orientation = imu->getOrientation();
}
}
timeSinceLaunch = timeAbsolute - timeOfLaunch;

if (stageNumber == 0)
timeSinceLaunch = 0;
else
timeSinceLaunch = timeAbsolute - timeOfLaunch;

determineAccelerationMagnitude();
determineStage();
if (stageNumber > 0)
Expand All @@ -232,7 +237,6 @@ void State::updateState(double newTimeAbsolute)
setDataString();
if (recordOwnFlightData)
recordFlightData(dataString);

}

void State::setCsvHeader()
Expand Down Expand Up @@ -361,8 +365,8 @@ void State::determineStage()
{
if (stageNumber == 0 &&
(sensorOK(imu) || sensorOK(baro)) &&
(sensorOK(imu) ? imu->getAcceleration().z() > 15 : true) &&
(sensorOK(baro) ? baro->getRelAltFt() > 5 : true))
(sensorOK(imu) ? abs(imu->getAcceleration().z()) > 25 : true) &&
(sensorOK(baro) ? baro->getRelAltFt() > 60 : true))
// if we are in preflight AND
// we have either the IMU OR the barometer AND
// imu is ok AND the z acceleration is greater than 29 ft/s^2 OR imu is not ok AND
Expand All @@ -375,6 +379,7 @@ void State::determineStage()
stageNumber = 1;
timeOfLaunch = timeAbsolute;
timePreviousStage = timeAbsolute;
timeSinceLaunch = 0;
strcpy(launchTimeOfDay, gps->getTimeOfDay());
recordLogData(INFO, "Launch detected.");
recordLogData(INFO, "Printing static data.");
Expand All @@ -389,36 +394,40 @@ void State::determineStage()
}
}
} // TODO: Add checks for each sensor being ok and decide what to do if they aren't.
else if (stageNumber == 1 && acceleration.z() < 10 && timeSinceLaunch > 10)
else if (stageNumber == 1 && abs(acceleration.z()) < 10)
{
bb.aonoff(33, 200, 2);
timePreviousStage = timeAbsolute;
stageNumber = 2;
recordLogData(INFO, "Coasting detected.");
}
else if (stageNumber == 2 && baroVelocity <= 0 && timeSinceLaunch > 20)
else if (stageNumber == 2 && baroVelocity <= 0 && timeSinceLaunch > 5)
{
bb.aonoff(33, 200, 3);
char logData[100];
snprintf(logData, 100, "Apogee detected at %.2f m.", position.z());
recordLogData(INFO, logData);
timePreviousStage = timeAbsolute;
stageNumber = 3;
recordLogData(INFO, "Drogue conditions detected.");
}
else if (stageNumber == 3 && baro->getRelAltFt() < 1000 && timeSinceLaunch > 30)
else if (stageNumber == 3 && baro->getRelAltFt() < 1000 && timeSinceLaunch > 10)
{
bb.aonoff(33, 200, 4);
stageNumber = 4;
timePreviousStage = timeAbsolute;
recordLogData(INFO, "Main parachute conditions detected.");
}
else if (stageNumber == 4 && baroVelocity > -1 && baro->getRelAltFt() < 66 && timeSinceLaunch > 40)
else if (stageNumber == 4 && baroVelocity > -1 && baro->getRelAltFt() < 66 && timeSinceLaunch > 15)
{
bb.aonoff(33, 200, 5);
timePreviousStage = timeAbsolute;
stageNumber = 5;
recordLogData(INFO, "Landing detected. Waiting for 5 seconds to dump data.");
}
else if (stageNumber == 5 && timeSinceLaunch > 50)
else if (stageNumber == 5 && timeSinceLaunch > 20)
{
if (landingCounter++ >= 50)
if (landingCounter++ >= 300)
{ // roughly 5 seconds of data after landing
setRecordMode(GROUND);
recordLogData(INFO, "Dumped data after landing.");
Expand Down
37 changes: 28 additions & 9 deletions Spaceport/23-24/Code/Teensy-Based Avionics/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ BMP390 bmp(13, 12); // I2C Address 0x77
MAX_M10S gps(13, 12, 0x42); // I2C Address 0x42
DS3231 rtc(); // I2C Address 0x68
APRSConfig config = {"KC3UTM", "APRS", "WIDE1-1", '[', '/'};
RadioSettings settings = {915.0, true, false, &hardware_spi, 10, 31, 32};
RadioSettings settings = {433.775, true, false, &hardware_spi, 10, 31, 32};
RFM69HCW radio = {settings, config};
State computer; // = useKalmanFilter = true, stateRecordsOwnData = true
uint32_t radioTimer = millis();
Expand All @@ -27,7 +27,7 @@ PSRAM *ram;

static double last = 0; // for better timing than "delay(100)"

//BlinkBuzz setup
// BlinkBuzz setup
int allowedPins[] = {LED_BUILTIN, BUZZER};
BlinkBuzz bb(allowedPins, 2, true);
extern unsigned long _heap_start;
Expand All @@ -47,14 +47,13 @@ void setup()

pinMode(LED_BUILTIN, OUTPUT);
pinMode(BUZZER, OUTPUT); // its very loud during testing
bb.onoff(BUZZER, 200, 3, 100);
bb.onoff(BUZZER, 100, 4, 100);
recordLogData(INFO, "Initializing Avionics System. 5 second delay to prevent unnecessary file generation.", TO_USB);
delay(5000);

pinMode(BMP_ADDR_PIN, OUTPUT);
digitalWrite(BMP_ADDR_PIN, HIGH);


pinMode(RPI_PWR, OUTPUT); // RASPBERRY PI TURN ON
pinMode(RPI_VIDEO, OUTPUT); // RASPBERRY PI TURN ON

Expand Down Expand Up @@ -105,19 +104,21 @@ void setup()
sendSDCardHeader(computer.getCsvHeader());
}
static bool more = false;
static bool first = false;
static bool second = false;
void loop()
{
bb.update();
double time = millis();

if (time - radioTimer >= 500)
{
//more = computer.transmit();
more = computer.transmit();
radioTimer = time;
}
if (radio.mode() != RHGenericDriver::RHModeTx && more)
{
//more = !radio.sendBuffer();
more = !radio.sendBuffer();
}
if (time - last < 100)
return;
Expand All @@ -126,13 +127,31 @@ void loop()
computer.updateState();
recordLogData(INFO, computer.getStateString(), TO_USB);
// RASPBERRY PI TURN ON
if (time / 1000.0 > 600)
if (time / 1000.0 > 810)
{
digitalWrite(RPI_PWR, HIGH);
if (!first)
{
bb.aonoff(BUZZER, 100, 2);
first = true;
}
}
if (computer.getStageNum() == 1)
if (computer.getStageNum() >= 1)
{
digitalWrite(RPI_VIDEO, LOW);
if (!second)
{
bb.aonoff(BUZZER, 100, 3);
second = true;
}
}
// if (time / 1000.0 > 180)
// {
// digitalWrite(RPI_VIDEO, LOW);
// if (!second)
// {
// bb.aonoff(BUZZER, 100, 3);
// second = true;
// }
// }
}

0 comments on commit a0d998f

Please sign in to comment.