This commit is contained in:
DashyFox 2024-02-22 14:01:27 +03:00
parent 462c69ce96
commit 6f5bbac83c
9 changed files with 470 additions and 568 deletions

1
.gitignore vendored
View File

@ -1,2 +1 @@
.vscode/*
!.vscode/arduino.json

View File

@ -1,7 +0,0 @@
{
"board": "GyverCore:avr:nano",
"sketch": "Car.ino",
"port": "COM2",
"configuration": "boot=old,clock=external_16,serial=default,compiler_version=avrgcc5,timers=yes_millis,init=enable,es=disabled,bod=bod_2_7,co=disabled",
"programmer": "arduinoasisp"
}

View File

@ -1,4 +1,4 @@
#include "IR_Decoder.h"
#include "IR_DecoderRaw.h"
#include "IR_Encoder.h"
#include "TimerStatic.h"
#include "MemoryCheck.h"
@ -19,14 +19,12 @@
//////////////// Var /////////////////
IR_Decoder decForward(2, 555);
IR_Decoder decBackward(3, 777);
IR_DecoderRaw decForward(2, 555);
IR_DecoderRaw decBackward(3, 777);
IR_Decoder* blindFromForward [] { &decBackward };
IR_Encoder encForward(42, encForward_PIN, &decForward);
IR_Encoder encBackward(321, encBackward_PIN);
IR_Encoder encTree(325, A2);
IR_Encoder encForward(42, encForward_PIN, &decBackward);
// IR_Encoder encBackward(321, encBackward_PIN);
// IR_Encoder encTree(325, A2);
//////////////////////// Функции прерываний ////////////////////////
@ -41,13 +39,13 @@ void decBackwardISR() {
static uint8_t* portOut;
ISR(TIMER2_COMPA_vect) {
encForward.isr();
encBackward.isr();
// encBackward.isr();
// encTree.isr();
//TODO: Сделать выбор порта
*portOut = (*portOut & 0b11001111) |
(
encForward.ir_out_virtual << 5U
| encBackward.ir_out_virtual << 6U
// | encBackward.ir_out_virtual << 6U
// | encTree.ir_out_virtual << 2U
);
}
@ -179,7 +177,8 @@ void setup() {
encForward.setBlindDecoders(blindFromForward, sizeof(blindFromForward) / sizeof(IR_Decoder*));
IR_DecoderRaw* blindFromForward [] { &decForward, &decBackward };
encForward.setBlindDecoders(blindFromForward, sizeof(blindFromForward) / sizeof(IR_DecoderRaw*));
attachInterrupt(0, decForwardISR, CHANGE); // D2
@ -227,69 +226,64 @@ void loop() {
//test
void status(IR_Decoder& dec) {
// if (dec.gotTune.avaliable()) {
// Serial.print(" Tune "); Serial.print(&dec == &decForward ? "decForward" : (&dec == &decBackward ? "decBackward" : "??")); Serial.print(": ");
void status(IR_DecoderRaw& dec) {
if (dec.available()) {
Serial.println("LOOP RAW DATA");
}
// Serial.println(dec.gotTune.getTune());
// Serial.println();
// dec.gotTune.resetAvaliable();
// IR_DecoderRaw::AnyData* infoArr [] = { &dec.gotData, &dec.gotBackData };
// for (auto&& obj : infoArr) {
// if (obj->available()) {
// String str;
// if (obj->getDataPrt()[1]) {
// str += ("Data on pin "); str += (dec.isrPin); str += "\n";
// uint8_t msg = obj->getMsgRAW();
// str += (" MSG: ");
// for (size_t i = 0; i < 8; i++) {
// if (i == 3) str += " ";
// str += (msg >> (7 - i)) & 1U;
// }
IR_Decoder::AnyData* infoArr [] = { &dec.gotData, &dec.gotBackData };
// str += "\n";
for (auto&& obj : infoArr) {
if (obj->avaliable()) {
String str;
if (obj->getDataPrt()[1]) {
str += ("Data on pin "); str += (dec.isrPin); str += "\n";
// str += (" DATA SIZE: "); str += (obj->getDataSize()); str += "\n";
// str += (" ADDRESS FROM: "); str += (obj->getAddrFrom()); str += "\n";
// str += (" ADDRESS TO: "); str += (obj->getAddrTo()); str += "\n";
// // str += (" CRC PACK: "); str += (obj->getCrcIN()); str += "\n";
// // str += (" CRC CALC: "); str += (obj->getCrcCALC()); str += "\n";
// str += "\n";
uint8_t msg = obj->getMsgRAW();
str += (" MSG: ");
for (size_t i = 0; i < 8; i++) {
if (i == 3) str += " ";
str += (msg >> (7 - i)) & 1U;
}
// for (size_t i = 0; i < obj->getDataSize(); i++) {
// switch (i) {
// // case 0:
// // str += (" ADDR: ");
// // break;
// // case 1:
// // str += (" CMD: ");
// // break;
str += "\n";
str += (" DATA SIZE: "); str += (obj->getDataSize()); str += "\n";
str += (" ADDRESS FROM: "); str += (obj->getAddrFrom()); str += "\n";
str += (" ADDRESS TO: "); str += (obj->getAddrTo()); str += "\n";
// str += (" CRC PACK: "); str += (obj->getCrcIN()); str += "\n";
// str += (" CRC CALC: "); str += (obj->getCrcCALC()); str += "\n";
str += "\n";
for (size_t i = 0; i < obj->getDataSize(); i++) {
switch (i) {
// case 0:
// str += (" ADDR: ");
// break;
// case 1:
// str += (" CMD: ");
// default:
// str += (" Data["); str += (i); str += ("]: ");
// break;
// }
// str += (obj->getDataPrt()[i]); str += "\n";
// }
default:
str += (" Data["); str += (i); str += ("]: ");
break;
}
str += (obj->getDataPrt()[i]); str += "\n";
}
str += ("\n*******ErrAll: "); str += (obj->getErrorCount()); str += "\n";
str += ("**ErrDistance: "); str += ((int)(obj->getErrorHighSignal() - obj->getErrorLowSignal())); str += "\n";
str += "\n";
} else {
str += ("SELF"); str += "\n";
str += "\n";
}
obj->resetAvaliable();
Serial.write(str.c_str());
}
}
// str += ("\n*******ErrAll: "); str += (obj->getErrorCount()); str += "\n";
// str += ("**ErrDistance: "); str += ((int)(obj->getErrorHighSignal() - obj->getErrorLowSignal())); str += "\n";
// str += "\n";
// } else {
// str += ("SELF"); str += "\n";
// str += "\n";
// }
// obj->resetAvailable();
// Serial.write(str.c_str());
// }
// }
}

View File

@ -1,320 +1,230 @@
#pragma once
#include "IR_config.h"
//#define IRDEBUG
#ifdef IRDEBUG
#define wrHigh A3 // Запись HIGH инициирована // green
#define wrLow A3 // Запись LOW инициирована // blue
#define writeOp 13 // Операция записи, 1 пульс для 0 и 2 для 1 // orange
// Исправленные ошибки // purle
// 1 пульс: fix
#define errOut A3
#endif
/////////////////////////////////////////////////////////////////////////////////////////////////
#define riseTime riseSyncTime //* bitTime */ 893U // TODO: Должно высчитываться медианой
#define riseTolerance tolerance /* 250U */ // погрешность
#define riseTimeMax (riseTime + riseTolerance)
#define riseTimeMin (riseTime - riseTolerance)
#define aroundRise(t) (riseTimeMin < t && t < riseTimeMax)
#define IR_timeout (riseTimeMax * (8 + syncBits +1)) // us // таймаут в 8 data + 3 sync + 1
// class IR_Decoder
class IR_Encoder;
class IR_Decoder : private IR_FOX {
friend IR_Encoder;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
class IDataPack : protected IR_FOX {
friend IR_Decoder;
public:
IDataPack(IR_Decoder* dec) : dec(dec) {};
public:
bool avaliable() { if (isAvaliable) { isAvaliable = false; return true; } else { return isAvaliable; }; };
virtual uint8_t getMsgInfo() { return msgPtr[0] & IR_MASK_MSG_INFO; };
virtual uint8_t getMsgType() { return (msgPtr[0] >> 5) & IR_MASK_MSG_TYPE; };
virtual uint8_t getMsgRAW() { return msgPtr[0]; };
virtual uint16_t getErrorCount() { return err.all(); };
virtual uint8_t getErrorLowSignal() { return err.lowSignal; };
virtual uint8_t getErrorHighSignal() { return err.highSignal; };
virtual uint8_t getErrorOther() { return err.other; };
// uint16_t getCrcIN() { return crcPtr[0] << 8 | crcPtr[1]; };
// uint16_t getCrcCALC() { return crcCalcVal; };
virtual uint16_t getTunerTime() { return bitPeriod; };
void resetAvaliable() { isAvaliable = false; };
protected:
inline static uint8_t data[dataByteSizeMax] { 0 };
inline static uint8_t* msgPtr;
inline static uint8_t* addrFromPtr;
inline static uint8_t* addrToPtr;
inline static uint8_t* dataPtr;
inline static uint8_t* crcPtr;
inline static ErrorsStruct err;
inline static uint8_t packRawSize;
inline static uint16_t bitPeriod;
inline static uint16_t crcCalcVal;
IR_Decoder* dec;
bool isAvaliable = false;
virtual bool checkAddress(PackOutInfo* packInfo) {
bool ret;
uint8_t targetAddrOffset = packInfo->offsets.addrToOffset;
uint16_t address = (packInfo->ptr[targetAddrOffset] << 8) | (packInfo->ptr[targetAddrOffset + 1]);
checkaddressRuleApply(address, dec->id, ret);
return ret;
};
virtual void onPackAddressCorrect(PackOutInfo* packInfo) {
memset(IDataPack::data, 0, dataByteSizeMax);
memcpy(data, packInfo->ptr, min(packInfo->packSize, dataByteSizeMax));
err = packInfo->err;
bitPeriod = packInfo->rTime;
crcCalcVal = packInfo->crc;
packRawSize = packInfo->packSize;
msgPtr = (data + packInfo->offsets.msgOffset);
addrFromPtr = (data + packInfo->offsets.addrFromOffset);
addrToPtr = (data + packInfo->offsets.addrToOffset);
dataPtr = (data + packInfo->offsets.dataOffset);
crcPtr = (data + packInfo->offsets.crcOffset);
}
virtual void onPackAddressIncorrect(PackOutInfo* packInfo) {}
virtual void afterSet() {}
void set(PackOutInfo packInfo, bool isNeedAddressCheck) {
isAvaliable = false;
if (!isNeedAddressCheck || checkAddress(&packInfo)) {
onPackAddressCorrect(&packInfo);
#ifdef IRDEBUG_INFO
Serial.println(" OK ");
#endif
isAvaliable = true;
} else {
onPackAddressIncorrect(&packInfo);
#ifdef IRDEBUG_INFO
Serial.println(" NOT MY ");
#endif
}
afterSet();
}
};
public:
///////////////////////////////////////////////////////////////////////////////////////////////////////////
class HasAddrFrom {
public:
virtual uint16_t getAddrFrom() { return IDataPack::addrFromPtr[0] << 8 | IDataPack::addrFromPtr[1]; };
};
class HasAddrTo {
public:
virtual uint16_t getAddrTo() { return IDataPack::addrToPtr[0] << 8 | IDataPack::addrToPtr[1]; };
};
class HasData {
public:
virtual uint8_t getDataSize() { return (uint8_t)(IDataPack::crcPtr - IDataPack::dataPtr); };
virtual uint8_t* getDataPrt() { return IDataPack::dataPtr; };
virtual uint8_t getDataRawSize() { return IDataPack::packRawSize; };
virtual uint8_t* getDataRawPtr() { return IDataPack::data; };
};
class IAcceptable {
public:
virtual bool isNeedAccept();
};
class AnyData : public IDataPack, public HasAddrFrom, public HasAddrTo, public HasData {
using IDataPack::IDataPack;
};
///////////////////////////////////////////////////////////////////////////////////////////////////////////
class Data : public AnyData, public IAcceptable {
using AnyData::AnyData;
public:
bool isNeedAccept() override { return ((IDataPack::msgPtr[0] >> 5) & IR_MASK_MSG_TYPE) == IR_MSG_DATA_ACCEPT; };
private:
void afterSet() override {
if (Data::IDataPack::isAvaliable && isNeedAccept()) {
dec->isWaitingAcceptSend = true;
dec->addrWaitingSendgTo = getAddrFrom();
Serial.print("\n Need to accept to ");
Serial.println(dec->addrWaitingSendgTo);
}
}
};
class BackData : public AnyData {
using AnyData::AnyData;
public:
bool isAddressed() { return IDataPack::msgPtr[0] & 0b00010000; };
uint8_t getMsgInfo() { return IDataPack::msgPtr[0] & (IR_MASK_MSG_INFO >> 1); };
uint16_t getAddrTo() override {
if (isAddressed()) {
return IDataPack::addrToPtr[0] << 8 | IDataPack::addrToPtr[1];
} else {
return IR_Broadcast;
}
};
};
class Accept : public IDataPack, public HasAddrFrom {
using IDataPack::IDataPack;
public:
private:
bool checkAddress(PackOutInfo* packInfo) override {
bool ret;
uint8_t targetAddrOffset = packInfo->offsets.addrFromOffset;
uint16_t address = (packInfo->ptr[targetAddrOffset] << 8) | (packInfo->ptr[targetAddrOffset + 1]);
ret = address == dec->addrWaitingFrom;
return ret;
}
};
class Request : public IDataPack, public HasAddrFrom, public HasAddrTo {
using IDataPack::IDataPack;
public:
private:
// class IDataPack : protected IR_FOX {
// friend IR_Decoder;
// public:
// IDataPack(IR_Decoder* dec) : dec(dec) {};
// public:
// bool available() { if (isAvailable) { isAvailable = false; return true; } else { return isAvailable; }; };
// virtual uint8_t getMsgInfo() { return msgPtr[0] & IR_MASK_MSG_INFO; };
// virtual uint8_t getMsgType() { return (msgPtr[0] >> 5) & IR_MASK_MSG_TYPE; };
// virtual uint8_t getMsgRAW() { return msgPtr[0]; };
// virtual uint16_t getErrorCount() { return err.all(); };
// virtual uint8_t getErrorLowSignal() { return err.lowSignal; };
// virtual uint8_t getErrorHighSignal() { return err.highSignal; };
// virtual uint8_t getErrorOther() { return err.other; };
// // uint16_t getCrcIN() { return crcPtr[0] << 8 | crcPtr[1]; };
// // uint16_t getCrcCALC() { return crcCalcVal; };
// virtual uint16_t getTunerTime() { return bitPeriod; };
// void resetAvailable() { isAvailable = false; };
// protected:
// inline static uint8_t data[dataByteSizeMax] { 0 };
// inline static uint8_t* msgPtr;
// inline static uint8_t* addrFromPtr;
// inline static uint8_t* addrToPtr;
// inline static uint8_t* dataPtr;
// inline static uint8_t* crcPtr;
// inline static ErrorsStruct err;
// inline static uint8_t packRawSize;
// inline static uint16_t bitPeriod;
// inline static uint16_t crcCalcVal;
// IR_Decoder* dec;
// bool isAvailable = false;
// virtual bool checkAddress(PackOutInfo* packInfo) {
// bool ret;
// uint8_t targetAddrOffset = packInfo->offsets.addrToOffset;
// uint16_t address = (packInfo->ptr[targetAddrOffset] << 8) | (packInfo->ptr[targetAddrOffset + 1]);
// checkaddressRuleApply(address, dec->id, ret);
// return ret;
// };
// virtual void onPackAddressCorrect(PackOutInfo* packInfo) {
// memset(IDataPack::data, 0, dataByteSizeMax);
// memcpy(data, packInfo->ptr, min(packInfo->packSize, dataByteSizeMax));
// err = packInfo->err;
// bitPeriod = packInfo->rTime;
// crcCalcVal = packInfo->crc;
// packRawSize = packInfo->packSize;
// msgPtr = (data + packInfo->offsets.msgOffset);
// addrFromPtr = (data + packInfo->offsets.addrFromOffset);
// addrToPtr = (data + packInfo->offsets.addrToOffset);
// dataPtr = (data + packInfo->offsets.dataOffset);
// crcPtr = (data + packInfo->offsets.crcOffset);
// }
// virtual void onPackAddressIncorrect(PackOutInfo* packInfo) {}
// virtual void afterSet() {}
// void set(PackOutInfo packInfo, bool isNeedAddressCheck) {
// isAvailable = false;
// if (!isNeedAddressCheck || checkAddress(&packInfo)) {
// onPackAddressCorrect(&packInfo);
// #ifdef IRDEBUG_INFO
// Serial.println(" OK ");
// #endif
// isAvailable = true;
// } else {
// onPackAddressIncorrect(&packInfo);
// #ifdef IRDEBUG_INFO
// Serial.println(" NOT MY ");
// #endif
// }
// afterSet();
// }
// };
// public:
// ///////////////////////////////////////////////////////////////////////////////////////////////////////////
// class HasAddrFrom {
// public:
// virtual uint16_t getAddrFrom() { return IDataPack::addrFromPtr[0] << 8 | IDataPack::addrFromPtr[1]; };
// };
// class HasAddrTo {
// public:
// virtual uint16_t getAddrTo() { return IDataPack::addrToPtr[0] << 8 | IDataPack::addrToPtr[1]; };
// };
// class HasData {
// public:
// virtual uint8_t getDataSize() { return (uint8_t)(IDataPack::crcPtr - IDataPack::dataPtr); };
// virtual uint8_t* getDataPrt() { return IDataPack::dataPtr; };
// virtual uint8_t getDataRawSize() { return IDataPack::packRawSize; };
// virtual uint8_t* getDataRawPtr() { return IDataPack::data; };
// };
// class IAcceptable {
// public:
// virtual bool isNeedAccept();
// };
// class AnyData : public IDataPack, public HasAddrFrom, public HasAddrTo, public HasData {
// using IDataPack::IDataPack;
// };
// ///////////////////////////////////////////////////////////////////////////////////////////////////////////
// class Data : public AnyData, public IAcceptable {
// using AnyData::AnyData;
// public:
// bool isNeedAccept() override { return ((IDataPack::msgPtr[0] >> 5) & IR_MASK_MSG_TYPE) == IR_MSG_DATA_ACCEPT; };
// private:
// void afterSet() override {
// if (isAvaliable) {
// dec->isWaitingAcceptSend = true;
// if (Data::IDataPack::isAvailable && isNeedAccept()) {
// dec->addrWaitingSendgTo = getAddrFrom();
// dec->startWaitingSendToTime = millis();
// dec->isWaitingAcceptSend = true;
// Serial.print("\n Need to accept to ");
// Serial.println(dec->addrWaitingSendgTo);
// }
// }
};
// };
class RawTune {
friend IR_Decoder;
public:
bool avaliable() { return isAvaliable; };
uint16_t getTunerTime() { return bitPeriod; };
void resetAvaliable() { isAvaliable = false; };
protected:
bool isAvaliable;
uint16_t bitPeriod;
// class BackData : public AnyData {
// using AnyData::AnyData;
// public:
// bool isAddressed() { return IDataPack::msgPtr[0] & 0b00010000; };
// uint8_t getMsgInfo() { return IDataPack::msgPtr[0] & (IR_MASK_MSG_INFO >> 1); };
// uint16_t getAddrTo() override {
// if (isAddressed()) {
// return IDataPack::addrToPtr[0] << 8 | IDataPack::addrToPtr[1];
// } else {
// return IR_Broadcast;
// }
// };
// };
void set(uint16_t time) {
bitPeriod = time;
isAvaliable = true;
}
};
// class Accept : public IDataPack, public HasAddrFrom {
// using IDataPack::IDataPack;
// public:
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// private:
// bool checkAddress(PackOutInfo* packInfo) override {
// bool ret;
// uint8_t targetAddrOffset = packInfo->offsets.addrFromOffset;
// uint16_t address = (packInfo->ptr[targetAddrOffset] << 8) | (packInfo->ptr[targetAddrOffset + 1]);
// ret = address == dec->addrWaitingFrom;
// return ret;
// }
// };
public:
uint16_t id;
// class Request : public IDataPack, public HasAddrFrom, public HasAddrTo {
// using IDataPack::IDataPack;
// public:
Data gotData = Data(this); /// @brief Контейнер с данными
BackData gotBackData = BackData(this);
Accept gotAccept = Accept(this); /// @brief Контейнер с подтверждением
Request gotRequest = Request(this); /// @brief Контейнер с запросом
RawTune gotTune; /// @brief Контейнер с информацией подстройки
// private:
// // void afterSet() override {
// // if (isAvailable) {
// // dec->isWaitingAcceptSend = true;
// // dec->addrWaitingSendgTo = getAddrFrom();
// // Serial.print("\n Need to accept to ");
// // Serial.println(dec->addrWaitingSendgTo);
// // }
// // }
// };
const uint8_t isrPin; // Пин прерывания
// class RawTune {
// friend IR_Decoder;
// public:
// bool available() { return isAvailable; };
// uint16_t getTunerTime() { return bitPeriod; };
// void resetAvailable() { isAvailable = false; };
// protected:
// bool isAvailable;
// uint16_t bitPeriod;
//////////////////////////////////////////////////////////////////////////
/// @brief Конструктор
/// @param isrPin Номер вывода прерывания/данных от приёмника (2 или 3 для atmega 328p)
/// @param addr Адрес приёмника
/// @param encPair Указатель на передатчик, работающий в паре
IR_Decoder(const uint8_t isrPin, uint16_t addr, IR_Encoder* encPair = nullptr);
// void set(uint16_t time) {
// bitPeriod = time;
// isAvailable = true;
// }
// };
~IR_Decoder();
void isr(); ///@brief Функция прерывания
void tick(); /// @brief Обработка приёмника, необходима для работы
// @return Буффер переполнился
bool isOverflow() { return isBufferOverflow; };
/// @brief Флаг приёма
/// @return Возвращает true, если происходит приём пакета
// bool isReciving() { return isRecive; }; //TODO: Некорректно работает
// @brief Слушатель для работы isReciving()
void listen();
//////////////////////////////////////////////////////////////////////////
bool isWaitingAccept = false; // Флаг ожидания подтверждения
bool isWaitingAcceptSend = false; // Флаг ожидания отправки подтверждения
uint16_t addrWaitingFrom = 0; // Адрес, от кого ожидается подтверждение
uint16_t addrWaitingSendgTo = 0; // Адрес, кому нужно отправить подтверждение
private:
IR_Encoder* encoder; // Указатель на парный передатчик
volatile uint16_t isPairSending = 0; // Флаг передачи парного передатчика
volatile bool isRecive = false; // Флаг приёма
volatile bool isPreamb = false; // флаг начальной последовости
bool isCrcCorrect = false; // Флаг корректности crc
bool isBufferOverflow = false; // Флаг переполнения буффера данных
bool isWrongPack = false; // Флаг битого пакета
uint16_t riseSyncTime = bitTime; // Подстраиваемое время бита в мкс
////////////////////////////////////////////////////////////////////////
volatile uint8_t currentSubBufferIndex; // Счетчик текущей позиции во вспомогательном буфере фронтов/спадов
struct FrontStorage { // Структура для хранения времени и направления фронта/спада
volatile uint32_t time = 0; // Время
volatile bool dir = false; // Направление (true = ↑; false = ↓)
volatile FrontStorage* next = nullptr; // Указатель на следующий связанный фронт/спад, или nullptr если конец
};
volatile FrontStorage* lastFront = nullptr; // Указатель последнего фронта/спада
volatile FrontStorage* firstUnHandledFront = nullptr; // Указатель первого необработанного фронта/спада
volatile FrontStorage subBuffer[subBufferSize]; // вспомогательный буфер для хранения необработанных фронтов/спадов
////////////////////////////////////////////////////////////////////////
uint8_t* dataBuffer = nullptr; // Указатель на буффер данных
uint32_t prevRise, prevPrevRise, prevFall, prevPrevFall; // Время предыдущих фронтов/спадов
uint16_t errorCounter = 0; // Счётчик ошибок
int8_t preambFrontCounter = 0; // Счётчик __/``` ↑ преамбулы
int16_t bufBitPos = 0; // Позиция для записи бита в буффер
private:
/// @brief Проверка CRC. Проверяет len байт со значением crc, пришедшим в пакете
/// @param len Длина в байтах проверяемых данных
/// @param crc Результат рассчёта crc (Выходной параметр)
/// @return true если crc верно
bool crcCheck(uint8_t len, uint16_t& crc);
////////////////////////////////////////////////////////////////////////
bool isData = true; // Флаг относится ли бит к данным, или битам синхронизации
uint16_t i_dataBuffer; // Счётчик буфера данных
uint8_t nextControlBit = bitPerByte; // Метка для смены флага isData
uint8_t i_syncBit; // Счётчик битов синхронизации
uint8_t err_syncBit; // Счётчик ошибок синхронизации
/// @brief Запиь бита в буффер, а так же проверка битов синхранизации и их фильтрация
/// @param Бит данных
void writeToBuffer(bool);
void packToOutClass(uint8_t packSize, IDataPack& obj, PackOffsets offsets, bool isNeedAddressCheck = true);
////////////////////////////////////////////////////////////////////////
/// @brief Установка и сброс начальных значений и флагов в готовность к приёму данных
void firstRX();
/// @brief Целочисленное деление с округлением вверх
/// @param val Значение
/// @param divider Делитель
/// @return Результат
uint16_t ceil_div(uint16_t val, uint16_t divider);
#ifdef IRDEBUG
inline void errPulse(uint8_t pin, uint8_t count);
inline void infoPulse(uint8_t pin, uint8_t count);
#endif
};
// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

