Select Git revision
meson.build
Forked from
card10 / firmware
Source project has a limited visibility.
__init__.py 25.62 KiB
from st3m.ui.widgets import constraints
import captouch
import math
import cmath
import time
class Widget:
def think(self, ins, delta_ms):
pass
def on_enter(self):
pass
def on_exit(self):
pass
class Altimeter(Widget):
def __init__(self, ref_temperature=15, filter_stages=4, filter_coeff=0.7):
self.ref_temperature = float(ref_temperature)
filter_stages = int(filter_stages)
if filter_stages < 0:
raise ValueError("filter_deg must be greater than or equal to 0")
self._filter_deg = filter_stages
self.filter_coeff = float(filter_coeff)
self.on_enter()
def on_enter(self):
self._filter = [None] * (self._filter_deg + 1)
self._altitude = None
@staticmethod
def _relative_altitude(pascal, celsius):
# https://en.wikipedia.org/wiki/Hypsometric_equation
# return (celsius + 273.15) * (287 / 9.81) * math.log(101325 / pascal)
return (celsius + 273.15) * (29.255861366) * (11.52608845 - math.log(pascal))
def think(self, ins, delta_ms):
temp = (
self.ref_temperature
if self.ref_temperature is not None
else ins.temperature
)
pressure = ins.imu.pressure
if not pressure > 0:
self._altitude = None
return
altitude = self._relative_altitude(ins.imu.pressure, temp)
self._filter[0] = altitude
mult = 1 - self.filter_coeff
for i in range(self._filter_deg):
if self._filter[i + 1] is None:
self._filter[i + 1] = self._filter[i]
return
else:
self._filter[i + 1] += (self._filter[i] - self._filter[i + 1]) * mult
self._altitude = self._filter[self._filter_deg]
@property
def meters_above_sea(self):
return self._altitude
class Inclinometer(Widget):
def __init__(self, buffer_len=2):
buffer_len = int(buffer_len)
if buffer_len <= 0:
raise ValueError("buffer_len must be greater than 0")
self._buffer_len = buffer_len
self._filter_len = buffer_len
self.on_enter()
@property
def filter_len(self):
return self._filter_len
@filter_len.setter
def filter_len(self, val):
val = int(val)
if not (0 < val <= self._buffer_len):
raise ValueError(
f"filter_len must be greater than 0 and smaller than or equal to buffer_len ({self._buffer_len})"
)
self._filter_len = val
def on_enter(self):
# transfer functions are highly nonlinear, so for consistent response we have
# to filter after the respective transformations, so we do need buffers for everything
self._acc_buf = [None] * self._buffer_len
self._incl_buf = [None] * self._buffer_len
self._azi_buf = [None] * self._buffer_len
self._roll_buf = [None] * self._buffer_len
self._pitch_buf = [None] * self._buffer_len
self._next_index = 0
def on_exit(self):
self._acc_buf = None
self._incl_buf = None
self._azi_buf = None
self._roll_buf = None
self._pitch_buf = None
def think(self, ins, delta_ms):
index = self._next_index
self._acc_buf[index] = ins.imu.acc
self._incl_buf[index] = None
self._azi_buf[index] = None
self._roll_buf[index] = None
self._pitch_buf[index] = None
self._next_index = (index + 1) % self._buffer_len
def _incl(self, index):
if self._incl_buf[index] is None:
x, y, z = self._acc_buf[index]
# we don't need to do modulo filtering on this one so we can store it as-is
# also makes _roll and _pitch faster, not needing to call phase and all
self._incl_buf[index] = math.atan2(math.sqrt(x * x + y * y), z)
return self._incl_buf[index]
def _azi(self, index):
if self._azi_buf[index] is None:
val = complex(self._acc_buf[index][0], self._acc_buf[index][1])
self._azi_buf[index] = val / abs(val) if val else complex(1)
return self._azi_buf[index]
def _roll(self, index):
if self._roll_buf[index] is None:
# note: .imag is equivalent to math.sin(cmath.phase()) since azi is normalized to 1
self._roll_buf[index] = cmath.rect(
1, self._azi(index).imag * self._incl(index)
)
return self._roll_buf[index]
def _pitch(self, index):
if self._pitch_buf[index] is None:
# note: .real is equivalent to math.cos(cmath.phase()) since azi is normalized to 1
self._pitch_buf[index] = cmath.rect(
1, self._azi(index).real * self._incl(index)
)
return self._pitch_buf[index]
def _filter(self, getter):
if self._acc_buf is None:
return None
ret = 0 # do NOT initialize as complex to make the hack below work
index = self._next_index
for _ in range(self._filter_len):
index = (index - 1) % self._buffer_len
if self._acc_buf[index] is None:
return None
ret += getter(index)
if type(ret) == complex:
# modulo filtering for most...
return cmath.phase(ret) if ret else None
else:
# regular filtering for incl
return ret / self._filter_len
@property
def inclination(self):
return self._filter(self._incl)
@property
def azimuth(self):
return self._filter(self._azi)
@property
def roll(self):
return self._filter(self._roll)
@property
def pitch(self):
return self._filter(self._pitch)
class CaptouchWidget(Widget):
def __init__(
self,
config,
*,
gain=complex(1),
constraint=None,
friction=0.7,
bounce=1,
):
self.pos = 0j
self.gain = gain
self.constraint = constraint
if constraint:
self.pos = constraint._center
self.friction = friction
self.bounce = bounce
self.active_callback = None
self._active = False
self._vel = 0j
self._ref = None
self._ignore = False
if not hasattr(self, "one_d"):
self.one_d = False
self.add_to_config(config)
def on_enter(self):
self._vel = 0j
self._ref = None
self._active = False
self._ignore = True
def add_to_config(self, config):
pass
@property
def active(self):
return self._active
@active.setter
def active(self, val):
val = bool(val)
if val != self._active:
self._active = val
if self.active_callback:
self._pre_active_callback()
self.active_callback(val)
def _pre_active_callback(self):
pass
def _apply_constraint(self, hold):
if self.constraint is None:
return
if hold:
self.pos = self.constraint.apply_hold(self.pos)
else:
self.pos, self._vel = self.constraint.apply_free(
self.pos, self._vel, self.bounce
)
def _apply_velocity(self, delta_ms):
vel_prev = self._vel
if (not self._vel) or abs(self._vel) < 0.0001:
self._vel = 0j
return False
self._vel *= (1 - self.friction) ** (delta_ms / 1000)
self.pos += self._vel * delta_ms
if self.constraint is not None:
self.pos, self._vel = self.constraint.apply_free(
self.pos, self._vel, self.bounce
)
return True
class PetalWidget(CaptouchWidget):
def __init__(
self,
config,
petal,
**kwargs,
):
if type(petal) is not int or petal >= 10 or petal < 0:
raise ValueError("invalid petal index")
self.petal = petal
self._log = captouch.PetalLog()
self.one_d = bool(self.petal % 2)
super().__init__(config, **kwargs)
def add_to_config(self, config):
config.petals[self.petal].logging = True
config.petals[self.petal].set_min_mode(2 if self.one_d else 3)
def on_enter(self):
super().on_enter()
self._log.clear()
def on_exit(self):
self._log.clear()
def _append_and_validate(self, frame):
if self._ignore:
if frame.pressed:
return False
else:
self._ignore = False
if frame.pressed:
self._log.append(frame)
return True
elif self._log.length():
self._autoclear()
self._log.clear()
self._ref = None
return False
def _autoclear(self):
pass
def _debug_print_logs(self, *args):
for x, log in enumerate(args):
ret = f"log{x} = [\n"
ret += " # [raw_pos, raw_cap, delta_t_ms]\n"
for frame in log.frames:
delta_t_ms = (
time.ticks_diff(log.frames[-1].ticks_us, frame.ticks_us) / 1000
)
ret += f" [{frame.raw_pos}, {frame.raw_cap}, {delta_t_ms}],\n"
ret += "]"
print(ret)
class Slider(PetalWidget):
def __init__(self, *args, **kwargs):
if "constraint" not in kwargs:
kwargs["constraint"] = constraints.Ellipse()
super().__init__(*args, **kwargs)
# minimum amount of samples used for averaging to consider data valid
self.min_precision = 3
# maximum amount of samples used for averaging to limit response time
self.max_precision = 8
# how much delay we apply to data to crop out the frames just before a petal is released
self.liftoff_ms = 30
# how long to wait after petal has been pressed to not consider frames garbage
self.lifton_ms = 20
# internal data
self._pos_range = None
def think(self, ins, delta_ms):
petal = ins.captouch.petals[self.petal]
if not petal.log:
# no new data has been received, there's nothing to do
return
for frame in petal.log:
if (
# if frame is not pressed
(not self._append_and_validate(frame))
# or log is too short for lifton
or ((start := self._log.index_offset_ms(0, self.lifton_ms)) is None)
# or log is too short for liftoff
or ((stop := self._log.index_offset_ms(-1, -self.liftoff_ms)) is None)
# or there's not enough samples for valid position data
or ((length := stop - start) < self.min_precision)
):
self.active = False
continue
if length > self.max_precision:
start = stop - self.max_precision
if length - self.max_precision >= 10:
crop = self._log.index_offset_ms(start, -self.lifton_ms)
start -= crop
stop -= crop
self._log.crop(crop)
self._pos_range = (start, stop)
self.active = True
self._update_pos()
def _update_pos(self):
if self._pos_range is not None:
self.pos = self.gain * self._log.average(*self._pos_range)
self._apply_constraint(hold=True)
self._pos_range = None
def _pre_active_callback(self):
self._update_pos()
def _autoclear(self):
self._update_pos()
class Scroller(PetalWidget):
def __init__(self, config, petal, *, friction=0.7, **kwargs):
if kwargs.get("constraint", None) is None:
raise TypeError("This widget requires a constraint")
super().__init__(config, petal, **kwargs)
self.friction = friction
self._vel = 0j
# amount of samples for reference position
self.ref_precision = 6
# amount of samples for position. must be smaller or equal to ref_position.
self.pos_precision = 3
# how much delay we apply to data to crop out the frames just before a petal is released
self.liftoff_ms = 30
# how long to wait after petal has been pressed to not consider frames garbage
self.lifton_ms = 20
# internal data
self._pos_range = None
self._ref = None
def think(self, ins, delta_ms, petal=None):
if not petal:
petal = ins.captouch.petals[self.petal]
if not petal.log:
return
for frame in petal.log:
if (
(not self._append_and_validate(frame))
or ((start := self._log.index_offset_ms(0, self.lifton_ms)) is None)
or ((stop := self._log.index_offset_ms(-1, -self.liftoff_ms)) is None)
or ((length := stop - start) < self.ref_precision)
):
self.active = False
continue
if length - self.ref_precision >= 10:
pos_crop = self._log.index_offset_ms(
stop - self.ref_precision, -self.lifton_ms
)
swipe_crop = self._log.index_offset_ms(-1, -150)
if swipe_crop is not None:
crop = min(swipe_crop, pos_crop)
if crop >= 10:
start -= crop
stop -= crop
self._log.crop(crop)
if self._ref is None:
self._ref = self._log.average(stop - self.ref_precision, stop)
else:
self._pos_range = (stop - self.pos_precision, stop)
self.active = True
self._update_pos()
if not self.active:
self._apply_velocity(delta_ms)
def _update_pos(self):
if self._ref is not None and self._pos_range is not None:
avg = self._log.average(*self._pos_range)
self.pos += self.gain * (avg - self._ref)
self._ref = avg
self._apply_constraint(hold=True)
self._pos_range = None
def _pre_active_callback(self):
self._update_pos()
def _autoclear(self):
self._update_pos()
self._ref = None
if self._log.length_ms() > 27:
start = 0
stop = self._log.length() - 1
if stop > 2:
stop = self._log.index_offset_ms(-1, -10)
if self._log.index_offset_ms(0, 80) is not None:
start = self._log.index_offset_ms(-1, -70)
if self._log.index_offset_ms(start, 20) is not None:
vel = self._log.slope_per_ms(start, stop)
# gate val experimentally determined on petal 2
# optimum might be different for each petal but good enough for now
if abs(vel) < 0.01:
self._vel = 0
else:
self._vel = vel * self.gain
self._pos_range = None
# trigger callbacks
self.active = True
self.active = False
class MultiSlider(CaptouchWidget):
def __init__(self, config, **kwargs):
self.avg_frames = 8
if "constraint" not in kwargs:
kwargs["constraint"] = constraints.Ellipse()
super().__init__(config, **kwargs)
def add_to_config(self, config):
for x in range(0, 10, 2):
config.petals[x].logging = True
config.petals[x].set_min_mode(3)
def on_enter(self):
super().on_enter()
self._log = [list() for _ in range(10)]
def on_exit(self):
self._log = None
def think(self, ins, delta_ms):
for i in range(0, 10, 2):
self._log[i] += ins.captouch.petals[i].log
self._log[i] = self._log[i][-self.avg_frames :]
for i in range(0, 10, 2):
if len(self._log[i]) < self.avg_frames:
self.active = False
return
pressed_petals = []
num_petals = 0
for i in range(0, 10, 2):
if all([self._log[i][j].pressed for j in range(self.avg_frames)]):
pressed_petals += [i]
num_petals += 1
if self._ignore:
if num_petals:
return False
else:
self._ignore = False
def avg_pos(l):
ret = 0j
for f in l:
ret += f.raw_pos
ret /= len(l)
return ret
def avg_cov(l):
ret = 0j
for f in l:
ret += f.raw_cap
ret /= len(l)
return ret
if num_petals == 1:
pressed_petals += [
(
pressed_petals[0]
+ (2 if avg_pos(self._log[pressed_petals[0]]).imag > 0 else -2)
)
% 10
]
elif num_petals == 2:
diff = pressed_petals[0] - pressed_petals[1]
if min(diff % 10, (-diff) % 10) != 2:
self.active = False
return
else:
self.active = False
return
# len(pressed_petals) == 2 from here on out
gain = [avg_cov(self._log[pressed_petals[i]]) for i in range(2)]
norm = 1 / sum(gain)
gain = [
gain[i] * norm * captouch.PETAL_ANGLES[pressed_petals[i]] for i in range(2)
]
k = [gain[i] * (avg_pos(self._log[pressed_petals[i]]) + 3) for i in range(2)]
self.pos = self.gain * sum(k) / 4
self._apply_constraint(hold=True)
self.active = True
# TODO: finish these
#
# class Wiggler(PetalWidget):
# def __init__(self, *args, **kwargs):
# super().__init__(*args, **kwargs)
# self.ref_precision = 4
# self.pos_precision = 2
# self.lifton_ms = 20
# self._ref = None
#
# def think(self, ins, delta_ms):
# petal = ins.captouch.petals[self.petal]
# if not petal.log:
# return
# latest_log = None
# for frame in petal.log:
# if (
# (not self._append_and_validate(frame))
# or ((start := self._log.index_offset_ms(0, self.lifton_ms)) is None)
# or ((length := self._log.length - 1 - start) < self.ref_precision)
# ):
# self.active = False
# continue
# if length - self.ref_precision >= 10:
# crop = self._log.index_offset_ms(-self.ref_precision, -self.lifton_ms)
# self._log.crop(crop)
# if self._ref is None:
# self._ref = self._log.average(-self.ref_precision)
# else:
# self.active = True
# self._update_pos()
#
# def _update_pos(self):
# if self._ref is not None:
# self.pos = self.gain * (self._log.average(-self.pos_precision) - self._ref)
# self._apply_constraint(hold=True)
#
# def _pre_active_callback(self):
# self._update_pos()
#
# def _autoclear(self):
# self._ref = None
# self.pos = 0j
#
#
# class Pool(PetalWidget):
# def __init__(self, config, petal, *, friction=0.7, speed=20, **kwargs):
# if kwargs.get("constraint", None) is None:
# raise TypeError("This widget requires a constraint")
# super().__init__(config, petal, **kwargs)
# self.aim = None
# self.speed = speed
# self.friction = friction
# self._vel = 0j
#
# # amount of samples for reference position
# self.ref_precision = 6
# # amount of samples for position. must be smaller or equal to ref_position.
# self.pos_precision = 3
# # how much delay we apply to data to crop out the frames just before a petal is released
# self.liftoff_ms = 30
# # how long to wait after petal has been pressed to not consider frames garbage
# self.lifton_ms = 20
#
# # internal data
# self._aim_range = None
# self._ref = None
#
# def think(self, ins, delta_ms, petal=None):
# if not petal:
# petal = ins.captouch.petals[self.petal]
# if not petal.log:
# return
# for frame in petal.log:
# if (
# (not self._append_and_validate(frame))
# or ((start := self._log.index_offset_ms(0, self.lifton_ms)) is None)
# or ((stop := self._log.index_offset_ms(-1, -self.liftoff_ms)) is None)
# or ((length := stop - start) < self.ref_precision)
# ):
# self.active = False
# continue
# if length - self.ref_precision >= 10:
# crop = self._log.index_offset_ms(
# stop - self.ref_precision, -self.lifton_ms
# )
# stop -= crop
# self._log.crop(crop)
# if self._ref is None:
# self._ref = self._log.average(stop - self.ref_precision, stop)
# else:
# self._aim_range = (stop - self.pos_precision, stop)
# self.active = True
# self._update_aim()
# self._apply_velocity(delta_ms)
#
# def _update_aim(self):
# if self._ref is not None and self._aim_range is not None:
# self.aim = -self.gain * (self._log.average(*self._aim_range) - self._ref)
# self._aim_range = None
#
# def _pre_active_callback(self):
# self._update_aim()
#
# def _autoclear(self):
# self._update_aim()
# if self.aim is not None:
# self._vel += self.aim * self.speed / 1000
# self.aim = None
#
# def _draw_inner(self, ctx, colors=_default_colors):
# if self.active:
# ctx.rgb(*colors.marker_col)
# else:
# ctx.rgb(*colors.marker_col_inactive)
# pos = colors.widget_scale * self.pos
# ctx.arc(pos.real, pos.imag, colors.marker_size / 2, 0, math.tau, 0).fill()
# if self.aim:
# ctx.move_to(pos.real, pos.imag)
# aim = colors.widget_scale * self.aim
# aim += colors.marker_size * aim / (2 * abs(aim))
# ctx.rel_line_to(aim.real, aim.imag).stroke()
class MultiItemScroller(CaptouchWidget):
def __init__(self, config, *, petals, gain=1, friction=0.7):
super().__init__(config, gain=gain, friction=friction)
self._item = 0
self._max_item = None
self._vel = 0
petals = set(petals)
for petal in petals:
if petal not in range(0, 10, 2):
raise ValueError("all petals must be top petals")
constraint = constraints.Rectangle()
self._widgets = [
Scroller(
config,
x,
gain=self.gain * captouch.PETAL_ANGLES[x],
friction=self.friction,
constraint=constraint,
)
for x in petals
]
for widget in self._widgets:
widget.ref_precision = 3
widget.constraint = None
def add_to_config(self, config):
if hasattr(self, "_widgets"):
for widget in self._widgets:
widget.add_to_config(config)
@property
def gain(self):
return self._gain
@gain.setter
def gain(self, value):
self._gain = value
if hasattr(self, "_widgets"):
for widget in self._widgets:
widget.gain = value * captouch.PETAL_ANGLES[widget.petal]
@property
def friction(self):
return self._friction
@friction.setter
def friction(self, value):
if value != 1:
raise ValueError("unfinished widget, friction must be 1 for now, sorry!")
self._friction = value
if hasattr(self, "_widgets"):
for widget in self._widgets:
widget.friction = value
@property
def item(self):
return self._item
@item.setter
def item(self, val):
val = int(val)
if val < 0:
val = 0
elif (self._max_item is not None) and (val > self._max_item):
val = self._max_item
self._item = val
@property
def max_item(self):
return self._max_item
@max_item.setter
def max_item(self, val):
if val is None:
self._max_item = val
return
val = int(val)
if val < 0:
raise ValueError("only non-negative values or None allowed for max_item")
self._max_item = val
self.item = self._item
def on_enter(self):
for widget in self._widgets:
widget.on_enter()
self.pos = self._item
def on_exit(self):
for widget in self._widgets:
widget.on_exit()
def think(self, ins, delta_ms) -> None:
for widget in self._widgets:
widget.think(ins, delta_ms)
delta = 0
for widget in self._widgets:
if widget.active:
delta += widget.pos.imag
else:
widget.pos = 0j
target = self._item + delta
delta_ms_phys = delta_ms
while delta_ms_phys:
if delta_ms_phys < 50:
delta_s = delta_ms_phys / 1000
delta_ms_phys = 0
else:
delta_s = 50 / 1000
delta_ms_phys -= 50
offset = target - self.pos
if offset > 4:
offset = 4
elif offset < -4:
offset = -4
acc = offset * 175
acc -= self._vel * 25
# unit: items per second
self._vel += acc * delta_s
# if self._vel > 8:
# self._vel = 8
# elif self._vel < -8:
# self._vel = -8
self.pos += self._vel * delta_s
delta_hyst = int(delta * 1.5)
if delta_hyst:
self.item += delta_hyst
for widget in self._widgets:
widget.pos = 0j