def gather_telemetry(bar: BMP085, accel: mpu6050):
"""
Gather data from the satellite's systems
:return: string of telemetry data
"""
accel_data = accel.get_accel_data()
gyro_data = accel.get_gyro_data()
temp = bar.read_temperature()
pressure = bar.read_pressure()
alt = bar.read_altitude()
accel_x = accel_data["x"]
accel_y = accel_data["y"]
accel_z = accel_data["z"]
gyro_x = gyro_data["x"]
gyro_y = gyro_data["y"]
gyro_z = gyro_data["z"]
payload_signal = GPIO.input(payload_in_pin)
t = datetime.datetime.now().timestamp()
data = "{},{},{},{},{},{},{},{},{},{},{}".format(t, temp, pressure, alt,
accel_x, accel_y, accel_z,
gyro_x, gyro_y, gyro_z,
payload_signal)
data += "\n"
return data
def main():
set("delayed", "0")
set("mode", "servo")
set("servo_max", "180")
set("active", "1")
delay_period = 0.01
t = 0
scale = 200
########given#######
# state launch conditions
R = 287 # J/(kg*K)
# h_o = 0 # m
# P_o = 101300 # Pa
# L = -0.0065 # K/m
# T_o = 273 # K
# g = 9.81 # m/s^2
# state the gain of controller
kp = 2
ki = 0
kd = 1
# initialize fin position
phi = 0 # degrees
setServo(phi)
# state desired roll
r = 10 # degrees/s
while True: # infinite loop
# calculate time
t_old = t
t = current_milli_time()
del_t = t - t_old
# calcualte om_x, om_y, om_z angular velocities
# read straight from MPU-6050 gyroscope (scaled)
om_x = read_word_2c(0x43) / 131
om_y = read_word_2c(0x45) / 131
om_z = read_word_2c(0x47) / 131
# calculate roll, pitch, yaw angular positions
roll = om_x * del_t
pitch = om_y * del_t
yaw = om_z * del_t
# calcualte al_x, al_y, al_z angular accelerations
al_x = om_x / del_t
al_y = om_y / del_t
al_z = om_z / del_t
# calcualte a_x, a_y, a_z accelerations
# read straight from ADXL377 high-g accelerometer
a_x = arduino_map(adc.read_adc(0), 0, 675, -scale, scale)
a_y = arduino_map(adc.read_adc(1), 0, 675, -scale, scale)
a_z = arduino_map(adc.read_adc(2), 0, 675, -scale, scale)
# calculate x, y, z position
x = a_x / (del_t) ^ 2
y = a_y / (del_t) ^ 2
z = a_z / (del_t) ^ 2
# calculate u, v, w velocities
u = a_x / del_t
v = a_y / del_t
w = a_z / del_t
# calculate P, T, h, rho
# read from BMP180 sensor
P = BMP180.read_pressure()
P_o = BMP180.read_sealevel_pressure()
T = BMP180.read_temperature()
rho = P / (R * T)
h = BMP180.read_altitude()
# h = (T/L)*((P_o/P)^(R*L/g)-1)+h_o
# calculate error
e = r - roll
e_int = e * t
e_der = e / del_t
# apply the controlled gain to the servo based on the error
phi = kp * e + ki * e_int + kd * e_der
setServo(phi)
time.sleep(delay_period)
var = [
t, del_t, roll, pitch, yaw, om_x, om_y, om_z, al_x, al_y, al_z, x,
y, z, u, v, w, a_x, a_y, a_z, phi, e, P, T, rho, h
]
write_to_csv(var)
# write out to .csv file
# delay to protect code from breaking.
time.sleep(0.010) # seconds
