#print "OUT: roll={:2.2f}, pitch={:2.2f}, thrust={:5.2f}, dt={:0.3f}, fps={:2.1f}".format(roll_corr, pitch_corr, thrust_sp, dt, 1/dt)
print "OUT: alt={:1.4f}, thrust={:5.2f}, dt={:0.3f}, fps={:2.1f}, speed={:+0.4f}".format(z, thrust_sp, dt, 1/dt, curr_velocity)
#print "dt={:0.3f}, fps={:2.1f}".format(dt, 1/dt)
cmd["ctrl"]["roll"] = roll_corr / 30.0
cmd["ctrl"]["pitch"] = pitch_corr / 30.0
cmd["ctrl"]["thrust"] = thrust_sp
cmd["ctrl"]["yaw"] = yaw_out
else:
on_detect_counter += 1
else:
#print "No detect"
cmd["ctrl"]["roll"] = 0
cmd["ctrl"]["pitch"] = 0
cmd["ctrl"]["thrust"] = 0
cmd["ctrl"]["yaw"] = 0
r_pid.reset_dt()
p_pid.reset_dt()
y_pid.reset_dt()
v_pid.reset_dt()
vv_pid.reset_dt()
#v
vv_pid.Integrator = 0.0
r_pid.Integrator = 0.0
p_pid.Integrator = 0.0
y_pid.Integrator = 0.0
on_detect_counter = 0
client_conn.send_json(cmd, zmq.NOBLOCK)
except simplejson.scanner.JSONDecodeError as e:
print e
#print "OUT: roll={:2.2f}, pitch={:2.2f}, thrust={:5.2f}, dt={:0.3f}, fps={:2.1f}".format(roll_corr, pitch_corr, thrust_sp, dt, 1/dt)
print "OUT: alt={:1.4f}, thrust={:5.2f}, dt={:0.3f}, fps={:2.1f}, speed={:+0.4f}".format(
z, thrust_sp, dt, 1 / dt, curr_velocity)
#print "dt={:0.3f}, fps={:2.1f}".format(dt, 1/dt)
cmd["ctrl"]["roll"] = roll_corr / 30.0
cmd["ctrl"]["pitch"] = pitch_corr / 30.0
cmd["ctrl"]["thrust"] = thrust_sp
cmd["ctrl"]["yaw"] = yaw_out
else:
on_detect_counter += 1
else:
#print "No detect"
cmd["ctrl"]["roll"] = 0
cmd["ctrl"]["pitch"] = 0
cmd["ctrl"]["thrust"] = 0
cmd["ctrl"]["yaw"] = 0
r_pid.reset_dt()
p_pid.reset_dt()
y_pid.reset_dt()
v_pid.reset_dt()
vv_pid.reset_dt()
#v
vv_pid.Integrator = 0.0
r_pid.Integrator = 0.0
p_pid.Integrator = 0.0
y_pid.Integrator = 0.0
on_detect_counter = 0
client_conn.send_json(cmd, zmq.NOBLOCK)
except simplejson.scanner.JSONDecodeError as e:
print e
