diff --git a/ports/esp32/modules/sensors.py b/ports/esp32/modules/sensors.py index 1a0d640e34e9..62f7ae4f8f92 100755 --- a/ports/esp32/modules/sensors.py +++ b/ports/esp32/modules/sensors.py @@ -2,44 +2,40 @@ from utime import ticks_us, ticks_diff -from avg_filter import * import mahony class Sensors(): def __init__(self, mrps, imu): + self.mrps = mrps self.imu = imu self.offset_roll = 0 self.offset_pitch = 0 self.offset_yaw = 0 - self.roll = 0 # None # discrete - self.pitch = 0 # None # discrete - self.temperature = 0 # None + self._roll = 0 # None # discrete + self._pitch = 0 # None # discrete + self._temperature = 0 + self._yaw = 0 # None # discrete - self.mrps = mrps - self.yaw = mrps.readAngleCom() # required before mrps.angle - self.yaw = self.read_yaw() + mrps.readAngleCom() # required before mrps.angle + self.read_yaw() self._t_us_IMU = ticks_us() # us !!! - - self.pitch_prev = 0 - self.roll_prev = 0 - self.temperature_prev = 0 - self.yaw_prev = 0 + @micropython.native def __repr__(self): return "Sensors(mrps={}, imu={})".format(self.mrps, self.imu) + @micropython.native def info(self): - return f"Sensors: roll={self.roll:-5.2}, pitch={self.pitch:-5.2}, yaw={self.yaw:-5.2}, temperature={self.temperature:3}" + return f"Sensors: roll={self._roll:5.1f}, pitch={self._pitch:5.1f}, yaw={self._yaw:5.1f}, temperature={self.temperature:5.1f}" - #@micropython.native + @micropython.native def handle(self): #print('Sensors().handle()') - _t = ticks_us() # us !!! - t = self.imu.temperature + _t_us = ticks_us() # us !!! a = self.imu.acceleration g = self.imu.gyro if not self.imu.error: @@ -50,41 +46,39 @@ def handle(self): -a[1], a[0], a[2], - ticks_diff(_t, self._t_us_IMU) + ticks_diff(_t_us, self._t_us_IMU) ) - self._t_us_IMU = _t + self._t_us_IMU = _t_us - self.pitch = round(mahony.Mahony_pitch() - self.offset_pitch, 2) # discrete - self.roll = round(mahony.Mahony_roll() - self.offset_roll, 2) # discrete - self.temperature = round(t, 1) - - self.pitch_prev = self.pitch - self.roll_prev = self.roll - self.temperature_prev = self.temperature - else: - self.pitch = self.pitch_prev - self.roll = self.roll_prev - self.temperature = self.temperature_prev + self._pitch = mahony.Mahony_pitch() - self.offset_pitch # discrete + self._roll = mahony.Mahony_roll() - self.offset_roll # discrete - yaw = self.read_yaw() # discrete - if not self.mrps.error: - self.yaw = yaw - self.yaw_prev = yaw - else: - self.yaw = self.yaw_prev + self.read_yaw() - #@micropython.native - def read_yaw(self): # instant - return round(self.mrps.readAngleComInfinity() - self.offset_yaw, 2) - #@micropython.native + @micropython.native + def read_yaw(self): # instant + yaw = self.mrps.readAngleComInfinity() + if not self.mrps.error: + self._yaw = yaw - self.offset_yaw + return self._yaw + + @micropython.native def get_pitch(self): # discrete - return self.pitch + return round(self._pitch, 2) - #@micropython.native + @micropython.native def get_roll(self): # discrete - return self.roll + return round(self._roll, 2) - #@micropython.native + @micropython.native def get_yaw(self): # discrete - return self.yaw + return round(self._yaw, 2) + + @micropython.native + @property + def temperature(self): + t = self.imu.temperature + if not self.imu.error: + self._temperature = round(t, 1) + return self._temperature