def run(self):
# continuous loop that monitors the information coming from the quadcopter
while 1:
# stores message from the quadcopter
msg = self.master.recv_match(blocking=False)
if msg:
# stores the message type
msg_type = msg.get_type()
#print msg_type # debugging to check which data streams are operational
# checks if the message contains information
if msg.get_type() == "BAD_DATA":
if mavutil.all_printable(msg.data):
sys.stdout.write(msg.data)
sys.stdout.flush()
else:
# add in new messages to monitor below
if msg_type == "ATTITUDE":
# publishes information to the rostopic
self.pub_attitude.publish(msg.roll * 180 / 3.1415, msg.pitch * 180 / 3.1415,
msg.yaw * 180 / 3.1415, msg.rollspeed, msg.pitchspeed, msg.yawspeed)
#upadtes imu values
self.imu_yaw = msg.yaw * 180 / 3.14159
self.imu_roll = msg.roll * 180 / 3.14159
self.imu_pitch = msg.pitch * 180 / 3.14159
# updates the channel data
elif msg_type == "RC_CHANNELS_RAW":
self.ch1 = msg.chan1_raw
self.ch2 = msg.chan2_raw
self.ch3 = msg.chan3_raw
self.ch4 = msg.chan4_raw
self.ch5 = msg.chan5_raw
self.ch6 = msg.chan6_raw
self.ch7 = msg.chan7_raw
self.ch8 = msg.chan8_raw
#checks if channel 5 is below the threshold and gives back control to the transmitter
if self.ch5 < 1400:
if not self.transmitter_control:
if self.have_control:
# informs via terminal
print "You no longer have control, Transmitter has taken over control"
self.transmitter_control = True
self.have_control = False
# changes the control message on the ui
ui.control_toggle_button.setText("Take Over Control")
self.disconnect_pwm_channels()
else:
# turns off transmitter control so that the program is able to take over when requested
self.transmitter_control = False
time.sleep(0.01)
def mainloop():
rospy.init_node('roscopter')
while not rospy.is_shutdown():
rospy.sleep(0.001)
msg = master.recv_match(blocking=False)
if not msg:
continue
#print msg.get_type()
if msg.get_type() == "BAD_DATA":
if mavutil.all_printable(msg.data):
sys.stdout.write(msg.data)
sys.stdout.flush()
else:
msg_type = msg.get_type()
if msg_type == "RC_CHANNELS_RAW":
pub_rc.publish([
msg.chan1_raw, msg.chan2_raw, msg.chan3_raw, msg.chan4_raw,
msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw
])
if msg_type == "HEARTBEAT":
pub_state.publish(
msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED,
msg.base_mode
& mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED,
mavutil.mode_string_v10(msg))
if msg_type == "VFR_HUD":
pub_vfr_hud.publish(msg.airspeed, msg.groundspeed, msg.heading,
msg.throttle, msg.alt, msg.climb)
if msg_type == "GPS_RAW_INT":
fix = NavSatStatus.STATUS_NO_FIX
if msg.fix_type >= 3:
fix = NavSatStatus.STATUS_FIX
pub_gps.publish(
NavSatFix(
latitude=msg.lat / 1e07,
longitude=msg.lon / 1e07,
altitude=msg.alt / 1e03,
status=NavSatStatus(status=fix,
service=NavSatStatus.SERVICE_GPS)))
#pub.publish(String("MSG: %s"%msg))
if msg_type == "ATTITUDE":
pub_attitude.publish(msg.roll, msg.pitch, msg.yaw,
msg.rollspeed, msg.pitchspeed,
msg.yawspeed)
if msg_type == "LOCAL_POSITION_NED":
print "Local Pos: (%f %f %f) , (%f %f %f)" % (
msg.x, msg.y, msg.z, msg.vx, msg.vy, msg.vz)
if msg_type == "RAW_IMU":
pub_raw_imu.publish(Header(), msg.time_usec, msg.xacc,
msg.yacc, msg.zacc, msg.xgyro, msg.ygyro,
msg.zgyro, msg.xmag, msg.ymag, msg.zmag)
def mainloop():
rospy.init_node('roscopter')
while not rospy.is_shutdown():
rospy.sleep(0.001)
msg = master.recv_match(blocking=False)
if not msg:
continue
#print msg.get_type()
if msg.get_type() == "BAD_DATA":
if mavutil.all_printable(msg.data):
sys.stdout.write(msg.data)
sys.stdout.flush()
else:
msg_type = msg.get_type()
if msg_type == "RC_CHANNELS_RAW" :
pub_rc.publish([msg.chan1_raw, msg.chan2_raw, msg.chan3_raw, msg.chan4_raw, msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw])
if msg_type == "HEARTBEAT":
pub_state.publish(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED,
msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED,
mavutil.mode_string_v10(msg))
if msg_type == "VFR_HUD":
pub_vfr_hud.publish(msg.airspeed, msg.groundspeed, msg.heading, msg.throttle, msg.alt, msg.climb)
if msg_type == "GPS_RAW_INT":
fix = NavSatStatus.STATUS_NO_FIX
if msg.fix_type >=3:
fix=NavSatStatus.STATUS_FIX
pub_gps.publish(NavSatFix(latitude = msg.lat/1e07,
longitude = msg.lon/1e07,
altitude = msg.alt/1e03,
status = NavSatStatus(status=fix, service = NavSatStatus.SERVICE_GPS)
))
#pub.publish(String("MSG: %s"%msg))
if msg_type == "ATTITUDE" :
pub_attitude.publish(msg.roll, msg.pitch, msg.yaw, msg.rollspeed, msg.pitchspeed, msg.yawspeed)
px4_time[0] = msg.time_boot_ms # get the time from px4
local_time[0] = time.time() # get the local start time
if msg_type == "LOCAL_POSITION_NED" :
print "Local Pos: (%f %f %f) , (%f %f %f)" %(msg.x, msg.y, msg.z, msg.vx, msg.vy, msg.vz)
if msg_type == "RAW_IMU" :
pub_raw_imu.publish (Header(), msg.time_usec,
msg.xacc, msg.yacc, msg.zacc,
msg.xgyro, msg.ygyro, msg.zgyro,
msg.xmag, msg.ymag, msg.zmag)
def mainloop():
print("Waiting")
global hz_counter
hz_counter=1;
trate = 1.0/rate
first_time_imu=True
first_time_pos=True
earliercmdtime = 0
## Set home values from Vicon/Kinect data ##
rospy.sleep(.1)
set_home()
## Integration and differentiation error initialized ##
I_ex=0.0; I_ey=0.0; I_ez=0.0
D_ex=0.0; D_ey=0.0; D_ez=0.0
new_err_x=0.0; new_err_y=0.0; new_err_z=0.0;
## IMU values initialized ##
imu_roll=0.0; imu_pitch=0.0; first_yaw=0.0;
imu_yaw=0.0; first_roll=0.0; first_pitch=0.0
## Position values initialised ##
pos_x=0.0; pos_y=0.0; pos_z=0.0
while not rospy.is_shutdown(): # start loop
#continue
if float(time.time())-trate >= earliercmdtime:
earliercmdtime = time.time()
hz_counter =1;
if connect_APM: # enable these lines if you want to obtain the data from the arducopter on-the-fly
# msg = master.recv_match(blocking=False)
# if not msg:
# continue
if 0:#msg:
if msg.get_type() == "BAD_DATA":
if mavutil.all_printable(msg.data):
sys.stdout.write(msg.data)
sys.stdout.flush()
else:
msg_type = msg.get_type()
if msg_type == "ATTITUDE" :
pub_attitude.publish(msg.roll*180/3.1415, msg.pitch*180/3.1415, msg.yaw*180/3.1415, msg.rollspeed, msg.pitchspeed, msg.yawspeed)
if first_time_imu:
first_time_imu=False
first_yaw = msg.yaw
first_roll = msg.roll
first_pitch = msg.pitch
imu_yaw = msg.yaw
imu_roll = msg.roll
imu_pitch = msg.pitch
else:
imu_yaw = msg.yaw
imu_roll = msg.roll
imu_pitch = msg.pitch
## Update position values ##
(old_pos_x,old_pos_y,old_pos_z,pos_x,pos_y,pos_z) = update_pos_values(pos_x,pos_y,pos_z,vicon_x,vicon_y,vicon_z,first_time_pos)
(err_x,err_y,err_z) = calc_pos_error(home_x,home_y,home_z,pos_x,pos_y,pos_z) # home - pos
(d_x, d_y, d_z) = calc_pos_error(old_pos_x,old_pos_y,old_pos_z,pos_x,pos_y,pos_z)
## Toggle for first time through loop ##
if first_time_pos:
first_time_pos=False
## PID controller (roll+pitch and thrust, yaw later) ##
I_ex = I_ex + err_x*trate
I_ey = I_ey + err_y*trate
I_ez = I_ez + err_z*trate
V_x = d_x*trate # units of m/s
V_y = d_y*trate # units of m/s
V_z = d_z*trate # units of m/s
## Cap the integration error as well after certain value ##
if (I_ex) > I_ex_MAX:
I_ex = I_ex_MAX #* I_ex / abs(I_ex)
if (I_ex) < -I_ex_MAX:
I_ex = -I_ex_MAX #* I_ex / abs(I_ex)
if (I_ey) > I_ey_MAX:
I_ey = I_ey_MAX #* I_ey / abs(I_ey)
if (I_ey) < -I_ey_MAX:
I_ey = -I_ey_MAX #* I_ey / abs(I_ey)
if (I_ez) > I_ez_MAX:
I_ez = I_ez_MAX #* I_ez / abs(I_ez)
if (I_ez) < I_ez_MAX:
I_ez = -I_ez_MAX #* I_ez / abs(I_ez)
p_pitch = pitch_P * err_x
i_pitch = pitch_I * I_ex
d_pitch = pitch_D * V_x
p_roll = roll_P * err_y
i_roll = roll_I * I_ey
d_roll = roll_D * V_y
p_th = thrust_P * err_z
i_th = thrust_I * (I_ez)
d_th = thrust_D * V_z
## Calculate PID values to send
sendroll = (p_roll + i_roll + d_roll) #
sendpitch = (p_pitch + i_pitch + d_pitch)
###### neg pitch === hexacopter will move forward
###### pos pitch === hexacopter will move backward
###### neg roll === hexacopter will move left
###### pos roll === hexacopter will move right
sendthrust = p_th + i_th + d_th
sendyaw = yaw_P*(vicon_yaw - home_yaw) #0 #-imu_yaw
## Cap the roll/pitch/yaw/thrust values ##
if (sendroll) > roll_MAX:
sendroll = roll_MAX #* sendroll / abs(sendroll)
if (sendroll) < -roll_MAX:
sendroll = -roll_MAX #* sendroll / abs(sendroll)
if (sendpitch) > pitch_MAX:
sendpitch = pitch_MAX #* sendpitch / abs(sendpitch)
if (sendpitch) < -pitch_MAX:
sendpitch = -pitch_MAX #* sendpitch / abs(sendpitch)
if sendthrust > thrust_MAX:
sendthrust = thrust_MAX
if sendthrust < 0:
sendthrust = 0
## Send values to APM ##
if connect_APM:
master.mav.set_roll_pitch_yaw_thrust_send(master.target_system, master.target_component,
sendroll, sendpitch, sendyaw, sendthrust)
send_odometry.transform.translation.x = sendroll * 180/3.14159
send_odometry.transform.translation.y = sendpitch * 180/3.14159
send_odometry.transform.translation.z = sendyaw * 180/3.14159
## Print some output to display for debugging ##
# stdscr.addstr(0, 0, "Position controller is running...")
# if connect_APM:
# stdscr.addstr(1, 0, "Connection to APM established...")
# else:
# stdscr.addstr(1, 0, "******Warning: No connection to APM!*************")
# stdscr.addstr(3, 0, "The gains are:")
# stdscr.addstr(4, 0, " P I D")
# stdscr.addstr(5, 0, "Roll %.3f %.3f %.3f" % (roll_P, roll_I, roll_D))
# stdscr.addstr(6, 0, "Pitch %.3f %.3f %.3f" % (pitch_P, pitch_I, pitch_D))
# stdscr.addstr(7, 0, "Yaw %.3f %.3f %.3f" % (yaw_P, yaw_I, yaw_D))
# stdscr.addstr(8, 0, "Thrust %.3f %.3f %.3f" % (thrust_P, thrust_I, thrust_D))
# stdscr.addstr(10, 0, "Home position is: (%+.3f,%+.3f,%+.3f) m" % (home_x, home_y, home_z))
# stdscr.addstr(11, 0, "Current position is: (%+.3f,%+.3f,%+.3f) m" % (pos_x, pos_y, pos_z))
# stdscr.addstr(12, 0, "Position error is: (%+.3f,%+.3f,%+.3f) m" % (err_x, err_y, err_z))
# if connect_APM:
# stdscr.addstr(14, 0, "The IMU values are (%+.3f,%+.3f,%+.3f) deg " % (imu_roll/deg2pi, imu_pitch/deg2pi, imu_yaw/deg2pi))
# else:
# stdscr.addstr(14, 0, "The IMU values are not available!")
# stdscr.addstr(15, 0, "The Vicon values are (%+.3f,%+.3f,%+.3f) deg " % (vicon_roll/deg2pi,vicon_pitch/deg2pi,vicon_yaw/deg2pi))
# stdscr.addstr(17, 0, "Sending control commands to APM:")
# stdscr.addstr(18, 0, "Roll (deg) Pitch (deg) Yaw (deg) Thrust (PWM)")
# stdscr.addstr(19, 0, "%+.3f %+.3f %+.3f %+.3f" % (sendroll/deg2pi, sendpitch/deg2pi, sendyaw/deg2pi, sendthrust))
# stdscr.addstr(20, 0, " \n")
# stdscr.refresh()
# else:
# hz_counter =0; #stdscr.addstr(22, 0, " The rate of command is not correct")
pub_odom .publish(send_odometry)