View File

@ -1,19 +1,17 @@
#include "IR_Decoder.h"
#include "IR_DecoderRaw.h"
#include "IR_Encoder.h"
IR_Decoder::IR_Decoder(const uint8_t isrPin, uint16_t addr, IR_Encoder* encPair = nullptr) : isrPin(isrPin), id(addr), encoder(encPair) {
dataBuffer = new uint8_t[dataByteSizeMax] { 0 };
IR_DecoderRaw::IR_DecoderRaw(const uint8_t isrPin, uint16_t addr, IR_Encoder* encPair = nullptr) : isrPin(isrPin), id(addr), encoder(encPair) {
prevRise = prevFall = prevPrevFall = prevPrevRise = 0;
if (encPair != nullptr) {
encPair->decPair = this;
}
IR_Decoder::~IR_Decoder() {
delete dataBuffer;
}
//////////////////////////////////// isr ///////////////////////////////////////////
void IR_Decoder::isr() {
void IR_DecoderRaw::isr() {
if (isPairSending) return;
subBuffer[currentSubBufferIndex].next = nullptr;
@ -40,22 +38,22 @@ void IR_Decoder::isr() {
lastFront = &subBuffer[currentSubBufferIndex];
}
currentSubBufferIndex == (subBufferSize - 1) ? currentSubBufferIndex = 0 : currentSubBufferIndex++; // Закольцовка буффера
}
////////////////////////////////////////////////////////////////////////////////////
void IR_Decoder::firstRX() {
void IR_DecoderRaw::firstRX() {
#ifdef IRDEBUG_INFO
Serial.print("\n>");
#endif
errors.reset();
packSize = 0;
isBufferOverflow = false;
isCrcCorrect = false;
isAvailable = false;
bufBitPos = 0;
isData = true;
i_dataBuffer = 0;
@ -69,17 +67,17 @@ void IR_Decoder::firstRX() {
memset(dataBuffer, 0x00, dataByteSizeMax);
}
void IR_Decoder::listen() {
void IR_DecoderRaw::listenEnd() {
if (isRecive && ((micros() - prevRise) > IR_timeout * 2)) {
isRecive = false;
firstRX();
}
}
void IR_Decoder::tick() {
void IR_DecoderRaw::tick() {
FrontStorage currentFront;
noInterrupts();
listen();
listenEnd();
if (firstUnHandledFront == nullptr) { interrupts(); return; } //Если данных нет - ничего не делаем
currentFront = *((FrontStorage*)firstUnHandledFront); //найти следующий необработанный фронт/спад
interrupts();
@ -116,7 +114,7 @@ void IR_Decoder::tick() {
// Serial.print("preambFrontCounter: "); Serial.println(preambFrontCounter);
} else {
if (isPreamb) {// первый фронт после
gotTune.set(riseSyncTime);
// gotTune.set(riseSyncTime);
}
isPreamb = false;
}
@ -269,7 +267,7 @@ void IR_Decoder::tick() {
firstUnHandledFront = firstUnHandledFront->next; //переместить флаг на следующий элемент для обработки (next or nullptr)
}
void IR_Decoder::writeToBuffer(bool bit) {
void IR_DecoderRaw::writeToBuffer(bool bit) {
if (i_dataBuffer > dataByteSizeMax * 8) {// проверка переполнения
//TODO: Буффер переполнен!
#ifdef IRDEBUG_INFO
@ -290,15 +288,15 @@ void IR_Decoder::writeToBuffer(bool bit) {
#endif
}
if (isData) { // Запись битов в dataBuffer
#ifdef IRDEBUG_INFO
Serial.print(bit);
#endif
if (i_dataBuffer % 8 == 7) {
// Serial.print("+");
}
// if (i_dataBuffer % 8 == 7) {
// // Serial.print("+");
// }
dataBuffer[(i_dataBuffer / 8)] |= bit << (7 - i_dataBuffer % 8); // Запись в буффер
i_dataBuffer++;
bufBitPos++;
@ -349,130 +347,23 @@ void IR_Decoder::writeToBuffer(bool bit) {
}
#endif
if ((i_dataBuffer >= (8 * msgBytes)) && !isCrcCorrect) {
uint16_t crcValue;
uint8_t packSize;
switch ((dataBuffer[0] >> 5) & IR_MASK_MSG_TYPE) {
case IR_MSG_BACK:
packSize = (dataBuffer[0] & (IR_MASK_MSG_INFO >> 1)) + crcBytes;
if (i_dataBuffer != packSize * bitPerByte) break;
if ((dataBuffer[0] & 0b00010000)) { // С адресацией
packToOutClass(
packSize, // packSize
gotBackData, // obj
PackOffsets {
0, // msgOffset
1, // addrFromOffset
3, // addrToOffset
5, // dataOffset
NULL // crcOffset
},
true
);
} else { // Без адресации
packToOutClass(
packSize, // packSize
gotBackData, // obj
PackOffsets {
0, // msgOffset
1, // addrFromOffset
3, // addrToOffset
3, // dataOffset
NULL // crcOffset
},
false
);
}
break;
case IR_MSG_ACCEPT:
packSize = msgBytes + addrBytes + crcBytes;
if (i_dataBuffer != packSize * bitPerByte) break;
packToOutClass(
packSize, // packSize
gotAccept, // obj
PackOffsets {
0, // msgOffset
1, // addrFromOffset
NULL, // addrToOffset
NULL, // dataOffset
3 // crcOffset
}
);
break;
case IR_MSG_REQUEST:
packSize = msgBytes + addrBytes + addrBytes + crcBytes;
if (i_dataBuffer != packSize * bitPerByte) break;
packToOutClass(
packSize, // packSize
gotRequest, // obj
PackOffsets {
0, // msgOffset
1, // addrFromOffset
3, // addrToOffset
NULL, // dataOffset
5 // crcOffset
}
);
break;
case IR_MSG_DATA_ACCEPT:
case IR_MSG_DATA_NOACCEPT:
if (!isAvailable && isData) {
if (i_dataBuffer == 8 * msgBytes) {// Ппервый байт
packSize = (dataBuffer[0] & IR_MASK_MSG_INFO) + crcBytes;
if (i_dataBuffer != packSize * bitPerByte) break;
packToOutClass(
packSize, // packSize
gotData, // obj
PackOffsets {
0, // msgOffset
1, // addrFromOffset
3, // addrToOffset
5, // dataOffset
NULL // crcOffset
Serial.print(" ["); Serial.print(packSize); Serial.print("] ");
}
);
break;
default:
break;
if (packSize && (i_dataBuffer == packSize*bitPerByte)) { // Конец
isAvailable = crcCheck(packSize - crcBytes, crcValue);
Serial.print(" END DATA ");
isRecive = false;
}
}/**/
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
void IR_Decoder::packToOutClass(uint8_t packSize, IDataPack& obj, PackOffsets offsets, bool isNeedAddressCheck = true) {
PackOutInfo packInfo;
#ifdef IRDEBUG_INFO
Serial.print(" IN ");
#endif
isCrcCorrect = crcCheck(packSize - crcBytes, packInfo.crc);
if (isCrcCorrect) {
packInfo.ptr = dataBuffer;
packInfo.packSize = packSize;
packInfo.offsets = offsets;
packInfo.offsets.crcOffset = packInfo.packSize - crcBytes;
packInfo.err = errors;
packInfo.rTime = riseSyncTime;
obj.set(packInfo, isNeedAddressCheck);
} else {
#ifdef IRDEBUG_INFO
Serial.println(" CRC WRONG ");
#endif
}
isRecive = false;
}
bool IR_Decoder::crcCheck(uint8_t len, crc_t& crc) {
bool IR_DecoderRaw::crcCheck(uint8_t len, crc_t& crc) {
bool crcOK = false;
crc = 0;
@ -493,7 +384,7 @@ bool IR_Decoder::crcCheck(uint8_t len, crc_t& crc) {
return crcOK;
}
uint16_t IR_Decoder::ceil_div(uint16_t val, uint16_t divider) {
uint16_t IR_DecoderRaw::ceil_div(uint16_t val, uint16_t divider) {
int ret = val / divider;
if ((val << 4) / divider - (ret << 4) >= 8)
ret++;
@ -504,7 +395,7 @@ uint16_t IR_Decoder::ceil_div(uint16_t val, uint16_t divider) {
// IRDEBUG FUNC
#ifdef IRDEBUG
inline void IR_Decoder::errPulse(uint8_t pin, uint8_t count) {
inline void IR_DecoderRaw::errPulse(uint8_t pin, uint8_t count) {
for (size_t i = 0; i < count; i++) {
digitalWrite(pin, 1);
digitalWrite(pin, 0);
@ -512,7 +403,7 @@ inline void IR_Decoder::errPulse(uint8_t pin, uint8_t count) {
digitalWrite(pin, 0);
}
inline void IR_Decoder::infoPulse(uint8_t pin, uint8_t count) {
inline void IR_DecoderRaw::infoPulse(uint8_t pin, uint8_t count) {
for (size_t i = 0; i < count; i++) {
digitalWrite(pin, 1);
digitalWrite(pin, 0);

114
IR_DecoderRaw.h Normal file
View File

@ -0,0 +1,114 @@
#pragma once
#include "IR_config.h"
//#define IRDEBUG
#ifdef IRDEBUG
#define wrHigh A3 // Запись HIGH инициирована // green
#define wrLow A3 // Запись LOW инициирована // blue
#define writeOp 13 // Операция записи, 1 пульс для 0 и 2 для 1 // orange
// Исправленные ошибки // purle
// 1 пульс: fix
#define errOut A3
#endif
/////////////////////////////////////////////////////////////////////////////////////////////////
#define riseTime riseSyncTime //* bitTime */ 893U // TODO: Должно высчитываться медианой
#define riseTolerance tolerance /* 250U */ // погрешность
#define riseTimeMax (riseTime + riseTolerance)
#define riseTimeMin (riseTime - riseTolerance)
#define aroundRise(t) (riseTimeMin < t && t < riseTimeMax)
#define IR_timeout (riseTimeMax * (8 + syncBits +1)) // us // таймаут в 8 data + 3 sync + 1
class IR_Encoder;
class IR_DecoderRaw : private IR_FOX {
friend IR_Encoder;
public:
const uint8_t isrPin; // Пин прерывания
//////////////////////////////////////////////////////////////////////////
/// @brief Конструктор
/// @param isrPin Номер вывода прерывания/данных от приёмника (2 или 3 для atmega 328p)
/// @param addr Адрес приёмника
/// @param encPair Указатель на передатчик, работающий в паре
IR_DecoderRaw(const uint8_t isrPin, uint16_t addr, IR_Encoder* encPair = nullptr);
void isr(); // Функция прерывания
void tick(); // Обработка приёмника, необходима для работы
bool available() { if(isAvailable) {isAvailable = false; return true;} else { return false;}};
bool isOverflow() { return isBufferOverflow; }; // Буффер переполнился
bool isReciving() { return isBufferOverflow; }; // Возвращает true, если происходит приём пакета
//////////////////////////////////////////////////////////////////////////
private:
uint16_t id;
bool isAvailable = false;
uint16_t packSize;
uint16_t crcValue;
IR_Encoder* encoder; // Указатель на парный передатчик
volatile uint16_t isPairSending = 0; // Флаг передачи парного передатчика
volatile bool isRecive = false; // Флаг приёма
volatile bool isPreamb = false; // флаг начальной последовости
bool isBufferOverflow = false; // Флаг переполнения буффера данных
bool isWrongPack = false; // Флаг битого пакета
uint16_t riseSyncTime = bitTime; // Подстраиваемое время бита в мкс
////////////////////////////////////////////////////////////////////////
volatile uint8_t currentSubBufferIndex; // Счетчик текущей позиции во вспомогательном буфере фронтов/спадов
struct FrontStorage { // Структура для хранения времени и направления фронта/спада
volatile uint32_t time = 0; // Время
volatile bool dir = false; // Направление (true = ↑; false = ↓)
volatile FrontStorage* next = nullptr; // Указатель на следующий связанный фронт/спад, или nullptr если конец
};
volatile FrontStorage* lastFront = nullptr; // Указатель последнего фронта/спада
volatile FrontStorage* firstUnHandledFront = nullptr; // Указатель первого необработанного фронта/спада
volatile FrontStorage subBuffer[subBufferSize]; // вспомогательный буфер для хранения необработанных фронтов/спадов
////////////////////////////////////////////////////////////////////////
uint8_t dataBuffer[dataByteSizeMax]{0}; // Буффер данных
uint32_t prevRise, prevPrevRise, prevFall, prevPrevFall; // Время предыдущих фронтов/спадов
uint16_t errorCounter = 0; // Счётчик ошибок
int8_t preambFrontCounter = 0; // Счётчик __/``` ↑ преамбулы
int16_t bufBitPos = 0; // Позиция для записи бита в буффер
private:
void listenEnd(); // @brief Слушатель для работы isReciving()
/// @brief Проверка CRC. Проверяет len байт со значением crc, пришедшим в пакете
/// @param len Длина в байтах проверяемых данных
/// @param crc Результат рассчёта crc (Выходной параметр)
/// @return true если crc верно
bool crcCheck(uint8_t len, uint16_t& crc);
////////////////////////////////////////////////////////////////////////
bool isData = true; // Флаг относится ли бит к данным, или битам синхронизации
uint16_t i_dataBuffer; // Счётчик буфера данных
uint8_t nextControlBit = bitPerByte; // Метка для смены флага isData
uint8_t i_syncBit; // Счётчик битов синхронизации
uint8_t err_syncBit; // Счётчик ошибок синхронизации
/// @brief Запиь бита в буффер, а так же проверка битов синхранизации и их фильтрация
/// @param Бит данных
void writeToBuffer(bool);
////////////////////////////////////////////////////////////////////////
void firstRX(); /// @brief Установка и сброс начальных значений и флагов в готовность к приёму данных
/// @brief Целочисленное деление с округлением вверх
/// @param val Значение
/// @param divider Делитель
/// @return Результат
uint16_t ceil_div(uint16_t val, uint16_t divider);
#ifdef IRDEBUG
inline void errPulse(uint8_t pin, uint8_t count);
inline void infoPulse(uint8_t pin, uint8_t count);
#endif
};

View File

@ -1,23 +1,26 @@
#include "IR_Encoder.h"
#include "IR_Decoder.h"
#include "IR_DecoderRaw.h"
#define LoopOut 12
#define ISR_Out 10
#define TestOut 13
IR_Encoder::IR_Encoder(uint16_t addr, uint8_t pin, IR_Decoder* decPair = nullptr) {
IR_Encoder::IR_Encoder(uint16_t addr, uint8_t pin, IR_DecoderRaw* decPair = nullptr) {
id = addr;
this->decPair = decPair;
signal = noSignal;
isSending = false;
#if disablePairDec
if (decPair != nullptr) {
blindDecoders = new IR_Decoder * [1] {decPair};
blindDecoders = new IR_DecoderRaw * [1] {decPair};
decodersCount = 1;
}
#endif
if (decPair != nullptr) {
decPair->encoder = this;
}
};
void IR_Encoder::setBlindDecoders(IR_Decoder* decoders [], uint8_t count) {
void IR_Encoder::setBlindDecoders(IR_DecoderRaw* decoders [], uint8_t count) {
#if disablePairDec
if (blindDecoders != nullptr) delete [] blindDecoders;
#endif
@ -58,26 +61,29 @@ void IR_Encoder::sendData(uint16_t addrTo, uint8_t* data, uint8_t len, bool need
sendBuffer[packSize - crcBytes] = crc8(sendBuffer, 0, packSize - crcBytes, poly1) & 0xFF;
sendBuffer[packSize - crcBytes + 1] = crc8(sendBuffer, 0, packSize - crcBytes + 1, poly2) & 0xFF;
if (decPair != nullptr) {
decPair->isWaitingAccept = ((msgType >> 5) & IR_MASK_MSG_TYPE == IR_MSG_DATA_ACCEPT);
if (decPair->isWaitingAccept) {
decPair->addrWaitingFrom = addrTo;
}
}
// if (decPair != nullptr) {
// decPair->isWaitingAccept = ((msgType >> 5) & IR_MASK_MSG_TYPE == IR_MSG_DATA_ACCEPT);
// if (decPair->isWaitingAccept) {
// decPair->addrWaitingFrom = addrTo;
// }
// }
// отправка
rawSend(sendBuffer, packSize);
}
void IR_Encoder::sendAccept(uint16_t addrTo, uint8_t addInfo, bool forAll = false) {
void IR_Encoder::sendAccept(uint16_t addrTo, uint8_t addInfo) {
memset(sendBuffer, 0x00, dataByteSizeMax);
sendBuffer[0] = IR_MSG_ACCEPT << 5;
sendBuffer[0] |= addInfo & IR_MASK_MSG_INFO;
// addr_self
if (!forAll) {
sendBuffer[1] = id >> 8 & 0xFF;
sendBuffer[2] = id & 0xFF;
}
Serial.print("\nRAW Accept to ");
Serial.println(addrTo);
// data crc
sendBuffer[3] = crc8(sendBuffer, 0, 3, poly1) & 0xFF;

View File

@ -3,9 +3,9 @@
//TODO: Отложенная передача после завершения приема
class IR_Decoder;
class IR_DecoderRaw;
class IR_Encoder : IR_FOX {
friend IR_Decoder;
friend IR_DecoderRaw;
public:
uint16_t id; /// @brief Адрес передатчика
@ -18,7 +18,7 @@ public:
/// @param pin Вывод передатчика
/// @param tune Подстройка несущей частоты
/// @param decPair Приёмник, для которого отключается приём в момент передачи передатчиком
IR_Encoder(uint16_t addr, uint8_t pin, IR_Decoder* decPair = nullptr);
IR_Encoder(uint16_t addr, uint8_t pin, IR_DecoderRaw* decPair = nullptr);
static void timerSetup() {
// TIMER2 Ini
@ -42,10 +42,10 @@ public:
SREG = oldSREG; // Return interrupt settings
}
void IR_Encoder::setBlindDecoders(IR_Decoder* decoders [], uint8_t count);
void IR_Encoder::setBlindDecoders(IR_DecoderRaw* decoders [], uint8_t count);
void rawSend(uint8_t* ptr, uint8_t len);
void sendData(uint16_t addrTo, uint8_t* data, uint8_t len, bool needAccept = false);
void sendAccept(uint16_t addrTo, uint8_t addInfo = 0, bool forAll = false);
void sendAccept(uint16_t addrTo, uint8_t addInfo = 0);
void sendRequest(uint16_t addrTo, uint8_t addInfo = 0);
void sendBack(uint8_t* data = nullptr, uint8_t len = 0);
void sendBackTo(uint16_t addrTo, uint8_t* data = nullptr, uint8_t len = 0);
@ -70,8 +70,8 @@ private:
sync = 3
};
IR_Decoder* decPair;
IR_Decoder** blindDecoders;
IR_DecoderRaw* decPair;
IR_DecoderRaw** blindDecoders;
uint8_t decodersCount;
uint8_t sendLen;

View File

@ -40,18 +40,14 @@ msg type:
                                        // | xxx..... | = тип сообщения
                                        // | ...xxxxx | = длина (максимум 31 бита)
                                        //  ---------- */
#define IR_MSG_BACK 0U // | 0000B.... | = Задний сигнал машинки
;// // | \\\x---- | = нужна ли адресация
;// // | \\\-xxxx | = длина данных (Равна нулю при отсутствии сквозных команд)
#define IR_MSG_BACK 0U // | 000...... | = Задний сигнал машинки
#define IR_MSG_ACCEPT 1U // | 001..... | = подтверждение
#define IR_MSG_REQUEST 2U // | 010..... | = запрос
#define IR_MSG_ 3U // | 011..... | = ??
#define IR_MSG_ 4U // | 100..... | = ??
#define IR_MSG_BACK_TO 4U // | 100..... | = Задний сигнал машинки c адресацией
#define IR_MSG_ 5U // | 101..... | = ??
#define IR_MSG_DATA_NOACCEPT 6U // | 110..... | = данные, не требующие подтверждения
;// // | \\\xxxxx | = длина данных
#define IR_MSG_DATA_ACCEPT 7U // | 111..... | = данные требующие подтверждения
;//                                     // | \\\xxxxx | = длина данных
/*   // ----------
/```````````````````` подтверждение ```````````````````\      /``````````````````````````````````````` запрос ``````````````````````````````````\
@ -163,10 +159,9 @@ public:
};
struct PackOutInfo {
struct PackInfo {
uint8_t* ptr;
uint8_t packSize;
PackOffsets offsets;
uint16_t crc;
ErrorsStruct err;
uint16_t rTime;