from random import random import math as m import time from hdf5.hdf5 import HDF5 from .model_interface import Model # this pulls in all the variables declared in model_config into the namespace of this module from .model_config import * class hdf_log(HDF5): def __init__(self, log_folder): super().__init__(log_folder) def new_log(self): super().new_log() # add ahrs data group self._create_group('ahrs data') self._create_dataset('ahrs data', 'time', 'seconds') self._create_dataset('ahrs data', 'pitch', 'radians') # add residuals group self._create_group('detection residuals') self._create_dataset('detection residuals', 'time', 'seconds') self._create_dataset('detection residuals', 'r0') self._create_dataset('detection residuals', 'r1') self._create_dataset('detection residuals', 'r2') self._create_dataset('detection residuals', 'r3') def append_motor_data(self, time, data): super().append(time, data) def append_pitch(self, time, pitch): self._append_data_to_dataset_in_group(time, 'time', 'ahrs data') self._append_data_to_dataset_in_group(pitch, 'pitch', 'ahrs data') def append_detection_residuals(self, time, r): self._append_data_to_dataset_in_group(time, 'time', 'detection residuals') self._append_data_to_dataset_in_group(r[0], 'r0', 'detection residuals') self._append_data_to_dataset_in_group(r[1], 'r1', 'detection residuals') self._append_data_to_dataset_in_group(r[2], 'r2', 'detection residuals') self._append_data_to_dataset_in_group(r[3], 'r3', 'detection residuals') class Model_v1(Model): ''' The work here is done inside two functions: * Detection is performed in detection_algorithm() which is installed as an 'after' callback on two triggers (getting out of the transient start period, and the reflexive transition based on the main loop timer.) This was implemented this way because there is another trigger (getting out of isolation) that should not trigger it, and this selectivity is not possible with a state-attached callback. * Isolation is performed in isolation_algorithm(), which is installed as a state-attached 'on_enter' callback. ''' def __init__(self, ahrs, daq, log_folder): super().__init__(ahrs, daq, log_folder) self.x_motor_previous = None # state variables: holds the previous motor position (from the observer) self.time_since_start_previous = None # holds the previous iteration's time since start self.fault_signature = None self.hdf = hdf_log(log_folder) self.hdf.new_log() #### this is where the real work happens: def detection_algorithm(self): ''' Returns true if there has been a detection, false otherwise. ''' print('running detection algorithm') # read & pre-process the time since start & data time_since_start, raw_data = self.daq.get_time_and_data() # it's possible for two invocations of this function to occur extremely closely together, due to async, # one from a timer ending and one from the main loop. Check if this has happened by making sure we're # not seeing the same data for a second time. If we are, just return. if self.time_since_start_previous: if self.time_since_start_previous == time_since_start: return sensor = raw_data.mean(axis=1) # compute the mean over the last block voltage = voltage_mult_factor * (voltage_flip * sensor[0] + voltage_add_factor) # rescale sensor reading to V. NB: those factors aren't expected to change after the initial calibration current = current_mult_factor * (current_flip * sensor[1] + current_add_factor) # ... to A str_pot = str_pot_mult_factor * (str_pot_flip * sensor[2] + str_pot_add_factor) # ... to mm encoder = encoder_flip * encoder_mult_factor * sensor[3] # ... to radians ( = 2*pi radians per 4*32 tks) self.hdf.append_motor_data(time_since_start, [voltage, current, str_pot, encoder]) # log the sensor data in physical units pitch = self.ahrs.get_data()[1] # vehicle pitch, in rad self.hdf.append_pitch(time.time()-self.start_time, pitch) # log the pitch print(pitch) # estimate the state x_motor = encoder # motor position, rad x_battery = str_pot # battery position, mm i = current # motor current, A omega = (voltage - Rm*current)/Km # motor speed, rad/s # sign_omega = m.copysign(1.0,omega) # torque flips polarity w/direction of motion r0 = x_motor - alpha*x_battery # delta between encoder and string pot, rad if self.x_motor_previous: dT = time_since_start - self.time_since_start_previous r1 = omega - (x_motor - self.x_motor_previous)/dT # delta between electrical and mechanical speed, rad/s else: r1 = 0 if omega >= 0.0: r2 = i - max(i_cutoff, i_0 + Kpitch*pitch) r3 = omega - omega_nominal else: r2 = i - min(-i_cutoff, -i_0 + Kpitch*pitch) r3 = omega + omega_nominal self.hdf.append_detection_residuals(time_since_start, [r0, r1, r2, r3]) # log the residuals # test for faults fault0 = m.fabs(r0) > threshold0 fault1 = m.fabs(r1) > threshold1 fault2 = m.fabs(r2) > threshold2 fault3 = m.fabs(r3) > threshold3 fault_signature = [fault0, fault1, fault2, fault3] print(r0, r1, r2, r3) self.x_motor_previous = x_motor self.time_since_start_previous = time_since_start self.fault_signature = fault_signature if any(fault_signature): self.fault_detected() def isolation_algorithm(self): ''' Emits a message and returns True if a fault is isolated, returns False otherwise. ''' # use self.fault_detected pass def on_exit_detection(self): self.x_motor_previous = None self.time_since_start_previous = None