def on_btn_payload_dialog_ok_clicked(self, btn_payload_dialog_ok):
try:
if self.actuator_payload == 0:
self.msg.controls[5] = 0.5
#self.btn_drop_payload0.set_sensitive(False)
self.payload0_drop_time = str(datetime.time(datetime.now()))[:-4]
self.payload0_altitude = self.current_altitude
try:
self.logger.payload_drop(0, self.payload0_drop_time, self.payload0_altitude)
self.video_window.payload_drop(0, self.payload0_drop_time, self.payload0_altitude)
except rospy.ROSInterruptException as e:
rospy.logerr(e)
pass
self.lbl_payload0_info.set_text("Drop Time: %s\n Altitude: %s ft" %(self.payload0_drop_time, float('%.4g' %self.payload0_altitude)))
else:
self.msg.controls[6] = -0.5
#self.btn_drop_payload1.set_sensitive(False)
self.payload1_drop_time = str(datetime.time(datetime.now()))[:-4]
self.payload1_altitude = self.current_altitude
try:
self.logger.payload_drop(1, self.payload1_drop_time, self.payload1_altitude)
self.video_window.payload_drop(1, self.payload1_drop_time, self.payload1_altitude)
except rospy.ROSInterruptException as e:
rospy.logerr(e)
pass
self.lbl_payload1_info.set_text("Drop Time: %s\n Altitude: %s ft" %(self.payload1_drop_time, float('%.4g' %self.payload1_altitude)))
self.msg.header.stamp = rospy.Time.now()
self.pub.publish(self.msg)
self.msg.controls[5] = 0
self.msg.controls[6] = 0
self.payload_dialog.hide()
except rospy.ROSInterruptException as e:
rospy.loggerr(e)
pass
def set_telemetry(self, data, landing_length):
try:
self.lbl_airspeed.set_text("%d ft/s" %data.airspeed)
self.lbl_groundspeed.set_text("%d ft/s" %data.groundspeed)
self.lbl_heading.set_text("%d deg" %data.heading)
self.lbl_throttle.set_text("%d %%" %data.throttle)
self.lbl_altitude.set_text("%d ft" %data.altitude)
self.lbl_climb.set_text("%d ft/s" %data.climb)
self.lbl_landing_length.set_text("%d ft" %landing_length)
self.current_altitude = data.altitude
except rospy.ROSInterruptException as e:
rospy.loggerr(e)
pass
def callback_vfr_hud(self, data): telemetry = VFR_HUD() if self.init_set == False: self.initial_altitude = data.altitude self.init_set = True # Convert to freedom units feet = 3.28084 # ft/m g = 32.174 # ft/s2 try: telemetry.airspeed = data.airspeed * feet # ft/s telemetry.groundspeed = data.groundspeed * feet # ft/s telemetry.heading = data.heading # deg telemetry.throttle = data.throttle * 100 # percent telemetry.altitude = (data.altitude - self.initial_altitude) * feet # ft telemetry.climb = data.climb * feet # ft/s if telemetry.altitude > 0: landing_time = (2 * telemetry.altitude / g)**0.5 # s landing_length = telemetry.groundspeed * landing_time # ft else: landing_length = 0 self.main_window.set_telemetry(telemetry, landing_length) try: if self.main_window.logger.writer_on == True: self.main_window.logger.write_row(telemetry) except rospy.ROSInterruptException as e: rospy.loggerr(e) pass try: main_window.video_window.update_loc(landing_length, 0, telemetry.altitude, telemetry.airspeed) except rospy.ROSInterruptException as e: rospy.loggerr(e) pass except rospy.ROSInterruptException as e: rospy.loggerr(e) pass
def callback_radiostatus(self, data): try: self.main_window.set_radio_status(data.rssi_dbm) except rospy.ROSInterruptException as e: rospy.loggerr(e) pass
def set_radio_status(self, rssi):
try:
self.lbl_rssi.set_text("%d dBm" %rssi)
except rospy.ROSInterruptException as e:
rospy.loggerr(e)
pass
rospy.logerr('Could not arm: ' + str(exc))
rospy.logerr('Armed')
rate = rospy.Rate(30)
throttle = 0
while not rospy.is_shutdown():
# Exit immediately if fatal
if safety_client.is_fatal_active():
break
# Turn off motors if put into safety mode
if safety_client.is_safety_active():
try:
arm_service(False)
except rospy.ServiceException as exc:
rospy.loggerr("Could not disarm: " + str(exc))
command = OrientationThrottleStamped()
command.header.stamp = rospy.Time.now()
command_pub.publish(command)
else:
command = OrientationThrottleStamped()
command.header.stamp = rospy.Time.now()
# Three second throttle ramp and three seconds off
throttle = (throttle + 1) % 150
if throttle <= 100:
command.throttle = float(throttle) / 100.0
command.data.pitch = 0.0
command.data.roll = 0.0