New Version 1.2

arduino-quick-upload

@@ -0,0 +1,45 @@
+#!/bin/bash
+
+source "arduino-ports-enable"
+
+
+# Check if a command-line argument is provided and if the path exists
+if [ "$#" -eq 1 ] && [ -d "$1" ]; then
+    project_path="$1"
+else
+    # Prompt the user for the project path
+    read -p "Enter the path to your Arduino project directory: " project_path
+
+    # Validate the project path
+    while [ ! -d "$project_path" ]; do
+        echo "Invalid path or path does not exist."
+        read -p "Please enter a valid path to your Arduino project DIRECTORY: " project_path
+    done
+fi
+
+# Get the list of connected Arduinos
+board_list=$(arduino-cli board list)
+
+# Count the number of connected Arduinos
+num_arduinos=$(echo "$board_list" | grep -c "Arduino")
+
+if [ "$num_arduinos" -eq 0 ]; then
+    echo "No Arduino boards found."
+    exit 1
+elif [ "$num_arduinos" -eq 1 ]; then
+    # If only one Arduino is connected, use it
+    arduino_port=$(echo "$board_list" | grep -oE '/dev/ttyACM[0-9]+')
+    board_type=$(echo "$board_list"| grep "$arduino_port" | awk '{print $(NF-1)}')
+else
+    # If multiple Arduinos are connected, ask the user to select one
+    echo "Multiple Arduino boards found:"
+    echo "$board_list" | tail -n +2 | awk '{print $1, $NF}'
+    read -p "Please enter the port of the Arduino you want to upload to: " arduino_port
+    board_type=$(echo "$board_list" | grep "$arduino_port" | awk '{print $(NF-1)}'
+)
+fi
+
+# Compile and upload the project to the selected Arduino
+arduino-cli compile -b "$board_type" "$project_path"
+arduino-cli upload ./ -p "$arduino_port" -b "$board_type"
+

sender_module/DEV_Config.cpp

@@ -0,0 +1,94 @@
+/*****************************************************************************
+* | File        :   DEV_Config.c
+* | Author      :   Waveshare team
+* | Function    :   Hardware underlying interface
+* | Info        :
+*                Used to shield the underlying layers of each master 
+*                and enhance portability
+*----------------
+* | This version:   V1.0
+* | Date        :   2018-11-22
+* | Info        :
+
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documnetation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to  whom the Software is
+# furished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS OR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+#
+******************************************************************************/
+#include "DEV_Config.h"
+SoftwareSerial mySerial(0, 1); // RX, TX
+
+/******************************************************************************
+function: 
+  Uart receiving and sending
+******************************************************************************/
+UBYTE DEV_Uart_ReceiveByte()
+{
+  while(1){
+    if(mySerial.available()){
+      return  mySerial.read();
+    }
+  }
+}
+
+void DEV_Uart_SendByte(char data)
+{
+  mySerial.write(data);
+}
+
+void DEV_Uart_SendString(char *data)
+{
+  UWORD i;
+  for(i=0; data[i] != '\0'; i++){
+    mySerial.write(data[i]);
+  }
+}
+
+void DEV_Uart_ReceiveString(char *data, UWORD Num)
+{  
+  UWORD i;
+  while(1){
+    if(mySerial.available()){
+        data[i] =  mySerial.read();
+        i++;
+        //Serial.print(data[i]);
+        if(i >= Num){
+            break;
+          }
+      }
+   }
+
+  data[Num-1] = '\0';
+}
+
+void DEV_Set_GPIOMode(UWORD Pin, UWORD mode)
+{
+
+  if(mode == 1){
+    pinMode(Pin, INPUT);
+  }
+  else if(mode == 0){
+    pinMode(Pin, OUTPUT);
+  }
+}
+
+
+void DEV_Set_Baudrate(UDOUBLE Baudrate)
+{
+  mySerial.begin(Baudrate);
+}
+

sender_module/DEV_Config.h

