def srvSensorGetOffset(self, params):
# print(params)
if params.id == 1:
data = self.i2c_driver.offset['acc']
elif params.id == 2:
data = self.i2c_driver.offset['gyro']
elif params.id == 3:
data = self.i2c_driver.offset['magn']
else:
return SensorGet3AxesResponse()
return SensorGet3AxesResponse(Sensor3Axes(data[0], data[1], data[2]))
def srvSensorGetOffset3Axes(self, params):
# print(params)
if params.id == 1:
data = self.i2c_driver.readAccData(True)
elif params.id == 2:
data = self.i2c_driver.readGyroData(True)
elif params.id == 3:
data = self.i2c_driver.readMagnData(True)
else:
return SensorGet3AxesResponse()
return SensorGet3AxesResponse(Sensor3Axes(data[0], data[1], data[2]))
def srvSensorGet3Axes(self,msg): print(msg) if msg.id == 1: data = self.i2c_driver.readAccSensor() elif msg.id == 2: data = self.i2c_driver.readGyroSensor() elif msg.id == 3: data = self.i2c_driver.readMagnSensor() else: return SensorGet3AxesResponse() return SensorGet3AxesResponse(Sensor3Axes(data[0],data[1],data[2]))
def srvSensorSetOffset(self, params):
# print(params)
data = params.data
data = [data.x, data.y, data.z]
if params.id == 1:
self.i2c_driver.offset['acc'] = data
elif params.id == 2:
self.i2c_driver.offset['gyro'] = data
elif params.id == 3:
self.i2c_driver.offset['magn'] = data
else:
return SetOffsetResponse()
return SetOffsetResponse(Sensor3Axes(data[0], data[1], data[2]))
def srvSystemGetVout2(self, params):
data = self.d51_driver.system_get_vout2()
return SensorGet3AxesResponse(Sensor3Axes(data[0], data[1], data[2]))
def srvSystemGetPowerMeas(self, params):
data = self.d51_driver.system_get_power_meas()
return SensorGet3AxesResponse(Sensor3Axes(data[0], data[1], data[2]))
def srvEstimatePose(self, params):
data = self.i2c_driver.estimatePose()
return SensorGet3AxesResponse(Sensor3Axes(data[0], data[1], data[2]))