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