From 8fb26f494c5ad7fa93379f3f6ab7240139e738a5 Mon Sep 17 00:00:00 2001 From: foglar Date: Mon, 3 Jun 2024 20:57:13 +0200 Subject: [PATCH] New Version 1.2 --- arduino-quick-upload | 45 +++++ sender_module/DEV_Config.cpp | 94 ++++++++++ sender_module/DEV_Config.h | 42 +++++ sender_module/L76X.cpp | 244 ++++++++++++++++++++++++++ sender_module/L76X.h | 83 +++++++++ sender_module/README.md | 8 +- sender_module/sender_module.ino | 293 ++++++++++++++++++++++++-------- 7 files changed, 736 insertions(+), 73 deletions(-) create mode 100755 arduino-quick-upload create mode 100644 sender_module/DEV_Config.cpp create mode 100644 sender_module/DEV_Config.h create mode 100644 sender_module/L76X.cpp create mode 100644 sender_module/L76X.h diff --git a/arduino-quick-upload b/arduino-quick-upload new file mode 100755 index 0000000..589572d --- /dev/null +++ b/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" + diff --git a/sender_module/DEV_Config.cpp b/sender_module/DEV_Config.cpp new file mode 100644 index 0000000..156c64b --- /dev/null +++ b/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); +} + diff --git a/sender_module/DEV_Config.h b/sender_module/DEV_Config.h new file mode 100644 index 0000000..ce8372e --- /dev/null +++ b/sender_module/DEV_Config.h @@ -0,0 +1,42 @@ +#ifndef _DEV_CONFIG_H_ +#define _DEV_CONFIG_H_ + +#include +#include +#include +#include +#include + +#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 + diff --git a/sender_module/L76X.cpp b/sender_module/L76X.cpp new file mode 100644 index 0000000..3a1ec8a --- /dev/null +++ b/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; +} diff --git a/sender_module/L76X.h b/sender_module/L76X.h new file mode 100644 index 0000000..842bd4e --- /dev/null +++ b/sender_module/L76X.h @@ -0,0 +1,83 @@ +#ifndef _L76X_H_ +#define _L76X_H_ + +#include "DEV_Config.h" +#include +#include + + + +#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 diff --git a/sender_module/README.md b/sender_module/README.md index 697413b..e730fc8 100644 --- a/sender_module/README.md +++ b/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 diff --git a/sender_module/sender_module.ino b/sender_module/sender_module.ino index 7ed9a9e..89ccf34 100644 --- a/sender_module/sender_module.ino +++ b/sender_module/sender_module.ino @@ -1,85 +1,236 @@ +#include #include +#include + #include #include +#include + +#include "DEV_Config.h" +#include "L76X.h" #include "Waveshare_10Dof-D.h" -//----------------------- CONSTANTS -----------------------// -bool gbSenserConnectState = false; -RF24 radio(9, 8); // CE, CSN -const byte address[6] = "00001"; // address through which two modules communicate +// Define pin numbers for modules +const int NRF_CE_PIN = 9; +const int NRF_CS_PIN = 8; +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(); - radio.openWritingPipe(address); - radio.stopListening(); // Set module as transmitter - - IMU_EN_SENSOR_TYPE enMotionSensorType, enPressureType; + // Initialize Serial Serial.begin(115200); - imuInit(&enMotionSensorType, &enPressureType); - if (IMU_EN_SENSOR_TYPE_ICM20948 == enMotionSensorType) { - Serial.println("# Motion sensor is ICM-20948"); - } else { - Serial.println("# Motion sensor NULL"); - } - if (IMU_EN_SENSOR_TYPE_BMP280 == enPressureType) { - Serial.println("# Pressure sensor is BMP280"); - } else { - Serial.println("# Pressure sensor NULL"); - } + Serial.println("# Welcome to CobraV2 operating system for rocket"); + delay(1000); + + // Buzzer pin + pinMode(BUZZER_PIN, OUTPUT); + pinMode(READY_STAGE_PIN, INPUT); + + // Start with preflight stage + current_stage = READY; + delay(1000); } -//----------------------- LOOP -----------------------// void loop() { - IMU_ST_ANGLES_DATA stAngles; - IMU_ST_SENSOR_DATA stGyroRawData; - IMU_ST_SENSOR_DATA stAccelRawData; - IMU_ST_SENSOR_DATA stMagnRawData; - int32_t s32PressureVal = 0, s32TemperatureVal = 0, s32AltitudeVal = 0; - - imuDataGet(&stAngles, &stGyroRawData, &stAccelRawData, &stMagnRawData); - pressSensorDataGet(&s32TemperatureVal, &s32PressureVal, &s32AltitudeVal); - - float temperature = s32TemperatureVal / 100.0; - float pressure = s32PressureVal / 100.0; - float altitude = s32AltitudeVal / 100.0; - - float angles[] = {stAngles.fRoll, stAngles.fPitch, stAngles.fYaw}; - float gyro[] = {stGyroRawData.s16X, stGyroRawData.s16Y, stGyroRawData.s16Z}; - float accel[] = {stAccelRawData.s16X, stAccelRawData.s16Y, stAccelRawData.s16Z}; - float magn[] = {stMagnRawData.s16X, stMagnRawData.s16Y, stMagnRawData.s16Z}; - - char msg[64]; - - // Send angles data - for (int i = 0; i < 3; i++) { - char float_str[8]; - dtostrf(angles[i], 6, 2, float_str); - String str = String("$") + String(i + 1) + ";" + String(float_str) + "*"; - str.toCharArray(msg, sizeof(msg)); - radio.write(&msg, sizeof(msg)); - Serial.println(msg); - } - - // Send other sensor data - float sensor_data[][3] = { - {temperature, pressure, altitude}, - {gyro[0], gyro[1], gyro[2]}, - {accel[0], accel[1], accel[2]}, - {magn[0], magn[1], magn[2]} - }; - - int index = 4; - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 3; j++) { - char float_str[8]; - dtostrf(sensor_data[i][j], 6, 2, float_str); - String str = String("$") + String(index) + ";" + String(float_str) + "*"; - str.toCharArray(msg, sizeof(msg)); - radio.write(&msg, sizeof(msg)); - Serial.println(msg); - index++; - } + switch (current_stage) { + case READY: + ready_stage(); + break; + case ARM: + arm_stag(); + break; + case ASCENT: + ascent_stage(); + break; + case DESCENT: + descent_stage(); + break; + case LANDED: + landed_stage(); + break; } } + +void ready_stage() { + Serial.println("# READY stage"); + int counter = 0; + + while (true) { + int state = digitalRead(READY_STAGE_PIN); + if (state == HIGH) { + counter = 0; + } else { + counter += 1; + delay(300) + } + + if (counter == 10) { + Serial.println("# Pin disconnected"); + break; + } + } + + current_stage = READY; +} + +void arm_stage() { + // System check + // Block parachute ejection + // Wait for launch pin removed + // Start sending data + 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("# Success nRF24L01 init"); + } + + // GPS + DEV_Set_Baudrate(9600); + DEV_Delay_ms(500); + + // IMU + IMU_EN_SENSOR_TYPE enMotionSensorType, enPressureType; + imuInit(&enMotionSensorType, &enPressureType); + if (IMU_EN_SENSOR_TYPE_ICM20948 == enMotionSensorType) { + Serial.println("# Success ICM-20948 init"); + Serial.println(IMU_EN_SENSOR_TYPE_ICM20948); + } else { + Serial.println("# Fail ICM-20948 init"); + } + 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; +} +