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