def receiveJoystick(self, cmd): """ Incoming Joystick Message """ # Emits callback; probably connected to flie and GUI # TODO: have the joydriver.py class integrated in here # TODO: send param set request for hover/on off self.sig_joydata.emit(cmd.roll, cmd.pitch, cmd.yaw, thrustToPercentage(cmd.thrust,flie=False), cmd.hover) self.sig_joydataRaw.emit(cmd.roll, cmd.pitch, cmd.yaw, cmd.thrust, cmd.hover)
def receiveJoystick(self, cmd): """ Incoming Joystick Message """ # Emits callback; probably connected to flie and GUI # TODO: have the joydriver.py class integrated in here # TODO: send param set request for hover/on off self.sig_joydata.emit(cmd.roll, cmd.pitch, cmd.yaw, thrustToPercentage(cmd.thrust, flie=False), cmd.hover) self.sig_joydataRaw.emit(cmd.roll, cmd.pitch, cmd.yaw, cmd.thrust, cmd.hover)
def incomingData(self, data, ts): """ All incoming data is routed to this function. Ros data is spammed from the tree groups """ rostime = rospy.Time.now() # Battery if isGroup(data, "pm"): if hasAllKeys(data, ["vbat"], "pm"): self.sig_batteryUpdated.emit(int(1000*data["pm.vbat"])) if hasAllKeys(data, ["state"], "pm"): self.sig_batteryState.emit(data["pm.state"]) # CPU Usage elif isGroup(data, "utilization"): if hasAllKeys(data, ["cpuUsage"], "utilization"): self.sig_cpuUpdated.emit(data["utilization.cpuUsage"]) # RPY elif isGroup(data, "stabilizer"): if data.has_key("stabilizer.yaw"): # Yaw offset, also negate so its CW #TODO: gyro velocities negated??? self.preYaw = -data["stabilizer.yaw"] data["stabilizer.yaw"] = normaliseAngle(self.yawOffset + self.preYaw) if hasAllKeys(data, ["roll","pitch","yaw"], "stabilizer"): self.sig_rpy.emit(data["stabilizer.roll"],data["stabilizer.pitch"],data["stabilizer.yaw"]) if hasAllKeys(data, ["thrust"], "stabilizer"): self.sig_thrust.emit(data["stabilizer.thrust"]) # Accelerometer elif isGroup(data, "acc"): if hasAllKeys(data, ["x","y","z"], "acc"): self.sig_acc.emit(data["acc.x"],data["acc.y"],data["acc.z"]) self.sig_g.emit(math.sqrt(data["acc.x"]*data["acc.x"]+data["acc.y"]*data["acc.y"]+data["acc.z"]*data["acc.z"])) elif hasAllKeys(data, ["mag2"], "acc"): self.sig_g.emit(math.sqrt(data["mag2.x"])) if hasAllKeys(data, ["zw"], "acc"): self.sig_accZ.emit(data["acc.zw"]) # Hover Target # Motor elif isGroup(data, "motor"): if hasAllKeys(data, ["m1","m2","m3","m4"], "motor"): self.sig_motors.emit(thrustToPercentage(data["motor.m1"]), thrustToPercentage(data["motor.m2"]), thrustToPercentage(data["motor.m3"]), thrustToPercentage(data["motor.m4"])) # Barometer elif isGroup(data, "baro"): if 'baro.asl' in data: self.preAsl = self.preAsl*0.9 + data['baro.asl']*0.1 #little bit of smoothing for k in data.keys(): if k.startswith('baro.asl'): data[k] = data[k] - self.groundLevel # show aslLong to gui if hasAllKeys(data, ["aslLong"], "baro"): self.sig_aslLong.emit(data["baro.aslLong"]) # aslLong = uncompensated asl for ROS #TODO: cheap hack, use aslLong to store raw data["baro.aslLong"] = data["baro.asl"] # asl is compensated, for ros and gui if hasAllKeys(data, ["asl"], "baro"): data["baro.asl"] = data["baro.asl"] - self.aslOffset self.sig_baroASL.emit(data["baro.asl"]) if hasAllKeys(data, ["temp"], "baro"): self.sig_temp.emit(data["baro.temp"]) if hasAllKeys(data, ["pressure"], "baro"): self.sig_pressure.emit(data["baro.pressure"]) # Hover Target elif isGroup(data, "altHold"): if hasAllKeys(data, ["target"], "altHold"): # baro offset data["altHold.target"] = data["altHold.target"] - self.groundLevel - self.aslOffset self.sig_hoverTarget.emit(data["altHold.target"]) ## Sys can fly TODO: doesnt seem to work #elif isGroup(data, "sys"): # if hasAllKeys(data, ["canfly"], "sys"): # self.sig_sysCanFly.emit(data["sys.canfly"]>0) # ROS if self.pubRos: self.sig_rosData.emit(data, ts, rostime) # Wall time ??
def incomingData(self, data, ts): """ All incoming data is routed to this function. Ros data is spammed from the tree groups """ rostime = rospy.Time.now() # Battery if isGroup(data, "pm"): if hasAllKeys(data, ["vbat"], "pm"): self.sig_batteryUpdated.emit(int(1000 * data["pm.vbat"])) if hasAllKeys(data, ["state"], "pm"): self.sig_batteryState.emit(data["pm.state"]) # CPU Usage elif isGroup(data, "utilization"): if hasAllKeys(data, ["cpuUsage"], "utilization"): self.sig_cpuUpdated.emit(data["utilization.cpuUsage"]) # RPY elif isGroup(data, "stabilizer"): if data.has_key("stabilizer.yaw"): # Yaw offset, also negate so its CW #TODO: gyro velocities negated??? self.preYaw = -data["stabilizer.yaw"] data["stabilizer.yaw"] = normaliseAngle(self.yawOffset + self.preYaw) if hasAllKeys(data, ["roll", "pitch", "yaw"], "stabilizer"): self.sig_rpy.emit(data["stabilizer.roll"], data["stabilizer.pitch"], data["stabilizer.yaw"]) if hasAllKeys(data, ["thrust"], "stabilizer"): self.sig_thrust.emit(data["stabilizer.thrust"]) # Accelerometer elif isGroup(data, "acc"): if hasAllKeys(data, ["x", "y", "z"], "acc"): self.sig_acc.emit(data["acc.x"], data["acc.y"], data["acc.z"]) self.sig_g.emit( math.sqrt(data["acc.x"] * data["acc.x"] + data["acc.y"] * data["acc.y"] + data["acc.z"] * data["acc.z"])) elif hasAllKeys(data, ["mag2"], "acc"): self.sig_g.emit(math.sqrt(data["mag2.x"])) if hasAllKeys(data, ["zw"], "acc"): self.sig_accZ.emit(data["acc.zw"]) # Hover Target # Motor elif isGroup(data, "motor"): if hasAllKeys(data, ["m1", "m2", "m3", "m4"], "motor"): self.sig_motors.emit(thrustToPercentage(data["motor.m1"]), thrustToPercentage(data["motor.m2"]), thrustToPercentage(data["motor.m3"]), thrustToPercentage(data["motor.m4"])) # Barometer elif isGroup(data, "baro"): if 'baro.asl' in data: self.preAsl = self.preAsl * 0.9 + data[ 'baro.asl'] * 0.1 #little bit of smoothing for k in data.keys(): if k.startswith('baro.asl'): data[k] = data[k] - self.groundLevel # show aslLong to gui if hasAllKeys(data, ["aslLong"], "baro"): self.sig_aslLong.emit(data["baro.aslLong"]) # aslLong = uncompensated asl for ROS #TODO: cheap hack, use aslLong to store raw data["baro.aslLong"] = data["baro.asl"] # asl is compensated, for ros and gui if hasAllKeys(data, ["asl"], "baro"): data["baro.asl"] = data["baro.asl"] - self.aslOffset self.sig_baroASL.emit(data["baro.asl"]) if hasAllKeys(data, ["temp"], "baro"): self.sig_temp.emit(data["baro.temp"]) if hasAllKeys(data, ["pressure"], "baro"): self.sig_pressure.emit(data["baro.pressure"]) # Hover Target elif isGroup(data, "altHold"): if hasAllKeys(data, ["target"], "altHold"): # baro offset data["altHold.target"] = data[ "altHold.target"] - self.groundLevel - self.aslOffset self.sig_hoverTarget.emit(data["altHold.target"]) ## Sys can fly TODO: doesnt seem to work #elif isGroup(data, "sys"): # if hasAllKeys(data, ["canfly"], "sys"): # self.sig_sysCanFly.emit(data["sys.canfly"]>0) # ROS if self.pubRos: self.sig_rosData.emit(data, ts, rostime) # Wall time ??