hello guys
i written a rocket flight controller (it has been attached here) but there are some problems happened when i changed my windows . the code was working well but when i changed my windows its stop working and the Arduino compiler keeps showing errors that's related to the library of the sensors even though i use same library as before. now i decide to change the mpu9250 library which i had used before, to another one which is written by sparkfon but i don't now how i should change the code because the new library has a mahony filters?
note: i'm using teensy4.1 as controller
#include <MPU9250.h>
#include <Wire.h>
#include <Adafruit_BMP280.h>
#include <Servo.h>
#include<SPIFlash.h>
#include <SD.h>
#include <Adafruit_GPS.h>
#include <Bounce.h>
#define GPSSerial Serial3
#if defined(SIMBLEE)
#define BAUD_RATE 250000
#define RANDPIN 1
#else
#define BAUD_RATE 115200
#if defined(ARCH_STM32)
#define RANDPIN PA0
#else
#define RANDPIN A0
#endif
#endif
SPIFlash flash;
const uint32_t logStartAddr = 0x00000; // Starting address for the log in flash memory
const uint32_t logSize = 4096; // Size of the log (adjust as needed)
bfs::Mpu9250 imu;
Adafruit_BMP280 bmp(&Wire2);
Servo servoX;
Servo servoY;
bool flightState = false;
bool descentState = false;
bool parachuteDeployed = false;
bool welcomeMessagePrinted = false;
int testState = 0;
int prelaunchState = 0;
int launchState = 0;
int recoveryState = 0;
bool runTest();
bool deployParachute();
#define measuringbatteryvoltage 16
Bounce teststatePIN = Bounce(33, 50);
Bounce prelaunchstatePIN = Bounce(34, 50);
Bounce launchstatePIN = Bounce(35, 50);
int redPin = 38;
int greenPin = 39;
int bluePin = 40;
int buzzer = 7;
enum BuzzerState {
BUZZER_OFF,
BUZZER_ON,
BUZZER_DONE
};
BuzzerState buzzerState = BUZZER_OFF;
unsigned long lastBuzzerChangeTime = 0;
const unsigned long buzzerOnDuration = 300;
const int servoXPin = 8;
const int servoYPin = 9;
const int flashclearPin = 41;
bool buttonPressed = false;
float previousFilteredGyro = 0.0;
float previousFilteredAccel = 0.0;
float PARACHUTE_DEPLOYMENT_ALTITUDE_MIN;
float maxAltitude = 0;
float initialPressure;
float currentAltitude = 0.0;
float velocity = 0.0;
float integralPitchError = 0;
float integralRollError = 0;
float previousPitchError = 0;
float previousRollError = 0;
const float MAX_GYRO_X = 5.0;
const float MAX_GYRO_Y = 5.0;
const float MIN_ALTITUDE = 0.0;
const float MAX_ALTITUDE = 1000.0;
const float MIN_BATTERY_VOLTAGE = 11.0;
const float MAX_VOLTAGE = 13.0;
const float PARACHUTE_DEPLOYMENT_ALTITUDE = 5.0;
const float MAX_NOISE_VOLTAGE = 1.0;
const float DESCENT_VELOCITY_THRESHOLD = 1.0;
const int pyroPins[] = {19, 18, 37, 36};
const int numPins = sizeof(pyroPins) / sizeof(pyroPins[0]);
float voltageThreshold = 4.3; // Set the voltage threshold of pyro
int servoXAngle;
int servoYAngle;
void setup() {
Serial.begin(115200);
while (!Serial) {}
setupIMU();
setupBMP();
setupLED();
setupBuzzer();
setupPyroChannels();
setupServos();
setupFlashMemory();
pinMode(33, INPUT_PULLUP); // Test state pin
pinMode(34, INPUT_PULLUP); // Prelaunch pin
pinMode(35, INPUT_PULLUP); // Launch pin
// Initialize initial pressure
initialPressure = bmp.readPressure();// Measure at ground level
// Initial parachute deployment altitude limits
PARACHUTE_DEPLOYMENT_ALTITUDE_MIN = initialPressure - 25.0;// Adjust the value accordingly
// Initial servo angles
servoX.write(0);
servoY.write(0);
pinMode(buzzer, OUTPUT);
}
void setupIMU() {
Wire2.begin();
Wire2.setClock(400000);
imu.Config(&Wire2, bfs::Mpu9250::I2C_ADDR_PRIM);
if (!imu.Begin()) {
Serial.println("Error initializing communication with IMU");
while (1) {}
}
if (!imu.ConfigSrd(19)) {
Serial.println("Error configured SRD");
while (1) {}
}
}
void setupBMP() {
unsigned status;
status = bmp.begin();
if (!status) {
Serial.println(F("Could not find a valid BMP280 sensor, check wiring or try a different address!"));
Serial.print("SensorID was: 0x");
Serial.println(bmp.sensorID(), 16);
Serial.print("ID of 0xFF probably means a bad address, a BMP 180 or BMP 085\n");
Serial.print("ID of 0x56-0x58 represents a BMP 280,\n");
Serial.print("ID of 0x60 represents a BME 280.\n");
Serial.print("ID of 0x61 represents a BME 680.\n");
while (1) delay(10);
}
bmp.setSampling(Adafruit_BMP280::MODE_NORMAL, Adafruit_BMP280::SAMPLING_X2,
Adafruit_BMP280::SAMPLING_X16, Adafruit_BMP280::FILTER_X16,
Adafruit_BMP280::STANDBY_MS_500);
}
void setupLED() {
pinMode(redPin, OUTPUT);
pinMode(greenPin, OUTPUT);
pinMode(bluePin, OUTPUT);
}
void setupBuzzer() {
pinMode(buzzer, OUTPUT);
}
void setupPyroChannels() {
for (int i = 0; i < 4; i++) {
pinMode(18 + i, OUTPUT); // Pyro channels
digitalWrite(18 + i, LOW);
}
}
void setupServos() {
servoX.attach(servoXPin);
servoY.attach(servoYPin);
}
void setupFlashMemory() {
while (!Serial)
delay(100);
Serial.print(F("Initializing"));
for (uint8_t i = 0; i < 10; ++i) {
Serial.print(F("."));
}
Serial.println();
randomSeed(analogRead(RANDPIN));
// Setup the button pin
pinMode(flashclearPin, INPUT_PULLUP);
}
void loop() {
teststatePIN.update();
prelaunchstatePIN.update();
launchstatePIN.update();
// Clear flash memory if the button is pressed
bool currentButtonState = digitalRead(flashclearPin) == LOW;
// Check if the button state has changed
if (currentButtonState != buttonPressed) {
delay(50); // Debounce delay
currentButtonState = digitalRead(flashclearPin) == LOW;
if (currentButtonState) {
Serial.println("Clearing flash memory...");
clearFlashMemory();
delay(1000); // Add a delay to debounce the button
}
buttonPressed = currentButtonState;
}
if (teststatePIN.fallingEdge()) {
// Test state
testState = runTest();
prelaunchState = false;
launchState = false;
flightState = false;
testState = true;
descentState = false;
// Check if the welcome message has been printed
}else if (teststatePIN.risingEdge()) {
Serial.println("Hello, my name is H&FC01. I'm ready to be tested sir");
}
if (prelaunchstatePIN.fallingEdge()) {
// Prelaunch state
prelaunchState = true;
launchState = false;
flightState = false;
testState = false;
descentState = false;
// If everything is safe, transition to launch state
if (prelaunchstatePIN.risingEdge()) {
Serial.println("Ready to launch");
launchstateSetup();
prelaunchState = false;
} else {
Serial.println("Pre-launch failed");
}
delay(100); // Delay for stability
}
if (launchstatePIN.fallingEdge()) {
// Launch state
launchState = launchstateSetup();
prelaunchState = false;
launchState = true;
flightState = false;
descentState = false;
testState = false;
// If everything is safe, transition to flight state
if (launchstateSetup()) {
Serial.println("Transitioning to flight state.");
flightstateSetup();
digitalWrite(18, HIGH); // Assuming Pyro channel two is connected to pin 18
} else {
launchState = false;
Serial.println("launch failed");
delay(500); // Delay for stability
}
}
// Flight state
if (launchState) {
flightState = flightstateSetup();
prelaunchState = false;
launchState = false;
flightState = true;
descentState = false;
testState = false;
}
// Descent state
if (flightState) {
descentState = descentstateSetup();
prelaunchState = false;
launchState = false;
flightState = false;
descentState = true;
testState = false;
}
delay(100); // Delay for stability
}
bool runTest() {
readNoiseVoltage();
// Run tests for all capabilities
bool imuTest = testIMU();
bool bmpTest = testBMP();
bool servoTest = testServos();
bool pyroTest = testPyroChannels();
bool flashTest = testFlashMemory();
// Print the status of each test
Serial.print("IMU Test: ");
printTestResult(imuTest);
Serial.print("BMP Test: ");
printTestResult(bmpTest);
Serial.print("Servo Test: ");
printTestResult(servoTest);
Serial.print("Pyro Channels Test: ");
printTestResult(pyroTest);
Serial.print("Flash Memory Test: ");
printTestResult(flashTest);
// Save logs regardless of test results
saveLogsToFlash();
// Return true only if all tests passed
return imuTest && bmpTest && servoTest && pyroTest && flashTest;
}
void printTestResult(bool result) {
if (result) {
Serial.println("Passed");
} else {
Serial.println("Failed");
}
}
bool prelaunchStateSetup() {
readNoiseVoltage();
adjustFins();
calculateRollError();
calculatePitchError();
safetyChecks();
saveLogsToFlash();
ringBuzzer();
blueLED();
Serial.println("Performing prelaunch safety checks...");
// Check conditions for successful setup
bool successfulSetup = (calculatePitchError() == 0.0) &&
(calculateRollError() == 0.0) &&
(servoXAngle == 0) && // Assuming servoXAngle represents the angle of the servo controlling the fins
(servoYAngle == 0) && // Assuming servoYAngle represents the angle of the servo controlling the fins
(readNoiseVoltage() < 1.0) &&
(safetyChecks());
return successfulSetup;
}
bool launchstateSetup() {
readNoiseVoltage();
calculateRollError();
calculatePitchError();
adjustFins();
saveLogsToFlash();
ringBuzzer();
blueLED();
bool successfulSetup =(calculatePitchError() == 0.0) &&
(calculateRollError() == 0.0) &&
(servoXAngle == 0) && // Assuming servoXAngle represents the angle of the servo controlling the fins
(servoYAngle == 0) && // Assuming servoYAngle represents the angle of the servo controlling the fins
(readNoiseVoltage() < 1.0) &&
(safetyChecks());
return successfulSetup;
}
bool flightstateSetup() {
readNoiseVoltage();
calculateRollError();
calculatePitchError();
adjustFins();
saveLogsToFlash();
ringBuzzer();
blueLED();
// Check for apogee and transition to descent state
if (isApogee() && currentAltitude < PARACHUTE_DEPLOYMENT_ALTITUDE && velocity < DESCENT_VELOCITY_THRESHOLD) {
// Deploy parachute
Serial.println("Rocket reached apogee. Transitioning to descent state.");
descentstateSetup();
flightState = false;
return true; // Return true if setup is successful, false otherwise
} else {
return false;
}
}
bool descentstateSetup() {
checkParachuteDeployment(); // Check and deploy parachute if conditions are met
saveLogsToFlash();
ringBuzzer();
blueLED();
if (!checkParachuteDeployment()) { // Check parachute deployment again
} else {
deployParachute(); // Deploy parachute if conditions are met
}
if (!deployParachute()) { // Check if parachute deployment was successful
deployParachute();
return true; // Return true if setup is successful
} else {
return false; // Return false otherwise
}
}
bool deployParachute() {
// Add code to trigger the parachute deployment mechanism, e.g., firing a pyro channel
// For example, if the parachute is connected to pyro channel one:
digitalWrite(pyroPins[19], HIGH); // Assuming pyro channel one is connected to pyroPins[0]
// Assuming parachute deployment is successful
Serial.println("Parachute deployed!");
saveLogsToFlash();
greenled(); // Assuming you have a green LED function to turn on the green LED
ringBuzzer; // Assuming you have a function to produce a short buzzer sound
// Return true to indicate successful parachute deployment
return true;
}
void blueLED() {
digitalWrite(bluePin, LOW);
digitalWrite(greenPin, HIGH);
digitalWrite(redPin, HIGH);
}
void greenled() {
digitalWrite(bluePin, HIGH);
digitalWrite(greenPin, LOW);
digitalWrite(redPin, HIGH);
}
void redLED() {
digitalWrite(bluePin, HIGH);
digitalWrite(greenPin, HIGH);
digitalWrite(redPin, LOW);
}
bool testIMU() {
blueLED();
ringBuzzer();
if (printIMUData()) {
greenled();
} else {
redLED();
}
ringBuzzer();
return true; // Replace with actual test result
}
bool testBMP() {
blueLED();
ringBuzzer();
if (printBMPData()) {
greenled();
} else {
redLED();
}
ringBuzzer();
return true; // Replace with actual test result
}
bool testGPS() {
blueLED();
ringBuzzer();
if (processGPSData()) {
greenled();
} else {
redLED();
}
ringBuzzer();
return true; // Replace with actual test result
}
bool testServos() {
blueLED();
ringBuzzer();
if (adjustFins()) {
greenled();
} else {
redLED();
}
ringBuzzer();
return true; // Replace with actual test result
}
bool testPyroChannels() {
blueLED();
ringBuzzer();
for (int i = 0; i < numPins; ++i) {
int pyroPin = pyroPins[i];
// Apply voltage to the pyro pin
digitalWrite(pyroPin, HIGH);
delay(100); // Adjust delay if needed
// Read the voltage on the pyro pin
float voltage = analogRead(A1) * (MAX_VOLTAGE / 1023.0);
// Check if voltage is above the threshold
if (voltage >= voltageThreshold) {
Serial.println("Pyro channel_" + String(i + 1) + "_test true");
greenled();
} else {
Serial.println("Pyro channel_" + String(i + 1) + "_test failed. Voltage: " + String(voltage));
redLED();
}
// Turn off the pyro pin
digitalWrite(pyroPin, LOW);
delay(100); // Adjust delay if needed
}
ringBuzzer();
return true; // Replace with actual test result
}
bool testFlashMemory() {
blueLED();
ringBuzzer();
const char testLetter = 'A'; // Replace with the letter you want to write and clear
// Write the test letter to flash memory
writeLogToFlash(String(testLetter).c_str());
// Read the data from flash memory
char readData[sizeof(testLetter) + 1];
readLogFromFlash(readData, sizeof(readData));
if (strcmp(readData, String(testLetter).c_str()) == 0) {
greenled();
} else {
// Failed to write or read data
redLED();
ringBuzzer();
delay(500); // Adjust delay if needed
ringBuzzer();
Serial.println("flash_memory_test failed");
}
ringBuzzer();
return true; // Replace with actual test result
}
void readLogFromFlash(char* logData, size_t logSize) {
uint32_t logStartAddr = 0x00000; // Starting address for the log in flash memory
uint32_t logEndAddr = logStartAddr;
// Read the log data from flash memory
while (flash.readByte(logEndAddr) != 0xFF && logEndAddr < (logStartAddr + logSize - 1)) {
logEndAddr++;
}
// Ensure the data is null-terminated
logSize = min(logSize, logEndAddr - logStartAddr);
// Read multiple bytes at once using readAnything
flash.readAnything(logStartAddr, logData);
// Null-terminate the data
logData[logSize] = '\0';
}
float readBatteryVoltage() {
// Replace A0 with your actual analog pin used for measuring battery voltage
int sensorValue = analogRead(16);
// Replace 1023 with the maximum value your analog-to-digital converter can produce
float voltage = sensorValue * (MAX_VOLTAGE / 1023.0);
return voltage;
}
bool safetyChecks() {
// Check for sensor health
if (!imu.Read()) {
Serial.println("Error reading from IMU. Check sensor connections.");
// Implement appropriate actions (e.g., shut down motors, activate safety protocols)
redLED();
ringBuzzer();
delay(2000);
return false;
}
// Check for actuator saturation
if (abs(servoXAngle) > 180 || abs(servoYAngle) > 180) {
Serial.println("Actuator saturation detected. Adjust control parameters.");
// Implement appropriate actions (e.g., adjust control gains, activate safety protocols)
redLED();
ringBuzzer();
delay(2000);
return false;
}
// Check for abnormal sensor readings
if (isnan(imu.accel_x_mps2()) || isnan(imu.gyro_x_radps()) || isnan(bmp.readTemperature())) {
Serial.println("Abnormal sensor readings detected. Check sensor calibration.");
// Implement appropriate actions (e.g., recalibrate sensors, activate safety protocols)
redLED();
ringBuzzer();
delay(2000);
return false;
}
// Check for abnormal compass readings
if (isnan(imu.mag_x_ut()) || isnan(imu.mag_y_ut()) || isnan(imu.mag_z_ut())) {
Serial.println("Abnormal compass readings detected. Check compass calibration.");
// Implement appropriate actions (e.g., recalibrate compass, activate safety protocols)
redLED();
ringBuzzer();
delay(2000);
return false;
}
// Check for abnormal system behavior
if (abs(imu.gyro_x_radps()) > MAX_GYRO_X || abs(imu.gyro_y_radps()) > MAX_GYRO_Y) {
Serial.println("Abnormal gyro readings detected. Check for system instability.");
// Implement appropriate actions (e.g., adjust control gains, activate safety protocols)
redLED();
ringBuzzer();
delay(2000);
return false;
}
// Check for low battery voltage
float batteryVoltage = readBatteryVoltage(); // Implement a function to read the battery voltage
if (batteryVoltage < MIN_BATTERY_VOLTAGE) {
Serial.println("Low battery voltage. Prepare for landing.");
// Implement appropriate actions (e.g., initiate landing sequence, activate safety protocols)
redLED();
ringBuzzer();
delay(2000);
return false;
}
// Check for valid GPS data
if (!processGPSData()) {
Serial.println("Invalid GPS data. Check GPS module.");
// Implement appropriate actions (e.g., activate safety protocols)
redLED();
ringBuzzer();
delay(2000);
return false;
}
// Additional checks can be added as needed
// If all checks pass, turn on green LED and continue
greenled();
ringBuzzer();
return true;
}
void updateMaxAltitude() {
float currentAltitude = bmp.readAltitude(initialPressure);
if (currentAltitude > maxAltitude) {
maxAltitude = currentAltitude;
}
}
bool isApogee() {
// Read current altitude from the BMP280 sensor
float currentAltitude = bmp.readAltitude(initialPressure);
// Update the maximum altitude during flight
updateMaxAltitude();
// Check if the current altitude is less than the maximum altitude by 5 meters
if (maxAltitude - currentAltitude >= 5.0) {
// Apogee condition is met
return true;
} else {
// Apogee condition not met
return false;
}
}
bool checkParachuteDeployment() {
bool deploymentSuccessful = false;
if (launchstatePIN.fallingEdge()) {
float currentAltitude = bmp.readAltitude(initialPressure);
float velocity = calculateVerticalVelocity();
updateMaxAltitude();
if (currentAltitude > PARACHUTE_DEPLOYMENT_ALTITUDE_MIN &&
currentAltitude < maxAltitude - 5.0 &&
velocity < DESCENT_VELOCITY_THRESHOLD) {
if (!parachuteDeployed) {
if (isSafeToDeployParachute()) {
parachuteDeployed = true;
deploymentSuccessful = true;
} else {
Serial.println("Parachute deployment conditions not met. Safety check failed.");
}
}
} else {
parachuteDeployed = false;
}
}
return deploymentSuccessful;
}
bool isSafeToDeployParachute() {
float velocity = calculateVerticalVelocity(); // Calculate the vertical velocity
float noiseVoltage = readNoiseVoltage();
// Adjust safety conditions based on your specific requirements
if (noiseVoltage < MAX_NOISE_VOLTAGE && velocity < DESCENT_VELOCITY_THRESHOLD) {
Serial.println("Safe to deploy parachute.");
return true;
} else {
Serial.println("Unsafe to deploy parachute. High noise voltage or high descent velocity detected.");
return false;
}
}
float calculateVerticalVelocity() {
static float previousAltitude = 0.0;
static unsigned long previousTime = 0;
float currentAltitude = bmp.readAltitude(initialPressure); // Assuming bmp is an instance of Adafruit_BMP280
unsigned long currentTime = millis();
unsigned long deltaTime = currentTime - previousTime;
if (deltaTime > 0) {
float velocity = (currentAltitude - previousAltitude) / (float)deltaTime * 1000.0; // Convert to meters per second
previousAltitude = currentAltitude;
previousTime = currentTime;
return velocity;
} else {
return 0.0; // Avoid division by zero
}
}
float readNoiseVoltage() {
int sensorValue = analogRead(A1);
float voltage = sensorValue * (MAX_VOLTAGE / 1023.0);
return voltage;
}
bool printIMUData() {
if (imu.Read()) {
Serial.print(imu.new_imu_data());
Serial.print("\t");
Serial.print(imu.new_mag_data());
Serial.print("\t");
Serial.print(imu.accel_x_mps2());
Serial.print("\t");
Serial.print(imu.accel_y_mps2());
Serial.print("\t");
Serial.print(imu.accel_z_mps2());
Serial.print("\t");
Serial.print(imu.gyro_x_radps());
Serial.print("\t");
Serial.print(imu.gyro_y_radps());
Serial.print("\t");
Serial.print(imu.gyro_z_radps());
Serial.print("\t");
Serial.print(imu.mag_x_ut());
Serial.print("\t");
Serial.print(imu.mag_y_ut());
Serial.print("\t");
Serial.print(imu.mag_z_ut());
Serial.print("\t");
Serial.print(imu.die_temp_c());
Serial.print("\n");
return true; // Return true to indicate successful data printing
} else {
return false; // Return false if data reading fails
}
}
bool printBMPData() {
Serial.print(F("Temperature = "));
Serial.print(bmp.readTemperature());
Serial.println(" *C");
Serial.print(F("Pressure = "));
Serial.print(bmp.readPressure());
Serial.println(" Pa");
Serial.print(F("Approx altitude = "));
Serial.print(bmp.readAltitude(1019)); /* Adjusted to local forecast! */
Serial.println(" m");
// Assuming printing is successful
return true;
}
bool processGPSData() {
bool dataPrinted = false;
if (Serial3.available()) {
char c = Serial3.read();
Serial3.write(c);
dataPrinted = true;
}
if (Serial3.available()) {
char c = Serial3.read();
Serial.write(c);
delay(100);
dataPrinted = true;
}
return dataPrinted;
}
bool adjustFins() {
if (imu.Read()) {
float pitchError = calculatePitchError();
float rollError = calculateRollError();
float Kp = 1.0;
float Ki = 0.0;
float Kd = 0.0;
integralPitchError += pitchError;
integralRollError += rollError;
float derivativePitchError = pitchError - previousPitchError;
float derivativeRollError = rollError - previousRollError;
float integratorLimit = 50.0;
integralPitchError = constrain(integralPitchError, -integratorLimit, integratorLimit);
integralRollError = constrain(integralRollError, -integratorLimit, integratorLimit);
servoXAngle = map(Kp * pitchError + Ki * integralPitchError + Kd * derivativePitchError, -90, 90, 0, 180);
servoYAngle = map(Kp * rollError + Ki * integralRollError + Kd * derivativeRollError, -90, 90, 0, 180);
servoX.write(servoXAngle);
servoY.write(servoYAngle);
previousPitchError = pitchError;
previousRollError = rollError;
Serial.print("Pitch Error: ");
Serial.print(pitchError);
Serial.print("\tRoll Error: ");
Serial.println(rollError);
return true; // Return true to indicate the adjustment was performed
}
return false; // Return false if adjustment couldn't be performed
}
float calculatePitchError() {
return imu.gyro_x_radps() - imu.accel_x_mps2();
// Apply a simple low-pass filter to gyro and accelerometer data
float alpha = 0.9; // Filter coefficient (adjust as needed).Adjust the alpha parameter based on your system's dynamics and the desired filtering strength. Higher values of alpha result in stronger filtering but slower response to changes
float filteredGyro = alpha * imu.gyro_x_radps() + (1 - alpha) * previousFilteredGyro;
float filteredAccel = alpha * imu.accel_x_mps2() + (1 - alpha) * previousFilteredAccel;
previousFilteredGyro = filteredGyro;
previousFilteredAccel = filteredAccel;
return filteredGyro - filteredAccel;
}
float calculateRollError() {
return imu.gyro_y_radps() - imu.accel_y_mps2();
// Apply a simple low-pass filter to gyro and accelerometer data
float alpha = 0.9; // Filter coefficient (adjust as needed).Adjust the alpha parameter based on your system's dynamics and the desired filtering strength. Higher values of alpha result in stronger filtering but slower response to changes
float filteredGyro = alpha * imu.gyro_y_radps() + (1 - alpha) * previousFilteredGyro;
float filteredAccel = alpha * imu.accel_y_mps2() + (1 - alpha) * previousFilteredAccel;
previousFilteredGyro = filteredGyro;
previousFilteredAccel = filteredAccel;
return filteredGyro - filteredAccel;
}
void saveLogsToFlash() {
// Replace this with your actual log data
const char* logData = "Sample Log Data. Replace this with your actual log.";
// Call the function to write log to flash
writeLogToFlash(logData);
// Optionally, print a message to Serial indicating successful log save
Serial.println("Logs saved to flash memory.");
}
// New function to clear flash memory
void clearFlashMemory() {
Serial.println("Clearing Flash Memory...");
flash.eraseSector(logStartAddr);
Serial.println("Flash Memory Cleared.");
}
// Modified function to write log to flash
void writeLogToFlash(const char* logData) {
uint32_t logEndAddr = logStartAddr;
while (flash.readByte(logEndAddr) != 0xFF && logEndAddr < (logStartAddr + logSize)) {
logEndAddr++;
}
if (logEndAddr >= (logStartAddr + logSize)) {
Serial.println("Log is full. Unable to write.");
return;
}
flash.writeByte(logEndAddr, (const uint8_t*)logData, strlen(logData));
}
void ringBuzzer() {
switch (buzzerState) {
case BUZZER_OFF:
// Buzzer is currently off
noTone(buzzer);
if (millis() - lastBuzzerChangeTime >= 1000) {
// Wait for 1 second before turning the buzzer on
buzzerState = BUZZER_ON;
lastBuzzerChangeTime = millis();
}
break;
case BUZZER_ON:
// Buzzer is currently on
tone(buzzer, 1000); // Send 1KHz sound signal
if (millis() - lastBuzzerChangeTime >= buzzerOnDuration) {
// Wait for the specified duration before turning the buzzer off
buzzerState = BUZZER_DONE;
lastBuzzerChangeTime = millis();
}
break;
case BUZZER_DONE:
// Buzzer is done ringing
noTone(buzzer);
// You can add additional logic or tasks here
break;
}
}