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