89 lines
3.2 KiB
C++
89 lines
3.2 KiB
C++
#pragma once
|
|
|
|
#include <cstdint>
|
|
#include <functional>
|
|
|
|
#include <espchrono.h>
|
|
|
|
#include "bobbycar-serial.h"
|
|
|
|
namespace {
|
|
class FeedbackParser
|
|
{
|
|
public:
|
|
FeedbackParser(const std::reference_wrapper<HardwareSerial> &serial, bool &feedbackValid, bobbycar::protocol::serial::Feedback &feedback) :
|
|
m_serial{serial}, m_feedbackValid{feedbackValid}, m_feedback{feedback}
|
|
{
|
|
}
|
|
|
|
void update()
|
|
{
|
|
using namespace bobbycar::protocol::serial;
|
|
|
|
// Check for new data availability in the Serial buffer
|
|
while (m_serial.get().available())
|
|
{
|
|
m_incomingByte = m_serial.get().read(); // Read the incoming byte
|
|
m_bufStartFrame = ((uint16_t)(m_incomingBytePrev) << 8) + m_incomingByte; // Construct the start frame
|
|
|
|
//Serial.printf("received: %x\r\n", m_incomingByte);
|
|
|
|
// Copy received data
|
|
if (m_bufStartFrame == Feedback::VALID_HEADER) { // Initialize if new data is detected
|
|
m_p = (uint8_t*)&m_newFeedback;
|
|
*m_p++ = m_incomingBytePrev;
|
|
*m_p++ = m_incomingByte;
|
|
m_idx = 2;
|
|
} else if (m_idx >= 2 && m_idx < sizeof(m_newFeedback)) { // Save the new received data
|
|
*m_p++ = m_incomingByte;
|
|
m_idx++;
|
|
}
|
|
|
|
// Check if we reached the end of the package
|
|
if (m_idx == sizeof(m_newFeedback)) {
|
|
uint16_t checksum = calculateChecksum(m_newFeedback);
|
|
|
|
// Check validity of the new data
|
|
if (m_newFeedback.start == Feedback::VALID_HEADER && checksum == m_newFeedback.checksum)
|
|
{
|
|
m_feedbackValid = true;
|
|
m_feedback = m_newFeedback;
|
|
m_lastFeedback = espchrono::millis_clock::now();
|
|
}
|
|
// else
|
|
// {
|
|
// if (m_newFeedback.start == Feedback::VALID_HEADER)
|
|
// //Serial.println("header matched");
|
|
// else
|
|
// //Serial.println("header did not match");
|
|
|
|
// if (checksum == m_newFeedback.checksum)
|
|
// //Serial.println("checksum matched");
|
|
// else
|
|
// //Serial.println("checksum did not match");
|
|
// }
|
|
m_idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
|
|
}
|
|
|
|
// Update previous states
|
|
m_incomingBytePrev = m_incomingByte;
|
|
}
|
|
|
|
if (espchrono::millis_clock::now() - m_lastFeedback > 500)
|
|
m_feedbackValid = false;
|
|
}
|
|
|
|
private:
|
|
uint8_t m_idx = 0; // Index for new data pointer
|
|
uint16_t m_bufStartFrame{}; // Buffer Start Frame
|
|
uint8_t *m_p{}; // Pointer declaration for the new received data
|
|
uint8_t m_incomingByte{};
|
|
uint8_t m_incomingBytePrev{};
|
|
|
|
espchrono::millis_clock::time_point m_lastFeedback{espchrono::millis_clock::now()};
|
|
const std::reference_wrapper<HardwareSerial> &m_serial;
|
|
bool &m_feedbackValid;
|
|
bobbycar::protocol::serial::Feedback &m_feedback, m_newFeedback;
|
|
};
|
|
}
|