@@ -0,0 +1,42 @@
+#ifndef _DEV_CONFIG_H_
+#define _DEV_CONFIG_H_
+
+#include <SoftwareSerial.h>
+#include <avr/pgmspace.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <SPI.h>
+
+#define UBYTE   uint8_t
+#define UWORD   uint16_t
+#define UDOUBLE uint32_t
+
+/**
+ * GPIO config
+**/
+#define DEV_FORCE 4
+#define DEV_STANDBY 5
+
+/**
+ * GPIO read and write
+**/
+#define DEV_Digital_Write(_pin, _value) digitalWrite(_pin, _value == 0? LOW:HIGH)
+#define DEV_Digital_Read(_pin) digitalRead(_pin)
+
+/**
+ * delay x ms
+**/
+#define DEV_Delay_ms(__xms)    delay(__xms)
+
+
+/*-----------------------------------------------------------------------------*/
+UBYTE DEV_Uart_ReceiveByte(void);
+void DEV_Uart_SendByte(char data);
+void DEV_Uart_SendString(char *data);
+void DEV_Uart_ReceiveString(char *data, UWORD Num);
+
+void DEV_Set_Baudrate(UDOUBLE Baudrate);
+
+void DEV_Set_GPIOMode(UWORD Pin, UWORD mode);
+#endif
+

sender_module/L76X.cpp

