void tick(const uint8_t tick_type) {
if(tick_type & TICK_TASK_TYPE_MESSAGE) {
if(PIN_OVER_CURRENT.pio->PIO_PDSR & PIN_OVER_CURRENT.mask) {
BC->over_current = false;
} else {
if(!BC->over_current) {
logbli("Over Current");
BC->over_current = true;
StandardMessage sm;
BA->com_make_default_header(&sm, BS->uid, sizeof(StandardMessage), FID_OVER_CURRENT);
BA->send_blocking_with_timeout(&sm,
sizeof(StandardMessage),
*BA->com_current);
}
}
}
simple_tick(tick_type);
}
void tick(const uint8_t tick_type) {
if(tick_type & TICK_TASK_TYPE_CALCULATION) {
if(!(BC->pin_alert->pio->PIO_PDSR & BC->pin_alert->mask)) {
if(BA->mutex_take(*BA->mutex_twi_bricklet, 10)) {
if(BC->gain_muldiv[1] != 0) {
BC->value[SIMPLE_UNIT_CURRENT] = (ina226_read_register(INA226_REG_CURRENT)*CURRENT_40OHM_MUL/CURRENT_40OHM_DIV)*BC->gain_muldiv[0]/BC->gain_muldiv[1];
} else {
BC->value[SIMPLE_UNIT_CURRENT] = ina226_read_register(INA226_REG_CURRENT);
}
BC->value[SIMPLE_UNIT_VOLTAGE] = ina226_read_register(INA226_REG_BUS_VOLTAGE)*VOLTAGE_MUL/VOLTAGE_DIV;
// clear alert pin
ina226_read_mask();
BA->mutex_give(*BA->mutex_twi_bricklet);
BC->value[SIMPLE_UNIT_POWER] = BC->value[SIMPLE_UNIT_CURRENT]*BC->value[SIMPLE_UNIT_VOLTAGE]/1000;
}
}
}
simple_tick(tick_type);
}
void tick(const uint8_t tick_type) {
simple_tick(tick_type);
}
void tick(const uint8_t tick_type) {
if(tick_type & TICK_TASK_TYPE_CALCULATION) {
BC->tick_counter++;
if(BC->tick_counter > 60000/10) {
BC->tick_counter = 0;
for(uint8_t i = 0; i < BEAT_INTERVALS_SIZE; i++) {
BC->beat_intervals[i] = 0;
}
BC->beat_intervals_filled = false;
BC->value[SIMPLE_UNIT_HEART_RATE] = 0;
}
if(PIN_SIGNAL.pio->PIO_PDSR & PIN_SIGNAL.mask) {
// Debounce mechanism after high was detected
if (BC->tick_counter < BC->debounce_high/REDUCE_DEBOUNCE) {
simple_tick(tick_type);
return;
}
if(!(BC->previous_signal_state)) {
BC->beat_state_changed = 1;
BC->beat_state = 1;
BC->last_high_at = BC->tick_counter;
if(BC->beat_intervals_iterator == BEAT_INTERVALS_SIZE) {
BC->beat_intervals_iterator = 0;
BC->beat_intervals_filled = true;
}
BC->beat_intervals[BC->beat_intervals_iterator] = BC->tick_counter;
if(BC->last_low_at != 0) {
BC->debounce_low_times[BC->beat_intervals_iterator] = BC->tick_counter - BC->last_low_at; // current detected low time
}
else {
BC->debounce_low_times[BC->beat_intervals_iterator] = BC->debounce_low;
}
BC->tick_counter = 0;
if(BC->beat_intervals_iterator >= 0 && BC->beat_intervals_iterator < 8) {
if(BC->beat_intervals_filled) {
uint32_t sum = 0;
uint32_t sum_debounce_low = 0;
for(uint8_t i = 0; i < BEAT_INTERVALS_SIZE; i++) {
sum += BC->beat_intervals[i];
sum_debounce_low += BC->debounce_low_times[i];
}
BC->value[SIMPLE_UNIT_HEART_RATE] = (BEAT_INTERVALS_SIZE*60000)/sum;
BC->debounce_low = BETWEEN(MIN_DEBOUNCE, sum_debounce_low/BEAT_INTERVALS_SIZE, MAX_DEBOUNCE);
}
else {
uint32_t sum = 0;
uint32_t sum_debounce_low = 0;
for(uint8_t i = 0; i < BC->beat_intervals_iterator+1; i++) {
sum += BC->beat_intervals[i];
sum_debounce_low += BC->debounce_low_times[i];
}
BC->value[SIMPLE_UNIT_HEART_RATE] = ((BC->beat_intervals_iterator+1)*60000)/sum;
BC->debounce_low = BETWEEN(MIN_DEBOUNCE, sum_debounce_low/(BC->beat_intervals_iterator+1), MAX_DEBOUNCE);
}
BC->beat_intervals_iterator ++;
}
BC->previous_signal_state = true;
}
}
if(!(PIN_SIGNAL.pio->PIO_PDSR & PIN_SIGNAL.mask)) {
// Debounce mechanism after low was detected
if (BC->tick_counter < BC->debounce_low/REDUCE_DEBOUNCE) {
simple_tick(tick_type);
return;
}
if(BC->previous_signal_state) {
BC->beat_state_changed = 1;
BC->beat_state = 0;
BC->last_low_at = BC->tick_counter;
if(BC->debounce_high_iterator == DEBOUNCE_HIGH_TIMES_SIZE) {
BC->debounce_high_iterator = 0;
BC->debounce_high_filled = true;
}
BC->debounce_high_times[BC->debounce_high_iterator] = BC->tick_counter; //current detected high time
if(BC->debounce_high_iterator >= 0 && BC->debounce_high_iterator < DEBOUNCE_HIGH_TIMES_SIZE) {
if(BC->debounce_high_filled) {
uint32_t sum_debounce_high = 0;
for(uint8_t i = 0; i < DEBOUNCE_HIGH_TIMES_SIZE; i++) {
sum_debounce_high += BC->debounce_high_times[i];
}
BC->debounce_high = BETWEEN(MIN_DEBOUNCE, sum_debounce_high/DEBOUNCE_HIGH_TIMES_SIZE, MAX_DEBOUNCE);
}
else {
uint32_t sum_debounce_high = 0;
for(uint8_t i = 0; i < BC->debounce_high_iterator+1; i++) {
sum_debounce_high += BC->debounce_high_times[i];
}
BC->debounce_high = BETWEEN(MIN_DEBOUNCE, sum_debounce_high/BC->debounce_high_iterator+1, MAX_DEBOUNCE);
}
BC->debounce_high_iterator ++;
}
BC->previous_signal_state = false;
}
}
}
if(tick_type & TICK_TASK_TYPE_MESSAGE) {
if(BC->beat_state_changed_callback_enabled) {
if(BC->beat_state_changed) {
BC->beat_state_changed = 0;
send_beat_state_changed_callback();
}
}
}
simple_tick(tick_type);
}
void tick(const uint8_t tick_type) {
if(tick_type & TICK_TASK_TYPE_CALCULATION) {
if(BC->next_measurement_state_counter > 0) {
BC->next_measurement_state_counter--;
} else {
switch(BC->measurement_state) {
case MS_DISABLED: {
new_velocity_value(0);
new_distance_value(0);
if(BC->update_mode) {
BC->measurement_state = MS_UPDATE_MODE;
} else if(BC->laser_enabled) {
BC->measurement_state = MS_START_ACQUISITION;
}
break;
}
case MS_UPDATE_MODE: {
if(BC->new_mode == 0) {
uint8_t data = 0;
if(lidar_write_register(REG_AQUISATION_MODE_CONTROL, 1, &data)) {
BC->update_mode = false;
if(BC->laser_enabled) {
BC->measurement_state = MS_START_ACQUISITION;
} else {
BC->measurement_state = MS_DISABLED;
};
}
} else {
uint8_t velocity_resoluition_value = 0xC8;
switch(BC->new_mode) {
case 1: velocity_resoluition_value = 0xC8; break;
case 2: velocity_resoluition_value = 0x50; break;
case 3: velocity_resoluition_value = 0x28; break;
case 4: velocity_resoluition_value = 0x14; break;
}
if(lidar_write_register(REG_VELOCITY_RESOLUTION, 1, &velocity_resoluition_value)) {
BC->update_mode = false;
if(BC->laser_enabled) {
BC->measurement_state = MS_START_ACQUISITION;
} else {
BC->measurement_state = MS_DISABLED;
}
}
}
break;
}
case MS_START_ACQUISITION: {
if(BC->update_mode) {
BC->measurement_state = MS_UPDATE_MODE;
} else if(!BC->laser_enabled) {
BC->measurement_state = MS_DISABLED;
} else {
const uint8_t data = 0x04; // Take acquisition & correlation processing with DC correction
if(lidar_write_register(REG_CONTROL, 1, &data)) {
if(BC->new_mode == 0) {
BC->measurement_state = MS_READ_DISTANCE;
} else {
BC->measurement_state = MS_TAKE_VELOCITY_MEASUREMENT;
}
}
}
break;
}
case MS_READ_DISTANCE: {
uint8_t data[2];
if(lidar_read_register(REG_DISTANCE, 2, data)) {
const uint16_t distance = (data[0] << 8) | data[1];
if((distance & (1 << 15)) != (1 << 15)) { // If the MSB is 1 then the reading is not considered valid
new_distance_value(distance);
new_velocity_value(0);
}
BC->measurement_state = MS_START_ACQUISITION;
}
break;
}
case MS_TAKE_VELOCITY_MEASUREMENT: {
const uint8_t data = 0x80; // Take velocity measurement
if(lidar_write_register(REG_AQUISATION_MODE_CONTROL, 1, &data)) {
BC->measurement_state = MS_READ_VELOCITY;
}
break;
}
case MS_READ_VELOCITY: {
int8_t velocity;
if(lidar_read_register(REG_VELOCITY_MEASUREMENT, 1, (uint8_t *)&velocity)) {
new_velocity_value(velocity);
new_distance_value(0);
BC->measurement_state = MS_START_ACQUISITION;
}
break;
}
default: {
BC->measurement_state = MS_START_ACQUISITION;
break;
}
}
}
}
simple_tick(tick_type);
}
