Exemplo n.º 1
0
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);
}
Exemplo n.º 4
0
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);
}