2019-06-01 23:31:32 +00:00
|
|
|
//
|
|
|
|
// DriveSpeedAccumulator.cpp
|
|
|
|
// Clock Signal
|
|
|
|
//
|
|
|
|
// Created by Thomas Harte on 01/06/2019.
|
|
|
|
// Copyright © 2019 Thomas Harte. All rights reserved.
|
|
|
|
//
|
|
|
|
|
|
|
|
#include "DriveSpeedAccumulator.hpp"
|
|
|
|
|
|
|
|
using namespace Apple::Macintosh;
|
|
|
|
|
|
|
|
void DriveSpeedAccumulator::post_sample(uint8_t sample) {
|
2019-07-30 19:08:55 +00:00
|
|
|
if(!number_of_drives_) return;
|
|
|
|
|
2019-06-01 23:31:32 +00:00
|
|
|
// An Euler-esque approximation is used here: just collect all
|
|
|
|
// the samples until there is a certain small quantity of them,
|
|
|
|
// then produce a new estimate of rotation speed and start the
|
|
|
|
// buffer afresh.
|
|
|
|
samples_[sample_pointer_] = sample;
|
|
|
|
++sample_pointer_;
|
|
|
|
|
|
|
|
if(sample_pointer_ == samples_.size()) {
|
|
|
|
sample_pointer_ = 0;
|
2019-07-30 19:08:55 +00:00
|
|
|
|
2019-07-30 20:18:36 +00:00
|
|
|
// Treat 33 as a zero point and count zero crossings; then approximate
|
2019-07-30 19:08:55 +00:00
|
|
|
// the RPM from the frequency of those.
|
2019-07-31 02:02:41 +00:00
|
|
|
int samples_over = 0;
|
2019-07-31 16:42:23 +00:00
|
|
|
int sum = 0;
|
2019-07-30 20:18:36 +00:00
|
|
|
const uint8_t centre = 33;
|
2019-07-31 02:02:41 +00:00
|
|
|
for(size_t c = 0; c < 512; ++c) {
|
|
|
|
if(samples_[c] > centre) ++ samples_over;
|
2019-07-31 16:42:23 +00:00
|
|
|
sum += samples_[c];
|
2019-07-30 19:08:55 +00:00
|
|
|
}
|
|
|
|
|
2019-07-31 02:02:41 +00:00
|
|
|
// TODO: if the above is the correct test, do it on sample receipt rather than
|
|
|
|
// bothering with an intermediate buffer.
|
|
|
|
|
2019-07-31 16:42:23 +00:00
|
|
|
// The below fits for a function like `a + bc`.
|
|
|
|
const float rotation_speed = (float(sum) * 0.052896440564137f) - 259.0f;
|
2019-07-31 02:02:41 +00:00
|
|
|
|
|
|
|
for(int c = 0; c < number_of_drives_; ++c) {
|
|
|
|
drives_[c]->set_rotation_speed(rotation_speed);
|
2019-07-30 19:08:55 +00:00
|
|
|
}
|
2019-07-31 16:42:23 +00:00
|
|
|
// printf("RPM: %0.2f (%d over; %d sum)\n", rotation_speed, samples_over, sum);
|
2019-06-01 23:31:32 +00:00
|
|
|
}
|
|
|
|
}
|
2019-07-30 19:08:55 +00:00
|
|
|
|
|
|
|
void DriveSpeedAccumulator::add_drive(Apple::Macintosh::DoubleDensityDrive *drive) {
|
|
|
|
drives_[number_of_drives_] = drive;
|
|
|
|
++number_of_drives_;
|
|
|
|
}
|