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())