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