kopia lustrzana https://github.com/IzzyBrand/spaceplane
added circular buffer to detect if glider is descending prematurely or never reaches burst altitude
rodzic
bd0e5868f6
commit
326f6904fa
20
companion.py
20
companion.py
|
@ -39,9 +39,21 @@ print '\n################# READY FOR FLIGHT #################\n'
|
||||||
# Ascent
|
# Ascent
|
||||||
###############################################################################
|
###############################################################################
|
||||||
|
|
||||||
|
# maintain a circular buffer of altitude
|
||||||
|
alt_buffer_len = int(60/LOOP_DELAY)
|
||||||
|
alt_buffer = np.ones([alt_buffer_len]) * vehicle.location.global_frame.alt
|
||||||
|
alt_buffer_ind = 0
|
||||||
|
|
||||||
prev_time_below_burn_alt = time()
|
prev_time_below_burn_alt = time()
|
||||||
while True:
|
while True:
|
||||||
alt = vehicle.location.global_frame.alt
|
alt = vehicle.location.global_frame.alt
|
||||||
|
alt_buffer[alt_buffer_ind] = alt
|
||||||
|
alt_buffer_ind += 1
|
||||||
|
alt_buffer_ind = alt_buffer_ind % alt_buffer_len
|
||||||
|
if (alt - alt_buffer[alt_buffer_ind] < 50):
|
||||||
|
print 'WARNING: descended 50m in 60 seconds. Disconnecting.'
|
||||||
|
break
|
||||||
|
|
||||||
if alt < BURN_ALTITUDE:
|
if alt < BURN_ALTITUDE:
|
||||||
prev_time_below_burn_alt = time()
|
prev_time_below_burn_alt = time()
|
||||||
else:
|
else:
|
||||||
|
@ -50,15 +62,19 @@ while True:
|
||||||
if time_above > BURN_TIME_ABOVE:
|
if time_above > BURN_TIME_ABOVE:
|
||||||
break
|
break
|
||||||
|
|
||||||
|
time.sleep(LOOP_DELAY)
|
||||||
|
|
||||||
print_status()
|
print_status()
|
||||||
print '\n################# REACHED BURN ALTITUDE #################\n'
|
print '\n################# REACHED BURN ALTITUDE #################\n'
|
||||||
|
|
||||||
|
|
||||||
###############################################################################
|
###############################################################################
|
||||||
# Burn
|
# Burn
|
||||||
###############################################################################
|
###############################################################################
|
||||||
|
|
||||||
GPIO.output(BURN_PIN, GPIO.HIGH)
|
GPIO.output(BURN_PIN, GPIO.HIGH)
|
||||||
vehicle.mode = VehicleMode("GUIDED")
|
vehicle.mode = VehicleMode("GUIDED")
|
||||||
|
vehicle.simple_goto
|
||||||
|
|
||||||
prev_time_above_burn_alt = time()
|
prev_time_above_burn_alt = time()
|
||||||
while True:
|
while True:
|
||||||
|
@ -71,6 +87,8 @@ while True:
|
||||||
if time_below > BURN_TIME_ABOVE:
|
if time_below > BURN_TIME_ABOVE:
|
||||||
break
|
break
|
||||||
|
|
||||||
|
time.sleep(LOOP_DELAY)
|
||||||
|
|
||||||
GPIO.output(BURN_PIN, GPIO.LOW)
|
GPIO.output(BURN_PIN, GPIO.LOW)
|
||||||
|
|
||||||
print_status()
|
print_status()
|
||||||
|
@ -82,7 +100,7 @@ print '\n################# VEHICLE DISCONNECTED #################\n'
|
||||||
|
|
||||||
while True():
|
while True():
|
||||||
print_status()
|
print_status()
|
||||||
time.sleep(1)
|
time.sleep(LOOP_DELAY)
|
||||||
|
|
||||||
def print_status():
|
def print_status():
|
||||||
print 'Time: {}\tMode: {}\t Alt: {}\tLoc: ({}, {})'.format(
|
print 'Time: {}\tMode: {}\t Alt: {}\tLoc: ({}, {})'.format(
|
||||||
|
|
|
@ -5,3 +5,4 @@ BURN_TIME_ABOVE = 20 # time above burn alt before ignite
|
||||||
TARGET_LAT = 42.345131 # desired landing latitude
|
TARGET_LAT = 42.345131 # desired landing latitude
|
||||||
TARGET_LON = -71.210126 # desired landing longitude
|
TARGET_LON = -71.210126 # desired landing longitude
|
||||||
TARGET_ALT = 0 # meters above sea level
|
TARGET_ALT = 0 # meters above sea level
|
||||||
|
LOOP_DELAY = 0.5 # time in seconds to delay within loops
|
||||||
|
|
Ładowanie…
Reference in New Issue