def logData():
while continueFM("FM104"):
try:
#Generate messages
#Generate Temp Press and Altitude Message
msgAlt = "ALT " + str(altitudeM) + " " + str(pressure) + " " + str(
cTemp)
#Generate Accelerometer Message
msgAcc = "ACC " + str(ACCx) + " " + str(ACCy) + " " + str(ACCz)
#Generate Gyroscope and Heading Message
msgGry = "GRY " + str(gyroXangle) + " " + str(
gyroYangle) + " " + str(gyroZangle) + " " + str(
tiltCompensatedHeading)
#Log Data
f = open("/home/pi/Cone_FlightSoftware/Logs/datalog.txt", "a")
f.write("\n" + str(msgAlt))
f.write("\n" + str(msgAcc))
f.write("\n" + str(msgGry))
f.write("\n" + str(datetime.datetime.now()))
time.sleep(0.11)
except:
pass
def transmitData():
while continueFM("FM104"):
try:
#Generate messages
#Generate Temp Press and Altitude Message
msgAlt = "ALT " + str(altitudeM) + " " + str(pressure) + " " + str(
cTemp)
#Generate Accelerometer Message
msgAcc = "ACC " + str(ACCx) + " " + str(ACCy) + " " + str(ACCz)
#Generate Gyroscope and Heading Message
msgGry = "GRY " + str(gyroXangle) + " " + str(
gyroYangle) + " " + str(gyroZangle) + " " + str(
tiltCompensatedHeading)
#Transmit stuff
Comm.fnc_CommTransmit("ALM " + str(altitudeM))
Comm.fnc_CommTransmit("PRS " + str(pressure))
Comm.fnc_CommTransmit("CTM " + str(cTemp))
Comm.fnc_CommTransmit("ACX " + str(ACCx))
Comm.fnc_CommTransmit("ACY " + str(ACCy))
Comm.fnc_CommTransmit("ACZ " + str(ACCz))
Comm.fnc_CommTransmit("GRX " + str(gyroXangle))
Comm.fnc_CommTransmit("GRY " + str(gyroYangle))
Comm.fnc_CommTransmit("GRZ " + str(gyroZangle))
Comm.fnc_CommTransmit("HDN " + str(tiltCompensatedHeading))
Comm.fnc_CommTransmit("VSP " + str(vSpeed))
#print("Transmission complete...")
except:
pass
def getVspeed():
while continueFM("FM104"):
global vSpeed
#set altitude1
altitude1 = altitudeM
time.sleep(1)
vSpeed = altitudeM - altitude1
print(round(vSpeed, 1))
def getVspeed():
while continueFM("FM110"):
try:
global vSpeed
#set altitude1
altitude1 = altitudeM
time.sleep(1)
vSpeed = altitudeM - altitude1
#print(round(vSpeed, 1))
except:
pass
def check_GPS():
print("Checking for GPS")
global GPS_available
while continueFM("FM101"):
#Get GPS Data
#print("Getting GPS")
time, lat, dirLat, lon, dirLon = IMUGps.fnc_IMU_Gps()
print("#GPS available")
GPS_available = True
Comm.fnc_CommTransmit("MSG PreFlightCheck_GPS_available")
break
def checkData():
while continueFM("FM110"):
try:
#check for Movement
gyroYangle1 = gyroYangle
time.sleep(1)
if (gyroYangle1 - 5) < gyroYangle < (gyroYangle + 5):
print("Not moving")
changeFM("FM102")
except:
pass
def checkData():
global INF
INF = 1
while continueFM("FM107"):
try:
#check for Main Chute opening
if -3 > vSpeed > -10:
print("Main Chute deployed!")
Comm.fnc_CommTransmit("MSG FM107_MainChuteDeployed")
#check for 30m above ground
if (GLA + 30) > altitudeM:
if INF == 1:
with open('config.txt', 'r') as file:
data = file.readlines()
for x in range(len(data)):
dataSplit = data[x].split(" ")
if dataSplit[0] == "INF":
data[x] = "INF 2 \n"
with open('config.txt', 'w') as file:
file.writelines(data)
break
print("Bracing for Impact")
Comm.fnc_CommTransmit("MSG FM107_ChangingINF2")
INF = 2
#check for impact
if ACCx > 1:
print("IMPACT")
#change to FM110
changeFM("FM110")
if ACCx < -1:
print("IMPACT")
#change to FM110
changeFM("FM110")
except:
pass
def getData():
while continueFM("FM110"):
global cTemp
global pressure
global altitudeM
global ACCx
global ACCy
global ACCz
global gyroXangle
global gyroYangle
global gyroZangle
global tiltCompensatedHeading
try:
#get Temp Pressure and altitude
cTemp, pressure, altitudeM = IMUTempPress.fnc_IMUTempPress(QnH)
#Get everything else from the IMU
ACCx, ACCy, ACCz, gyroXangle, gyroYangle, gyroZangle, tiltCompensatedHeading = IMUAccComGyro.fnc_IMU_AxxComGry()
time.sleep(0.1)
except:
pass
def GPS():
while continueFM("FM104"):
try:
#if True:
#Get GPS Data
#print("Getting GPS")
time, lat, dirLat, lon, dirLon = IMUGps.fnc_IMU_Gps()
#Generate GPS Message
msgGps = "GPS " + str(time) + " " + str(lat) + " " + str(
dirLat) + " " + str(lon) + " " + str(dirLon)
#print(msgGps)
Comm.fnc_CommTransmit(msgGps)
#Log Data
f = open("/home/pi/Cone_FlightSoftware/Logs/gpslog.txt", "a")
f.write("\n" + str(msgGps))
f.write("\n" + str(datetime.datetime.now()))
delay(1)
#print("Logged GPS")
except:
pass
def checkData():
global Burn
global Apogee
global PassedApg
PassedApg = False
Burn = True
Apogee = 0
while continueFM("FM104"):
time.sleep(0.1)
try:
#print("Burn " + str(Burn))
#check Orientation
if ORR == "upwards":
#check if Burning
if ACCx > 1.5:
#Burn ongoing
Burn = True
else:
#Burn ended
if Burn == True:
print("---BURN ENDED---")
Burn = False
Comm.fnc_CommTransmit("MSG FM104_BurnEnded")
if ORR == "downwards":
#check if Burning
if ACCx > -0.5:
#Burn ongoing
Burn = True
else:
#Burn ended
if Burn == True:
print("---BURN ENDED---")
Burn = False
Comm.fnc_CommTransmit("MSG FM104_BurnEnded")
#print(Apogee)
#print(altitudeM)
#check for Apogee
if altitudeM > Apogee:
#new Apogee!
Apogee = int(round(altitudeM))
with open('config.txt', 'r') as file:
data = file.readlines()
for x in range(len(data)):
dataSplit = data[x].split(" ")
if dataSplit[0] == "APG":
data[x] = "APG " + str(Apogee) + " \n"
with open('config.txt', 'w') as file:
file.writelines(data)
break
Comm.fnc_CommTransmit("APG " + str(Apogee))
#print(gyroYangle)
#check for deployment
if ORR == "upwards":
#print("Checking smth")
if 0 > gyroYangle:
print("")
else:
print("Deployment detected!!")
Comm.fnc_CommTransmit("MSG FM104_Deployment")
import timer
#print("Uhm")
#GOTO HighDescend
changeFM("FM106")
#print("dafuw")
if ORR == "downwards":
#print("Checking smth")
if 0 < gyroYangle:
print("")
else:
print("Deployment detected!!")
Comm.fnc_CommTransmit("MSG FM104_Deployment")
import timer
#print("Uhm")
changeFM("FM106")
print("dafuq")
#check for flight passed apogee
if (altitudeM + 20) < Apogee:
print("Passed Apogee!")
Comm.fnc_CommTransmit("MSG FM104_PassedApg")
import timer
changeFM("FM106")
except:
pass
def checkMovement():
print("Checking if satellite moved")
#print FCMS.continueFM("FM100")
while continueFM("FM100"):
print("Checking")
time.sleep(5)
try:
#---Reading---
ACCx, ACCy, ACCz, gyroXangle, gyroYangle, gyroZangle, tiltCompensatedHeading = IMUAccComGyro.fnc_IMU_AxxComGry()
heading1 = int(tiltCompensatedHeading)
ACCx1 = ACCx
ACCy1 = ACCy
gryX1 = gyroXangle
gryY1 = gyroYangle
gryZ1 = gyroZangle
#print(heading1)
time.sleep(2)
ACCx, ACCy, ACCz, gyroXangle, gyroYangle, gyroZangle, tiltCompensatedHeading = IMUAccComGyro.fnc_IMU_AxxComGry()
heading2 = int(tiltCompensatedHeading)
ACCx2 = ACCx
ACCy2 = ACCy
gryX2 = gyroXangle
gryY2 = gyroYangle
gryZ2 = gyroZangle
#print(heading2)
#print("Checking...")
#Heading
if heading1 > (heading2 + 2):
#print(" heading TURNING!")
heading_state = "turning"
elif heading2 > (heading1 + 2):
#print(" heading TURNING!")
heading_state = "turning"
else:
#print("heading not turning")
heading_state = "still"
#Gyro X
if gryX1 > (gryX2 + 3):
gryX_state = "turning"
#print("Gyro X Turning!")
elif gryX2 > (gryX1 + 3):
#print("Gyro X Turning!")
gryX_state = "turning"
else:
#print("Gyro X not turning")
gryX_state = "still"
#Gyro Y
if gryY1 > (gryY2 + 3):
#print("Gyro Y Turning!")
gryY_state = "turning"
elif gryY2 > (gryY1 + 3):
#print("Gyro Y Turning!")
gryY_state = "turning"
else:
#print("Gyro Y not turning")
gryY_state = "still"
#Gyro Z
if gryZ1 > (gryZ2 + 0.5):
#print("Gyro Z Turning!")
gryZ_state = "turning"
elif gryZ2 > (gryZ1 + 0.5):
#print("Gyro Z Turning!")
gryZ_state = "turning"
else:
#print("Gyro Z not turning")
gryZ_state = "still"
#find out if satellite is moving or not
turning = 0
still = 0
if heading_state == "turning":
turning = turning + 1
if gryX_state == "turning":
turning = turning + 1
if gryY_state == "turning":
turning = turning + 1
if gryZ_state == "turning":
turning = turning + 1
if heading_state == "still":
still = still + 1
if gryX_state == "still":
still = still + 1
if gryY_state == "still":
still = still + 1
if gryZ_state == "still":
still = still + 1
if turning > 1:
print("Satellite moving!")
satellite_state = "moving"
Comm.fnc_CommTransmit("MSG FM100_SatelliteMoving")
return
break
elif still > 3:
print("Satellite still!")
satellite_state = "still"
Comm.fnc_CommTransmit("MSG FM100_SatelliteStill")
except:
pass
#
# Flight Mode 201
# Set time from GPS
#
print("---FM201 setup---")
import time
import IMUGps
import os
import datetime
import Comm
from FCMS import continueFM
Comm.fnc_CommTransmit("CFM FM201")
print ("---FM201 setup done---")
while continueFM("FM201"):
#try to get GPS Data
try:
#Get GPS Data
#print("Getting GPS")
time, lat, dirLat, lon, dirLon = IMUGps.fnc_IMU_Gps()
#print("Test")
date = IMUGps.fnc_IMU_Gps_Date()
splitDate = date.split("-")
corrDate = "20" + splitDate[2] + "-" + splitDate[1] + "-" + splitDate[0]
print(corrDate)
if len(time) > 1:
print(time)
os.system("sudo date -s '" + corrDate + " " + time + "'")
print("Changed to: " + str(datetime.datetime.now()))
Comm.fnc_CommTransmit("MSG FM201_changedTime")
def checkMovement():
global GLA_set
global QnH
global GPS_available
global Orientation
global Orientation_stable
global Temperature_stable
global QnH_set
global GroundConfirmation
global PreFlightCheck
print("Checking if satellite moved")
#print FCMS.continueFM("FM100")
while continueFM("FM101"):
print("Checking")
time.sleep(1)
#See if Altitude is climbing or descending, if not set the current altitude as Ground Level Altitude (GLA)
print("Checking state of flight...")
cTemp, pressure, altitudeM = IMUTempPress.fnc_IMUTempPress(QnH)
altitudeM1 = altitudeM
pressure1 = pressure
cTemp1 = cTemp
time.sleep(0.7)
cTemp, pressure, altitudeM = IMUTempPress.fnc_IMUTempPress(QnH)
altitudeM2 = altitudeM
pressure2 = pressure
cTemp2 = cTemp
#Check if Temperature is stable
if ((cTemp1 - 1) < cTemp2 < (cTemp1 + 1)):
Temperature_stable = True
print("#Temperature Stable")
Comm.fnc_CommTransmit("MSG PreFlightCheck_Temp_stable")
#Check if QnH is set
if QnH_set == False:
try:
f = open("config.txt", "r")
for x in f:
if x[0:3] == "QNH":
xSplit = x.split(" ")
QnH = int(xSplit[1])
f.close()
QnH_set = True
print("#QnH Set")
Comm.fnc_CommTransmit("MSG PreFlightCheck_QnH_Set")
except:
print("QnH not yet set")
Comm.fnc_CommTransmit("MSG WARNING_PreFlightCheck_QnH_NotSet")
#Check if all PreFlight-Checks are completed and get Ground Confirmation
if GLA_set == True and GPS_available == True and Orientation_stable == True and Temperature_stable == True and QnH_set == True:
print(" ")
print("-PRE FLIGHT CHECKLIST-")
print("#GLA Set")
print("#GPS Available")
print("#Orientation Stable")
print("#Temperature Stable")
print("#QnH Set")
print("#GroundConfirmation ...")
Comm.fnc_CommTransmit("MSG PreFlightCheck_ConfirmationPending")
while continueFM("FM101"):
data = Comm.fnc_CommRecieve()
if str(data) == "MSG ChecklistConfirmed":
print(" received")
print("-CHECKLIST COMPLETE-")
Comm.fnc_CommTransmit("MSG PreFlightCheck_Completed")
#FCMS.changeFM("FM103")
#Check if both measurements are within boundaries (not decending or ascending)
if ((altitudeM1 - 2) < altitudeM2 < (altitudeM1 + 2)):
print("Altitude level")
if GLA_set == False:
GLA = round(altitudeM2)
GLA = int(GLA)
print(GLA)
#Change GLA in the config file
#Get contents of config.txt
with open('config.txt', 'r') as file:
data = file.readlines()
#Find GLA
for x in range(len(data)):
#print("Trying to find GLA at " + str(x))
dataSplit = data[x].split(" ")
#print("Gla?" + str(dataSplit[0]))
if dataSplit[0] == "GLA":
print("FOUND GLA!")
data[x] = "GLA " + str(GLA) + " \n"
with open('config.txt', 'w') as file:
#print(data)
file.writelines(data)
GLA_set = True
print("#Setting GLA")
Comm.fnc_CommTransmit("MSG PreFlightCheck_GLA_set")
break
with open('config.txt', 'r') as file:
data = file.readlines()
for x in range(len(data)):
dataSplit = data[x].split(" ")
if dataSplit[0] == "APG":
data[x] = "APG 0 \n"
with open('config.txt', 'w') as file:
file.writelines(data)
break
elif altitudeM1 < altitudeM2:
print("Alt Climbing!")
elif altitudeM2 < altitudeM1:
print("Alt Descending!")
try:
#---Reading---
ACCx, ACCy, ACCz, gyroXangle, gyroYangle, gyroZangle, tiltCompensatedHeading = IMUAccComGyro.fnc_IMU_AxxComGry(
)
heading1 = int(tiltCompensatedHeading)
ACCx1 = ACCx
ACCy1 = ACCy
gryX1 = gyroXangle
gryY1 = gyroYangle
gryZ1 = gyroZangle
#print(heading1)
time.sleep(2)
ACCx, ACCy, ACCz, gyroXangle, gyroYangle, gyroZangle, tiltCompensatedHeading = IMUAccComGyro.fnc_IMU_AxxComGry(
)
heading2 = int(tiltCompensatedHeading)
ACCx2 = ACCx
ACCy2 = ACCy
gryX2 = gyroXangle
gryY2 = gyroYangle
gryZ2 = gyroZangle
#print(heading2)
#print("Checking...")
#Heading
if heading1 > (heading2 + 2):
#print(" heading TURNING!")
heading_state = "turning"
elif heading2 > (heading1 + 2):
#print(" heading TURNING!")
heading_state = "turning"
else:
#print("heading not turning")
heading_state = "still"
#Gyro X
if gryX1 > (gryX2 + 3):
gryX_state = "turning"
#print("Gyro X Turning!")
elif gryX2 > (gryX1 + 3):
#print("Gyro X Turning!")
gryX_state = "turning"
else:
#print("Gyro X not turning")
gryX_state = "still"
#Gyro Y
if gryY1 > (gryY2 + 3):
#print("Gyro Y Turning!")
gryY_state = "turning"
elif gryY2 > (gryY1 + 3):
#print("Gyro Y Turning!")
gryY_state = "turning"
else:
#print("Gyro Y not turning")
gryY_state = "still"
#Gyro Z
if gryZ1 > (gryZ2 + 0.5):
#print("Gyro Z Turning!")
gryZ_state = "turning"
elif gryZ2 > (gryZ1 + 0.5):
#print("Gyro Z Turning!")
gryZ_state = "turning"
else:
#print("Gyro Z not turning")
gryZ_state = "still"
#find out if satellite is moving or not
turning = 0
still = 0
if heading_state == "turning":
turning = turning + 1
if gryX_state == "turning":
turning = turning + 1
if gryY_state == "turning":
turning = turning + 1
if gryZ_state == "turning":
turning = turning + 1
if heading_state == "still":
still = still + 1
if gryX_state == "still":
still = still + 1
if gryY_state == "still":
still = still + 1
if gryZ_state == "still":
still = still + 1
#Check pointing direction
print(gryY2)
if (-80 > gryY2 > -100):
print("Pointing Upwards!")
Orientation = "upwards"
print("Changing orientation")
#Get contents of config.txt
with open('config.txt', 'r') as file:
data = file.readlines()
#Find ORR
for x in range((len(data))):
dataSplit = data[x].split(" ")
if dataSplit[0] == "ORR":
data[x] = "ORR " + str(Orientation) + " \n"
with open('config.txt', 'w') as file:
file.writelines(data)
break
Orientation_stable = True
Comm.fnc_CommTransmit("MSG PreFlightCheck_Orientation_Upwards")
elif (80 < gryY2 < 100):
print("Pointing Downwards!")
Orientation = "downwards"
print("Changing orientation")
#Get contents of config.txt
with open('config.txt', 'r') as file:
data = file.readlines()
#Find ORR
for x in range((len(data))):
dataSplit = data[x].split(" ")
if dataSplit[0] == "ORR":
data[x] = "ORR " + str(Orientation) + " \n"
with open('config.txt', 'w') as file:
file.writelines(data)
break
Orientation_stable = True
Comm.fnc_CommTransmit(
"MSG PreFlightCheck_Orientation_Downwards")
if turning > 1:
print("Satellite moving!")
satellite_state = "moving"
Comm.fnc_CommTransmit("MSG FM101_SatelliteMoving")
#return
#break
elif still > 3:
print("Satellite still!")
satellite_state = "still"
Comm.fnc_CommTransmit("MSG FM101_SatelliteStill")
except:
pass
def checkData():
try:
f = open("config.txt", "r")
for x in f:
if x[0:3] == "APG":
xSplit = x.split(" ")
#global APG
APG = int(xSplit[1])
f.close()
except:
APG = 0
global glAPG
glAPG = APG
global drogueChute
drogueChute = False
while continueFM("FM106"):
try:
#if True:
#check for Apogee
if altitudeM > glAPG:
#new Apogee!
glAPG = int(round(altitudeM))
with open('config.txt', 'r') as file:
data = file.readlines()
for x in range(len(data)):
dataSplit = data[x].split(" ")
if dataSplit[0] == "APG":
data[x] = "APG " + str(glAPG) + " \n"
with open('config.txt', 'w') as file:
file.writelines(data)
break
Comm.fnc_CommTransmit("APG " + str(Apogee))
#print("Well")
#check if Apogee fits with timer
#print("trying")
import timer
if timer.getTimer() > 25:
#print("Timer exceeded 25!")
#check if Apogee is above 200 above GLA
#print(GLA)
if glAPG < (GLA + 250):
#print("Altimeter not working")
Comm.fnc_CommTransmit("MSG FM106_AltimeterIncorrect")
#switch to FM107
changeFM("FM107")
#Check for parachute opening
if -5 > vSpeed > -15:
print("Parachute is open")
Comm.fnc_CommTransmit("MSG FM106_DrogueOpen")
#Check for Main Chute Deployment Altitude
if (GLA + 100) > altitudeM:
print("Deploying Main Chute!!")
#switch to FM107
changeFM("FM107")
time.sleep(0.1)
except:
pass