@@ -0,0 +1,244 @@
+#include "L76X.h"
+
+char const Temp[16]={'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'};
+
+static const double pi = 3.14159265358979324;
+static const double a = 6378245.0;
+static const double ee = 0.00669342162296594323;
+static const double x_pi = 3.14159265358979324 * 3000.0 / 180.0;
+
+static char buff_t[BUFFSIZE]={0};
+
+
+
+GNRMC GPS;
+
+/******************************************************************************
+function:	
+	Latitude conversion
+******************************************************************************/
+static double transformLat(double x,double y)
+{
+	double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 *sqrt(abs(x));
+    ret += (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0;
+    ret += (20.0 * sin(y * pi) + 40.0 * sin(y / 3.0 * pi)) * 2.0 / 3.0;
+    ret += (160.0 * sin(y / 12.0 * pi) + 320 * sin(y * pi / 30.0)) * 2.0 / 3.0;
+    return ret;
+}
+
+/******************************************************************************
+function:	
+	Longitude conversion
+******************************************************************************/
+static double transformLon(double x,double y)
+{
+	double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * sqrt(abs(x));
+    ret += (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0;
+    ret += (20.0 * sin(x * pi) + 40.0 * sin(x / 3.0 * pi)) * 2.0 / 3.0;
+    ret += (150.0 * sin(x / 12.0 * pi) + 300.0 * sin(x / 30.0 * pi)) * 2.0 / 3.0;
+    return ret;
+}
+
+/******************************************************************************
+function:	
+	GCJ-02 international standard converted to Baidu map BD-09 standard
+******************************************************************************/
+static Coordinates bd_encrypt(Coordinates gg)
+{
+	Coordinates bd;
+    double x = gg.Lon, y = gg.Lat;
+	double z = sqrt(x * x + y * y) + 0.00002 * sin(y * x_pi);
+	double theta = atan2(y, x) + 0.000003 * cos(x * x_pi);
+	bd.Lon = z * cos(theta) + 0.0065;
+	bd.Lat = z * sin(theta) + 0.006;
+	return bd;
+}
+
+/******************************************************************************
+function:	
+	GPS's WGS-84 standard is converted into GCJ-02 international standard
+******************************************************************************/
+static Coordinates transform(Coordinates gps)
+{
+	Coordinates gg;
+    double dLat = transformLat(gps.Lon - 105.0, gps.Lat - 35.0);
+    double dLon = transformLon(gps.Lon - 105.0, gps.Lat - 35.0);
+    double radLat = gps.Lat / 180.0 * pi;
+    double magic = sin(radLat);
+    magic = 1 - ee * magic * magic;
+    double sqrtMagic = sqrt(magic);
+    dLat = (dLat * 180.0) / ((a * (1 - ee)) / (magic * sqrtMagic) * pi);
+    dLon = (dLon * 180.0) / (a / sqrtMagic * cos(radLat) * pi);
+    gg.Lat = gps.Lat + dLat;
+    gg.Lon = gps.Lon + dLon;
+	return gg;
+}
+
+/******************************************************************************
+function:	
+	Send a command to the L76X，Automatic calculation of the code
+parameter:
+    data ：The end of the command ends with ‘\0’ or it will go wrong, 
+           no need to increase the validation code.
+******************************************************************************/
+void L76X_Send_Command(char *data)
+{
+    char Check = data[1], Check_char[3]={0};
+    UBYTE i = 0;
+    DEV_Uart_SendByte('\r');
+    DEV_Uart_SendByte('\n');
+    
+    //printf(" 1i = %d Check =%x \n", i, Check);
+    for(i=2; data[i] != '\0'; i++){
+        Check ^= data[i];       //Calculate the check value
+    }
+    //printf(" i = %d Check =%x \n", i, Check);
+    Check_char[0] = Temp[Check/16%16];
+    Check_char[1] = Temp[Check%16];
+		Check_char[2] = '\0';
+   
+
+    DEV_Uart_SendString(data);
+    DEV_Uart_SendByte('*');
+    DEV_Uart_SendString(Check_char);
+		DEV_Uart_SendByte('\r');
+		DEV_Uart_SendByte('\n');
+}
+
+void L76X_Exit_BackupMode()
+{
+    DEV_Set_GPIOMode(DEV_FORCE, 0);
+    
+    DEV_Digital_Write(DEV_FORCE, 1);
+    DEV_Delay_ms(1000);
+    DEV_Digital_Write(DEV_FORCE, 0);
+    
+    DEV_Set_GPIOMode(DEV_FORCE, 1);
+}
+
+/******************************************************************************
+function:	
+	Analyze GNRMC data in L76x, latitude and longitude, time
+******************************************************************************/
+GNRMC L76X_Gat_GNRMC()
+{
+    UWORD add = 0, x = 0, z = 0, i = 0;
+    UDOUBLE Time = 0, latitude = 0, longitude = 0;
+
+    GPS.Status = 0;
+
+		GPS.Time_H = 0;
+    GPS.Time_M = 0;
+    GPS.Time_S = 0;
+	
+    DEV_Uart_ReceiveString(buff_t, BUFFSIZE);
+     Serial.print(buff_t);
+    add = 0; 
+    while(add < BUFFSIZE){
+        if(buff_t[add] == '$' && buff_t[add+1] == 'G' && (buff_t[add+2] == 'N' || buff_t[add+2] == 'P')\
+            && buff_t[add+3] == 'R' && buff_t[add+4] == 'M' && buff_t[add+5] == 'C'){
+            x = 0;
+            for(z = 0; x < 12; z++){
+                if(buff_t[add+z]=='\0'){
+                    break;
+                }
+                if(buff_t[add+z]==','){
+                    x++;
+                    if(x == 1){//The first comma is followed by time
+                        Time = 0;
+                        for(i = 0; buff_t[add+z+i+1] != '.'; i++){
+                            if(buff_t[add+z+i+1]=='\0'){
+                                break;
+                            }   
+                            if(buff_t[add+z+i+1] == ',')
+                                break;
+                            Time = (buff_t[add+z+i+1]-'0') + Time*10;
+                        }
+                        
+                        GPS.Time_H = Time/10000+8;
+                        GPS.Time_M = Time/100%100;
+                        GPS.Time_S = Time%100;
+												if(GPS.Time_H >= 24)
+													GPS.Time_H = GPS.Time_H - 24;
+                    }else if(x == 2){
+                     //A indicates that it has been positioned
+                     //V indicates that there is no positioning.
+                        if(buff_t[add+z+1] == 'A'){
+                             GPS.Status = 1;
+                        }else{
+                             GPS.Status = 0;
+                        }
+                    }else if(x == 3){
+                        latitude = 0;
+                        //If you need to modify, please re-edit the calculation method below.
+                        for(i = 0; buff_t[add+z+i+1] != ','; i++){
+                            if(buff_t[add+z+i+1] == '\0'){
+                                break;
+                            } 
+                            if(buff_t[add+z+i+1] == '.'){
+                                continue;
+                            }
+                            latitude = (buff_t[add+z+i+1]-'0') + latitude*10;
+                        }
+                        GPS.Lat = latitude/10000000.0;
+                    }else if(x == 4){
+                        GPS.Lat_area = buff_t[add+z+1];
+                    }
+                    else if(x == 5){
+                        longitude = 0;
+                        for(i = 0; buff_t[add+z+i+1] != ','; i++){
+                            if(buff_t[add+z+i+1] == '\0'){
+                                break;
+                            } 
+                            if(buff_t[add+z+i+1] == '.')
+                                continue;
+                            longitude = (buff_t[add+z+i+1]-'0') + longitude*10;
+                        }
+                        GPS.Lon = longitude/10000000.0;
+                    }else if(x == 6){
+                        GPS.Lon_area = buff_t[add+z+1];
+                    }
+                }
+            }
+            add = 0;
+            break;
+        }
+        if(buff_t[add+5] == '\0'){
+            add = 0;
+						break;
+        }
+        add++;
+        if(add > BUFFSIZE){
+            add = 0;
+            break;
+        }
+    }
+    return GPS;
+}
+
+/******************************************************************************
+function:	
+	Convert GPS latitude and longitude into Baidu map coordinates
+******************************************************************************/
+Coordinates L76X_Baidu_Coordinates()
+{
+    Coordinates temp;
+    temp.Lat =((int)(GPS.Lat)) + (GPS.Lat - ((int)(GPS.Lat)))*100 / 60;
+    temp.Lon =((int)(GPS.Lon)) + (GPS.Lon - ((int)(GPS.Lon)))*100 / 60;
+    temp = transform(temp);
+    temp = bd_encrypt(temp);
+    return temp;
+}
+
+/******************************************************************************
+function:	
+	Convert GPS latitude and longitude into Google Maps coordinates
+******************************************************************************/
+Coordinates L76X_Google_Coordinates()
+{
+    Coordinates temp;
+    GPS.Lat =((int)(GPS.Lat)) + (GPS.Lat - ((int)(GPS.Lat)))*100 / 60;
+    GPS.Lon =((int)(GPS.Lon)) + (GPS.Lon - ((int)(GPS.Lon)))*100 / 60;
+    temp = transform(temp);
+    return temp;
+}

sender_module/L76X.h

@@ -0,0 +1,83 @@
+#ifndef _L76X_H_
+#define _L76X_H_
+
+#include "DEV_Config.h"
+#include <math.h>
+#include <stdlib.h>
+
+
+
+#define BUFFSIZE 800
+
+//Startup mode
+#define HOT_START       "$PMTK101"
+#define WARM_START      "$PMTK102"
+#define COLD_START      "$PMTK103"
+#define FULL_COLD_START "$PMTK104"
+
+//Standby mode -- Exit requires high level trigger
+#define SET_PERPETUAL_STANDBY_MODE      "$PMTK161"
+
+#define SET_PERIODIC_MODE               "$PMTK225"
+#define SET_NORMAL_MODE                 "$PMTK225,0"
+#define SET_PERIODIC_BACKUP_MODE        "$PMTK225,1,1000,2000"
+#define SET_PERIODIC_STANDBY_MODE       "$PMTK225,2,1000,2000"
+#define SET_PERPETUAL_BACKUP_MODE       "$PMTK225,4"
+#define SET_ALWAYSLOCATE_STANDBY_MODE   "$PMTK225,8"
+#define SET_ALWAYSLOCATE_BACKUP_MODE    "$PMTK225,9"
+
+//Set the message interval,100ms~10000ms
+#define SET_POS_FIX         "$PMTK220"
+#define SET_POS_FIX_100MS   "$PMTK220,100"
+#define SET_POS_FIX_200MS   "$PMTK220,200"
+#define SET_POS_FIX_400MS   "$PMTK220,400"
+#define SET_POS_FIX_800MS   "$PMTK220,800"
+#define SET_POS_FIX_1S      "$PMTK220,1000"
+#define SET_POS_FIX_2S      "$PMTK220,2000"
+#define SET_POS_FIX_4S      "$PMTK220,4000"
+#define SET_POS_FIX_8S      "$PMTK220,8000"
+#define SET_POS_FIX_10S     "$PMTK220,10000"
+
+//Switching time output
+#define SET_SYNC_PPS_NMEA_OFF   "$PMTK255,0"
+#define SET_SYNC_PPS_NMEA_ON    "$PMTK255,1"
+
+//Baud rate
+#define SET_NMEA_BAUDRATE           "$PMTK251"
+#define SET_NMEA_BAUDRATE_115200    "$PMTK251,115200"
+#define SET_NMEA_BAUDRATE_57600     "$PMTK251,57600"
+#define SET_NMEA_BAUDRATE_38400     "$PMTK251,38400"
+#define SET_NMEA_BAUDRATE_19200     "$PMTK251,19200"
+#define SET_NMEA_BAUDRATE_14400     "$PMTK251,14400"
+#define SET_NMEA_BAUDRATE_9600      "$PMTK251,9600"
+#define SET_NMEA_BAUDRATE_4800      "$PMTK251,4800"
+
+//To restore the system default setting
+#define SET_REDUCTION               "$PMTK314,-1"
+
+//Set NMEA sentence output frequencies 
+#define SET_NMEA_OUTPUT "$PMTK314,0,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0"
+
+typedef struct {
+	double Lon;     //GPS Latitude and longitude
+	double Lat;
+    char Lon_area;
+    char Lat_area;
+    UBYTE Time_H;   //Time
+    UBYTE Time_M;
+    UBYTE Time_S;
+    UBYTE Status;   //1:Successful positioning 0：Positioning failed
+}GNRMC;
+
+typedef struct {
+    double Lon;
+    double Lat;
+}Coordinates;
+
+void L76X_Send_Command(char *data);
+Coordinates L76X_Baidu_Coordinates(void);
+Coordinates L76X_Google_Coordinates(void);
+GNRMC L76X_Gat_GNRMC(void);
+void L76X_Exit_BackupMode(void);
+
+#endif

sender_module/README.md

@@ -3,7 +3,7 @@
 ## Stages of flight
 
 1. Preflight - Remove before flight button or smthing like that
-    - no data sending, blocked parachute ejection, all systems down
+    - no communication, blocked parachute ejection, all systems down
 2. Ready - Signal from groundstation
     - sys check (modules calibration, battery check, atd...), blocked parachute ejection, wait for launch pin removed, only send data, do not save them
 3. Launch - Removed pin from the rocket
@@ -11,7 +11,7 @@
 4. Apogee - Detected that rocket is in apogee with accelerometer
     - parachute ejection, all systems working, gps check and height check
 5. Return - Rocket has no velocity
-    - all unneeded systems shutdown/sleep, buzz on, gps sending location, battery check, turn off gyro and accelerometer
+    - all not needed systems shutdown/sleep, buzz on, gps sending location, battery check, turn off gyro and accelerometer
 
 ## Modules
 
@@ -26,3 +26,7 @@
 
 - Ejection mechanism
 - PCB for our computer
+- force parachute ejection
+- if cable connected return back to ready stage
+- send signal of listening
+- wait for recieve

sender_module/sender_module.ino

@@ -1,85 +1,236 @@
+#include <SoftwareSerial.h>
 #include <SPI.h>
+#include <SD.h>
+
 #include <nRF24L01.h>
 #include <RF24.h>
+#include <Adafruit_INA219.h>
+
+#include "DEV_Config.h"
+#include "L76X.h"
 #include "Waveshare_10Dof-D.h"
 
-//----------------------- CONSTANTS -----------------------//
+// Define pin numbers for modules
-bool gbSenserConnectState = false;
+const int NRF_CE_PIN = 9;
-RF24 radio(9, 8);                 // CE, CSN
+const int NRF_CS_PIN = 8;
-const byte address[6] = "00001";  // address through which two modules communicate
+const byte address[6] = "00001";
+
+const int GPS_TX_PIN = 0;
+const int GPS_RX_PIN = 1;
+
+//const String INA219_SDA_PIN = "A4";
+//const String INA219_SCL_PIN = "A5";
+
+const int BUZZER_PIN = 7;
+const int READY_STAGE_PIN = 5;
+const int LAUNCH_STAGE_PIN = 6;
+
+// Create objects for modules
+RF24 radio(NRF_CE_PIN, NRF_CS_PIN);
+GNRMC GPS1;
+//Adafruit_INA219 ina219;
+File dataFile;
+
+// Flight stages
+enum FlightStage {
+  READY,
+  ARM,
+  ASCENT,
+  DESCENT,
+  LANDED
+};
+
+FlightStage current_stage = READY;
 
-//----------------------- SETUP -----------------------//
 void setup() {
-  radio.begin();
+  // Initialize Serial
-  radio.openWritingPipe(address);
-  radio.stopListening();  // Set module as transmitter
-
-  IMU_EN_SENSOR_TYPE enMotionSensorType, enPressureType;
   Serial.begin(115200);
-  imuInit(&enMotionSensorType, &enPressureType);
+  Serial.println("# Welcome to CobraV2 operating system for rocket");
-  if (IMU_EN_SENSOR_TYPE_ICM20948 == enMotionSensorType) {
+  delay(1000);
-    Serial.println("# Motion sensor is ICM-20948");
+
-  } else {
+  // Buzzer pin
-    Serial.println("# Motion sensor NULL");
+  pinMode(BUZZER_PIN, OUTPUT);
-  }
+  pinMode(READY_STAGE_PIN, INPUT); 
-  if (IMU_EN_SENSOR_TYPE_BMP280 == enPressureType) {
+  
-    Serial.println("# Pressure sensor is BMP280");
+  // Start with preflight stage
-  } else {
+  current_stage = READY;
-    Serial.println("# Pressure sensor NULL");
+
-  }
   delay(1000);
 }
 
-//----------------------- LOOP -----------------------//
 void loop() {
-  IMU_ST_ANGLES_DATA stAngles;
+  switch (current_stage) {
-  IMU_ST_SENSOR_DATA stGyroRawData;
+    case READY:
-  IMU_ST_SENSOR_DATA stAccelRawData;
+      ready_stage();
-  IMU_ST_SENSOR_DATA stMagnRawData;
+      break;
-  int32_t s32PressureVal = 0, s32TemperatureVal = 0, s32AltitudeVal = 0;
+    case ARM:
+      arm_stag();
+      break;
+    case ASCENT:
+      ascent_stage();
+      break;
+    case DESCENT:
+      descent_stage();
+      break;
+    case LANDED:
+      landed_stage();
+      break;
+  }
+}
 
-  imuDataGet(&stAngles, &stGyroRawData, &stAccelRawData, &stMagnRawData);
+void ready_stage() {
-  pressSensorDataGet(&s32TemperatureVal, &s32PressureVal, &s32AltitudeVal);
+  Serial.println("# READY stage");
+  int counter = 0;
 
-  float temperature = s32TemperatureVal / 100.0;
+  while (true) {
-  float pressure = s32PressureVal / 100.0;
+    int state = digitalRead(READY_STAGE_PIN);
-  float altitude = s32AltitudeVal / 100.0;
+    if (state == HIGH) {
+      counter = 0;
+    } else {
+      counter += 1;
+      delay(300)
+    }
 
-  float angles[] = {stAngles.fRoll, stAngles.fPitch, stAngles.fYaw};
+    if (counter == 10) {
-  float gyro[] = {stGyroRawData.s16X, stGyroRawData.s16Y, stGyroRawData.s16Z};
+      Serial.println("# Pin disconnected");
-  float accel[] = {stAccelRawData.s16X, stAccelRawData.s16Y, stAccelRawData.s16Z};
+      break;
-  float magn[] = {stMagnRawData.s16X, stMagnRawData.s16Y, stMagnRawData.s16Z};
+    }
+  }
 
-  char msg[64];
+  current_stage = READY;
+}
 
-  // Send angles data
+void arm_stage() {
-  for (int i = 0; i < 3; i++) {
+  // System check
-    char float_str[8];
+  // Block parachute ejection
-    dtostrf(angles[i], 6, 2, float_str);
+  // Wait for launch pin removed
-    String str = String("$") + String(i + 1) + ";" + String(float_str) + "*";
+  // Start sending data
-    str.toCharArray(msg, sizeof(msg));
+  Serial.println("# READY stage");
+  // Radio
+  if (!radio.begin()) {
+    Serial.println("# Fail nRF24L01 init");
+  } else {
+    radio.openWritingPipe(address);
+    radio.stopListening();  // Set module as transmitter
+    const char msg[] = "# Radio connection activated";
     radio.write(&msg, sizeof(msg));
-    Serial.println(msg);
+    Serial.println("# Success nRF24L01 init");
   }
   
-  // Send other sensor data
+  // GPS
-  float sensor_data[][3] = {
+  DEV_Set_Baudrate(9600);
-    {temperature, pressure, altitude},
+  DEV_Delay_ms(500);
-    {gyro[0], gyro[1], gyro[2]},
-    {accel[0], accel[1], accel[2]},
-    {magn[0], magn[1], magn[2]}
-  };
   
-  int index = 4;
+  // IMU
-  for (int i = 0; i < 4; i++) {
+  IMU_EN_SENSOR_TYPE enMotionSensorType, enPressureType;
-    for (int j = 0; j < 3; j++) {
+  imuInit(&enMotionSensorType, &enPressureType);
-      char float_str[8];
+  if (IMU_EN_SENSOR_TYPE_ICM20948 == enMotionSensorType) {
-      dtostrf(sensor_data[i][j], 6, 2, float_str);
+    Serial.println("# Success ICM-20948 init");
-      String str = String("$") + String(index) + ";" + String(float_str) + "*";
+    Serial.println(IMU_EN_SENSOR_TYPE_ICM20948);
-      str.toCharArray(msg, sizeof(msg));
+  } else {
-      radio.write(&msg, sizeof(msg));
+    Serial.println("# Fail ICM-20948 init");
-      Serial.println(msg);
+  }
-      index++;
+  if (IMU_EN_SENSOR_TYPE_BMP280 == enPressureType) {
+    Serial.println("# Success BMP280 init");
+  } else {
+    Serial.println("# Fail BMP280 init");
+    Serial.println(enPressureType);
+  }
+  
+  // INA219
+  // if (!ina219.begin()) {
+  //   Serial.println("# Fail INA219 sensor init");
+  // } else {
+  //   Serial.println("# Success INA219 sensor init");
+  // }
+  
+  // SD card
+  if (!SD.begin(4)) {
+    Serial.println("# Fail SD module init");
+  } else {
+    Serial.println("# Success SD module init");
+  }
+
+  if (SD.exists("data.txt")) {
+    Serial.println("# File exists");
+  } else {
+    Serial.println("# File does not exist");
+  }
+
+  dataFile = SD.open("data.txt", FILE_WRITE);
+  if (dataFile) {
+    Serial.println("# File opened");
+    dataFile.println("# CobraV2 flight data");
+  } else {
+    Serial.println("# Error opening file");
+  }
+
+  // Buzzer
+  tone(BUZZER_PIN, 1000);
+  delay(1000);
+  noTone(BUZZER_PIN);
+
+  // Check battery level
+  // float batteryLevel = getBatteryLevel();
+  // Serial.print("# Battery level: ");
+  // Serial.print(batteryLevel);
+  // Serial.println("%");
+
+  // Check for launch pin
+  int counter = 0;
+  while (true) {
+    int state = digitalRead(LAUNCH_STAGE_PIN);
+    if (state == HIGH) {
+      counter = 0;
+    } else {
+      counter += 1;
+      delay(300)
+    }
+
+    if (counter == 10) {
+      Serial.println("# Pin disconnected");
+      break;
     }
   }
+  current_stage = ASCENT;
 }
+
+void ascent_stage() {
+  // Launch
+  // Start sending and saving data on SD card
+  // Check for apogee
+  // Eject parachute
+}
+
+void descent_stage() {
+  // Detect apogee with accelerometer
+  // Eject parachute
+  // GPS and height check
+}
+
+void landed_stage() {
+  // Check for zero velocity
+  // Shut down unneeded systems
+  // Buzz on
+  // Send GPS location
+  // Battery check
+  // Turn off gyro and accelerometer
+}
+
+float getBatteryLevel() {
+  // Read bus voltage from INA219
+  float busVoltage = ina219.getBusVoltage_V();
+
+  // Assuming fully charged battery voltage is 4.2V and empty is 3.0V
+  float maxVoltage = 4.2;
+  float minVoltage = 3.0;
+
+  // Map the bus voltage to a battery level percentage
+  float batteryLevel = map(busVoltage, minVoltage, maxVoltage, 0, 100);
+
+  // Constrain the battery level to be within 0 and 100
+  batteryLevel = constrain(batteryLevel, 0, 100);
+
+  return batteryLevel;
+}
+