except zmq.error.Again:
pass
#print "RP P/I/D={}/{}/{}".format(rp_p, rp_i, rp_d)
if time.time() - last_detect_ts < detect_threas_ms:
if on_detect_counter >= 5:
#print "IN : x={:4.2f}, y={:4.2f}, z={:4.2f}, angle={:4.2f}".format(x, y, z, angle)
#print "CORR: x={:4.2f}, y={:4.2f}, z={:4.2f}".format(x_r, y_r, z)
safety = 10
#roll = r_pid.update(sp_x-x)
#pitch = p_pid.update(sp_z-z)
#thrust = t_pid.update(sp_y-y)
roll = r_pid.update(x)
pitch = p_pid.update(y)
thrust = t_pid.update(z)
yaw = y_pid.update(((angle - yaw_sp + 360 + 180) % 360)-180)
roll_sp = roll
pitch_sp = pitch
yaw_out = yaw
#thrust_sp = thrust+0.73
velocity = v_pid.update(z)
velocity = max(min(velocity, 10), -10) #Limit vertical velocity between -1 and 1 m/sec
velocity = midi_acc
vv_pid.set_point = velocity
dt = (time.time() - prev_t)
curr_velocity = (z-prev_z)/dt
except zmq.error.Again:
pass
#print "RP P/I/D={}/{}/{}".format(rp_p, rp_i, rp_d)
if time.time() - last_detect_ts < detect_threas_ms:
if on_detect_counter >= 5:
#print "IN : x={:4.2f}, y={:4.2f}, z={:4.2f}, angle={:4.2f}".format(x, y, z, angle)
#print "CORR: x={:4.2f}, y={:4.2f}, z={:4.2f}".format(x_r, y_r, z)
safety = 10
#roll = r_pid.update(sp_x-x)
#pitch = p_pid.update(sp_z-z)
#thrust = t_pid.update(sp_y-y)
roll = r_pid.update(x)
pitch = p_pid.update(y)
thrust = t_pid.update(z)
yaw = y_pid.update(((angle - yaw_sp + 360 + 180) % 360) - 180)
roll_sp = roll
pitch_sp = pitch
yaw_out = yaw
#thrust_sp = thrust+0.73
velocity = v_pid.update(z)
velocity = max(
min(velocity, 10),
-10) #Limit vertical velocity between -1 and 1 m/sec
velocity = midi_acc
vv_pid.set_point = velocity
