94 lines
2.1 KiB
C
94 lines
2.1 KiB
C
|
#include <furi.h>
|
||
|
#include "fsk_demod.h"
|
||
|
|
||
|
struct FSKDemod {
|
||
|
uint32_t low_time;
|
||
|
uint32_t low_pulses;
|
||
|
uint32_t hi_time;
|
||
|
uint32_t hi_pulses;
|
||
|
|
||
|
bool invert;
|
||
|
uint32_t mid_time;
|
||
|
uint32_t time;
|
||
|
uint32_t count;
|
||
|
bool last_pulse;
|
||
|
};
|
||
|
|
||
|
FSKDemod*
|
||
|
fsk_demod_alloc(uint32_t low_time, uint32_t low_pulses, uint32_t hi_time, uint32_t hi_pulses) {
|
||
|
FSKDemod* demod = malloc(sizeof(FSKDemod));
|
||
|
demod->invert = false;
|
||
|
|
||
|
if(low_time > hi_time) {
|
||
|
uint32_t tmp;
|
||
|
tmp = hi_time;
|
||
|
hi_time = low_time;
|
||
|
low_time = tmp;
|
||
|
|
||
|
tmp = hi_pulses;
|
||
|
hi_pulses = low_pulses;
|
||
|
low_pulses = tmp;
|
||
|
|
||
|
demod->invert = true;
|
||
|
}
|
||
|
|
||
|
demod->low_time = low_time;
|
||
|
demod->low_pulses = low_pulses;
|
||
|
demod->hi_time = hi_time;
|
||
|
demod->hi_pulses = hi_pulses;
|
||
|
|
||
|
demod->mid_time = (hi_time - low_time) / 2 + low_time;
|
||
|
demod->time = 0;
|
||
|
demod->count = 0;
|
||
|
demod->last_pulse = false;
|
||
|
|
||
|
return demod;
|
||
|
}
|
||
|
|
||
|
void fsk_demod_free(FSKDemod* demod) {
|
||
|
free(demod);
|
||
|
}
|
||
|
|
||
|
void fsk_demod_feed(FSKDemod* demod, bool polarity, uint32_t time, bool* value, uint32_t* count) {
|
||
|
*count = 0;
|
||
|
|
||
|
if(polarity) {
|
||
|
// accumulate time
|
||
|
demod->time = time;
|
||
|
} else {
|
||
|
demod->time += time;
|
||
|
|
||
|
// check for valid pulse
|
||
|
if(demod->time >= demod->low_time && demod->time < demod->hi_time) {
|
||
|
bool pulse;
|
||
|
|
||
|
if(demod->time < demod->mid_time) {
|
||
|
pulse = false;
|
||
|
} else {
|
||
|
pulse = true;
|
||
|
}
|
||
|
|
||
|
demod->count++;
|
||
|
|
||
|
// check for edge transition
|
||
|
if(demod->last_pulse != pulse) {
|
||
|
uint32_t data_count = demod->count + 1;
|
||
|
|
||
|
if(demod->last_pulse) {
|
||
|
data_count /= demod->hi_pulses;
|
||
|
*value = !demod->invert;
|
||
|
} else {
|
||
|
data_count /= demod->low_pulses;
|
||
|
*value = demod->invert;
|
||
|
}
|
||
|
|
||
|
*count = data_count;
|
||
|
demod->count = 0;
|
||
|
demod->last_pulse = pulse;
|
||
|
}
|
||
|
} else {
|
||
|
demod->count = 0;
|
||
|
}
|
||
|
}
|
||
|
}
|