def ahrs_cb(self, msg):
'''
AHRS callback. Used to extract roll, pitch, yaw and their rates.
We used the following order of rotation - 1)yaw, 2) pitch, 3) roll
:param msg: Type sensor_msgs/Imu
'''
qx = msg.orientation.x
qy = msg.orientation.y
qz = msg.orientation.z
qw = msg.orientation.w
# conversion quaternion to euler (yaw - pitch - roll)
self.euler_mv.x = math.atan2(2 * (qw * qx + qy * qz),
qw * qw - qx * qx - qy * qy + qz * qz)
self.euler_mv.y = math.asin(2 * (qw * qy - qx * qz))
self.euler_mv.z = math.atan2(2 * (qw * qz + qx * qy),
qw * qw + qx * qx - qy * qy - qz * qz)
# gyro measurements (p,q,r)
p = msg.angular_velocity.x
q = msg.angular_velocity.y
r = msg.angular_velocity.z
sx = math.sin(self.euler_mv.x) # sin(roll)
cx = math.cos(self.euler_mv.x) # cos(roll)
cy = math.cos(self.euler_mv.y) # cos(pitch)
ty = math.tan(self.euler_mv.y) # cos(pitch)
# conversion gyro measurements to roll_rate, pitch_rate, yaw_rate
self.euler_rate_mv.x = p + sx * ty * q + cx * ty * r
self.euler_rate_mv.y = cx * q - sx * r
self.euler_rate_mv.z = sx / cy * q + cx / cy * r
# If we are in first pass initialize filter
if not self.start_flag:
self.start_flag = True
self.euler_rate_mv_old = copy.deepcopy(self.euler_rate_mv)
# Filtering angular velocities
self.euler_rate_mv.x = simple_filters.filterPT1(
self.euler_rate_mv_old.x, self.euler_rate_mv.x,
self.rate_mv_filt_T, self.Ts, self.rate_mv_filt_K)
self.euler_rate_mv.y = simple_filters.filterPT1(
self.euler_rate_mv_old.y, self.euler_rate_mv.y,
self.rate_mv_filt_T, self.Ts, self.rate_mv_filt_K)
self.euler_rate_mv.z = simple_filters.filterPT1(
self.euler_rate_mv_old.z, self.euler_rate_mv.z,
self.rate_mv_filt_T, self.Ts, self.rate_mv_filt_K)
# Set old to current
self.euler_rate_mv_old = copy.deepcopy(self.euler_rate_mv)
def run(self):
'''
Runs ROS node - computes PID algorithms for cascade attitude control.
'''
while (rospy.get_time() == 0) and (not rospy.is_shutdown()):
print 'Waiting for clock server to start'
print 'Received first clock message'
while (not self.start_flag) and (not rospy.is_shutdown()):
print "Waiting for the first measurement."
rospy.sleep(0.5)
print "Starting attitude control."
self.t_old = rospy.Time.now()
clock_old = self.clock
#self.t_old = datetime.now()
self.count = 0
self.loop_count = 0
while not rospy.is_shutdown():
#self.ros_rate.sleep()
while (not self.start_flag) and (not rospy.is_shutdown()):
print "Waiting for the first measurement."
rospy.sleep(0.5)
rospy.sleep(1.0/float(self.rate))
self.euler_sp_filt.x = simple_filters.filterPT1(self.euler_sp_old.x,
self.euler_sp.x, self.roll_reference_prefilter_T, self.Ts,
self.roll_reference_prefilter_K)
self.euler_sp_filt.y = simple_filters.filterPT1(self.euler_sp_old.y,
self.euler_sp.y, self.pitch_reference_prefilter_T, self.Ts,
self.pitch_reference_prefilter_K)
self.euler_sp_filt.z = self.euler_sp.z
#self.euler_sp.z = simple_filters.filterPT1(self.euler_sp_old.z, self.euler_sp.z, 0.2, self.Ts, 1.0)
self.euler_sp_old = copy.deepcopy(self.euler_sp_filt)
clock_now = self.clock
dt_clk = (clock_now.clock - clock_old.clock).to_sec()
clock_old = clock_now
if dt_clk > (1.0 / self.rate + 0.005):
self.count += 1
print self.count, ' - ', dt_clk
if dt_clk < (1.0 / self.rate - 0.005):
self.count += 1
print self.count, ' - ', dt_clk
if dt_clk < 0.005:
dt_clk = 0.01
# Roll
roll_rate_sv = self.pid_roll.compute(self.euler_sp_filt.x, self.euler_mv.x, dt_clk)
# roll rate pid compute
roll_rate_output = self.pid_roll_rate.compute(roll_rate_sv, self.euler_rate_mv.x, dt_clk) + self.roll_rate_output_trim
# Pitch
pitch_rate_sv = self.pid_pitch.compute(self.euler_sp_filt.y, self.euler_mv.y, dt_clk)
# pitch rate pid compute
pitch_rate_output = self.pid_pitch_rate.compute(pitch_rate_sv, self.euler_rate_mv.y, dt_clk) + self.pitch_rate_output_trim
# Yaw
yaw_rate_sv = self.pid_yaw.compute(self.euler_sp_filt.z, self.euler_mv.z, dt_clk)
# yaw rate pid compute
yaw_rate_output = self.pid_yaw_rate.compute(yaw_rate_sv, self.euler_rate_mv.z, dt_clk)
# VPC stuff
vpc_roll_output = -self.pid_vpc_roll.compute(0.0, roll_rate_output, dt_clk)
# Due to some wiring errors we set output to +, should be -
vpc_pitch_output = -self.pid_vpc_pitch.compute(0.0, pitch_rate_output, dt_clk)
# Publish mass position
#mass0_command_msg = Float64()
#mass0_command_msg.data = dx_pitch
#mass2_command_msg = Float64()
#mass2_command_msg.data = -dx_pitch
#mass1_command_msg = Float64()
#mass1_command_msg.data = -dy_roll
#mass3_command_msg = Float64()
#mass3_command_msg.data = dy_roll
#self.pub_mass0.publish(mass0_command_msg)
#self.pub_mass1.publish(mass1_command_msg)
#self.pub_mass2.publish(mass2_command_msg)
#self.pub_mass3.publish(mass3_command_msg)
# Publish attitude
attitude_output = Float64MultiArray()
attitude_output.data = [roll_rate_output, pitch_rate_output, \
yaw_rate_output, vpc_roll_output, vpc_pitch_output]
self.attitude_pub.publish(attitude_output)
# Publish PID data - could be usefule for tuning
self.pub_pid_roll.publish(self.pid_roll.create_msg())
self.pub_pid_roll_rate.publish(self.pid_roll_rate.create_msg())
self.pub_pid_pitch.publish(self.pid_pitch.create_msg())
self.pub_pid_pitch_rate.publish(self.pid_pitch_rate.create_msg())
self.pub_pid_yaw.publish(self.pid_yaw.create_msg())
self.pub_pid_yaw_rate.publish(self.pid_yaw_rate.create_msg())
# Publish VPC pid data
self.pub_pid_vpc_roll.publish(self.pid_vpc_roll.create_msg())
self.pub_pid_vpc_pitch.publish(self.pid_vpc_pitch.create_msg())