class AttitudeHoldNode(): def __init__(self): rospy.init_node('attitude_hold_controller') self.node_identifier = 2 self.input_rc = [1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000] self.sub_imu = rospy.Subscriber('/phx/imu', Imu, self.imuCallback) self.sub_attitude = rospy.Subscriber('/phx/fc/attitude', Attitude, self.attitudeCallback) self.autopilot_commands = rospy.Subscriber( '/phx/controller_commands', ControllerCmd, self.controllerCommandCallback) self.pub = rospy.Publisher('/phx/autopilot/input', AutoPilotCmd, queue_size=1) self.imu = Imu() self.freq = 100 # Hz self.r = rospy.Rate(self.freq) self.enabled = True self.currentPose = Attitude() self.yawController = PIDController(1500, 1, 0, 1, 0, 100, 0, 0) self.pitchController = PIDController(1500, 1, 0, 1, 0, 100, 0, 0) self.rollController = PIDController(1500, 1, 0, 1, 0, 100, 0, 0) self.controlCommand_pitch = 1500 self.controlCommand_roll = 1500 self.controlCommand_yaw = 1500 def run(self): while not rospy.is_shutdown(): self.r.sleep() def attitudeCallback(self, attitude_msg): if self.enabled: self.currentPose = attitude_msg self.currentPose.yaw -= 180 controlCommand_pitch = self.pitchController.calculate_control_command( attitude_msg.pitch, self.imu.angular_velocity.y) controlCommand_roll = self.rollController.calculate_control_command( attitude_msg.roll, self.imu.angular_velocity.x) controlCommand_yaw = self.yawController.calculate_control_command( attitude_msg.yaw, self.imu.angular_velocity.z) autopilot_command = AutoPilotCmd() joy_msg = RemoteControl() joy_msg.pitch = controlCommand_pitch joy_msg.roll = controlCommand_roll joy_msg.yaw = controlCommand_yaw autopilot_command.rc = joy_msg autopilot_command.node_identifier = self.node_identifier self.pub.publish(autopilot_command) #print 'cc: ', self.controlCommand_pitch, 'p: ', pitch_controlCommand_p, 'd: ', pitch_controlCommand_d, 'i: ', pitch_controlCommand_i, 'pitch: ', attitude_msg.pitch #print 'cc: ', self.controlCommand_roll, 'p: ', roll_controlCommand_p, 'd: ', roll_controlCommand_d, 'i: ', roll_controlCommand_i, 'roll: ', attitude_msg.roll #print 'x: ', self.imu.angular_velocity.x, 'y: ', self.imu.angular_velocity.y, 'z: ', self.imu.angular_velocity.z print controlCommand_yaw, controlCommand_roll, controlCommand_pitch #print 'cc: ', self.controlCommand_roll, 'p: ', roll_controlCommand_p, 'd: ', yaw_controlCommand_d, 'i: ', yaw_controlCommand_i, 'yaw: ', attitude_msg.yaw def imuCallback(self, imu_msg): self.imu = imu_msg def controllerCommandCallback(self, controller_msg): self.enabled = controller_msg.enabled[self.node_identifier]
class AltitudeHoldNode(): def __init__(self): rospy.init_node('altitude_hold_controller') self.input_rc = [1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000] self.node_identifier = 1 self.sub1 = rospy.Subscriber('/phx/fc/rc', RemoteControl, self.rcCallback) self.sub2 = rospy.Subscriber('/phx/marvicAltitude/altitude', Altitude, self.altitudeCallback) self.autopilot_commands = rospy.Subscriber( '/phx/controller_commands', ControllerCmd, self.controllerCommandCallback) # self.rc_pub = rospy.Publisher('/phx/rc_computer', RemoteControl, queue_size=1) self.altitude_pub = rospy.Publisher('/phx/autopilot/input', AutoPilotCmd, queue_size=1) setPoint_p = 1 self.enabled = True p = 1 i = 0 d = 4 setPoint_d = 0 i_stop = 1 controlCommand = 1500 self.altitudeController = PIDController(controlCommand, p, i, d, setPoint_p, i_stop, setPoint_d, 0) self.freq = 100 # Hz self.r = rospy.Rate(self.freq) self.previousAltitude = 0 self.firstCall = 1 def run(self): while not rospy.is_shutdown(): self.r.sleep() def controllerCommandCallback(self, controller_msg): self.enabled = controller_msg.enabled[self.node_identifier] def altitudeCallback(self, altitude_msg): if self.enabled: print("Altitude Callback") # set previousAltitude to current Altitude in first call if self.firstCall: self.previousAltitude = altitude_msg.estimated_altitude self.firstCall = 0 un_cliped = self.altitudeController.calculate_control_command( altitude_msg.estimated_altitude, (altitude_msg.estimated_altitude - self.previousAltitude) * 100) if self.input_rc[4] > 1500: un_cliped = self.input_rc[3] controlCommand = np.clip(un_cliped, 1000, 2000) autopilot_command = AutoPilotCmd() autopilot_command.rc.throttle = controlCommand autopilot_command.node_identifier = self.node_identifier # Replay and override current rc self.previousAltitude = altitude_msg.estimated_altitude # self.rc_pub.publish(joy_msg) self.altitude_pub.publish(autopilot_command) print 'alt:', altitude_msg.estimated_altitude, 'controlCommand:', controlCommand else: print 'altitude hold node disabled' def rcCallback(self, rc_msg): self.input_rc[0] = rc_msg.pitch self.input_rc[1] = rc_msg.roll self.input_rc[2] = rc_msg.yaw self.input_rc[3] = rc_msg.throttle # Throttle self.input_rc[4] = rc_msg.aux1 self.input_rc[5] = rc_msg.aux2 self.input_rc[6] = rc_msg.aux3 self.input_rc[7] = rc_msg.aux4
class LandingNode(): def __init__(self): rospy.init_node('landing_controller') self.node_identifier = 3 self.input_rc = [1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000] self.sub_imu = rospy.Subscriber('/phx/imu', Imu, self.imuCallback) # self.autopilot_commands = rospy.Subscriber('/phx/controller_commands', ControllerCmd, self.controllerCommandCallback) self.sub = rospy.Subscriber('/phx/marvicAltitude/altitude', Altitude, self.altitudeCallback) self.pub = rospy.Publisher('/phx/rc_computer', RemoteControl, queue_size=1) # self.autopilot_commands = rospy.Subscriber('/phx/controller_commands', ControllerCmd, self.controllerCommandCallback) self.pub = rospy.Publisher('/phx/autopilot/input', AutoPilotCmd, queue_size=1) self.enabled = True self.p = 1 self.d = 5 self.setPoint_d = 9.81 self.setPoint = 1 self.freq = 100 # Hz self.r = rospy.Rate(self.freq) self.altitude_start = 1 self.altitude = 0.0 self.linear_acceleration_z = 0.0 self.controlCommand = 1500 self.landController = PIDController(1500, 1, 0, 5, 1, 0, 9.81, 0) def altitudeCallback(self, altitude_msg): self.altitude = altitude_msg.estimated_altitude def imuCallback(self, imu_msg): self.linear_acceleration_z = imu_msg.linear_acceleration.z def controllerCommandCallback(self, controller_msg): self.enabled = controller_msg.enabled[self.node_identifier] def run(self): while not rospy.is_shutdown(): if self.enabled: #controlCommand_p = (self.setPoint - self.altitude) * self.p #controlCommand_d = (self.setPoint_d - self.linear_acceleration_z) * self.d un_cliped = self.landController.calculate_control_command( self.altitude, self.linear_acceleration_z) #un_cliped = self.controlCommand + controlCommand_p + controlCommand_d controlCommand = np.clip(un_cliped, 1000, 2000) #print(self.controlCommand, self.altitude, self.linear_acceleration_z) autopilot_command = AutoPilotCmd() autopilot_command.rc.throttle = controlCommand autopilot_command.node_identifier = self.node_identifier print controlCommand self.r.sleep() '''