pwm = PWM()
# arm the motors
pwm.arm()
# initialize Pressure Sensor
pressure = PressureSensor()
# calibrate Pressure Sensor
pressure.calibrate()
imu = IMU()
sleep(1)
depthPID = PID(p=145, i=0.1, d=1)
depthPID.target = 0.9
headingPID = PID(p=0.75, i=0.01, d=0)
init_heading, _, _ = imu.get_required(0)
headingPID.target = init_heading - 15
count = 0
start = time()
while True:
try:
if not controller.connected():
depth = pressure.get_depth()
heading, roll, accy = imu.get_required(
ref_heading=headingPID.target)
depthCorrection = -int(depthPID(feedback=depth))
headingCorrection = -int(headingPID(feedback=heading))
if depth > 0.1:
val = drone.get_param(params[i])
if val is not None:
print "found existing value for param", params[i], val
i = val
else:
print "creating new param", params[i], i
drone.create_param(params[i], i)
v_s_switch = True
# Initialize two separate PID controllers for two axis.
# Controller's target is to bring delta angle between image center and object's centroid to zero.
# Controller is not aware of current position of gimbal.
# Since position feedback from gimbal is not available run controller at lower speed than the gimbal can move. Then assume that current position of gimbal is last commanded state.
pidc_X = PID(p=kp, i=ki, d=kd)
pidc_X.target = 0
pidc_Y = PID(p=kp, i=ki, d=kd)
pidc_Y.target = 0
# Initialize gimbal to (0,0,0) angles.
drone.gimbal_set(0.0, 0.0, 0.0)
time.sleep(5.0)
# initialize a rospy.Timer to force controller rate.
control_loop = rospy.Timer(period=rospy.Duration(loop_rate),
callback=control_loop_callback,
oneshot=False)
# subscribe to param update msg
param__update_sub = rospy.Subscriber(
"/" + drone.namespace + "/parameter_updated", String,
param_update_callback)