Skip to content

Commit 982f51d

Browse files
askuricaskuric
committed
an error in the resolution
1 parent f78262e commit 982f51d

3 files changed

Lines changed: 140 additions & 4 deletions

File tree

‎src/comms/streams/BinaryIOUnbuffered.cpp‎

Lines changed: 100 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
// #include "./BinaryIOUnbuffered.h"
2+
3+
// BinaryIOUnbuffered::BinaryIOUnbuffered(Stream& io) : _io(io) {
4+
// // nothing to do here
5+
// };
6+
7+
// BinaryIOUnbuffered::~BinaryIOUnbuffered(){
8+
// // nothing to do here
9+
// };
10+
11+
12+
// BinaryIOUnbuffered& BinaryIOUnbuffered::operator<<(float value) {
13+
// _io.write((uint8_t*)&value, 4);
14+
// return *this;
15+
// };
16+
17+
18+
// BinaryIOUnbuffered& BinaryIOUnbuffered::operator<<(uint32_t value) {
19+
// _io.write((uint8_t*)&value, 4);
20+
// return *this;
21+
// };
22+
23+
24+
// BinaryIOUnbuffered& BinaryIOUnbuffered::operator<<(uint8_t value) {
25+
// _io.write(value);
26+
// return *this;
27+
// };
28+
29+
30+
// BinaryIOUnbuffered& BinaryIOUnbuffered::operator<<(char value) {
31+
// _io.write((uint8_t)value);
32+
// return *this;
33+
// };
34+
35+
36+
// BinaryIOUnbuffered& BinaryIOUnbuffered::operator<<(Packet value) {
37+
// if (value.type!=0x00) {
38+
// _io.write(MARKER_BYTE);
39+
// _io.write(value.payload_size+1);
40+
// _io.write(value.type);
41+
// }
42+
// return *this;
43+
// };
44+
45+
46+
// BinaryIOUnbuffered& BinaryIOUnbuffered::operator<<(Separator value) {
47+
// // separator is ignored in binary mode
48+
// return *this;
49+
// };
50+
51+
52+
// // Immediate flush - no buffering, so nothing to do
53+
// void BinaryIOUnbuffered::_flush() {
54+
// // No internal buffer to flush
55+
// };
56+
57+
58+
// BinaryIOUnbuffered& BinaryIOUnbuffered::operator>>(float &value) {
59+
// remaining -= _io.readBytes((uint8_t*)&value, 4);
60+
// return *this;
61+
// };
62+
63+
64+
// BinaryIOUnbuffered& BinaryIOUnbuffered::operator>>(uint32_t &value) {
65+
// remaining -= _io.readBytes((uint8_t*)&value, 4);
66+
// return *this;
67+
// };
68+
69+
70+
// BinaryIOUnbuffered& BinaryIOUnbuffered::operator>>(uint8_t &value) {
71+
// value = (uint8_t)_io.read();
72+
// remaining--;
73+
// return *this;
74+
// };
75+
76+
77+
// PacketIO& BinaryIOUnbuffered::operator>>(Packet& value) {
78+
// while (!in_sync && _io.available() > 0) {
79+
// if (_io.peek() == MARKER_BYTE)
80+
// in_sync = true;
81+
// else
82+
// _io.read();
83+
// }
84+
// if (_io.peek() == MARKER_BYTE) {
85+
// _io.read(); // discard the marker
86+
// }
87+
// if (!in_sync || _io.available() < 3) { // size, frame type, payload = 3 bytes minimum frame size
88+
// value.type = 0x00;
89+
// value.payload_size = 0;
90+
// return *this;
91+
// }
92+
// value.payload_size = (uint8_t)_io.read() - 1;
93+
// value.type = (uint8_t)_io.read();
94+
// remaining = value.payload_size;
95+
// return *this;
96+
// };
97+
98+
// bool BinaryIOUnbuffered::is_complete() {
99+
// return (remaining <= 0);
100+
// };

‎src/comms/streams/BinaryIOUnbuffered.h‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
// #pragma once
2+
3+
// #include "../RegisterIO.h"
4+
// #include <Stream.h>
5+
// #include "Arduino.h"
6+
7+
8+
// #define MARKER_BYTE 0xA5
9+
10+
// // Unbuffered Binary IO - writes data immediately without buffering
11+
// // Useful for real-time applications where latency is critical
12+
// // Trade-off: More serial writes (potentially slower overall) but lower latency
13+
14+
// class BinaryIOUnbuffered : public PacketIO {
15+
// public:
16+
// BinaryIOUnbuffered(Stream& io);
17+
// virtual ~BinaryIOUnbuffered();
18+
// BinaryIOUnbuffered& operator<<(float value) override;
19+
// BinaryIOUnbuffered& operator<<(uint32_t value) override;
20+
// BinaryIOUnbuffered& operator<<(uint8_t value) override;
21+
// BinaryIOUnbuffered& operator<<(char value) override;
22+
// BinaryIOUnbuffered& operator<<(Packet value) override;
23+
// BinaryIOUnbuffered& operator<<(Separator value) override;
24+
// BinaryIOUnbuffered& operator>>(float& value) override;
25+
// BinaryIOUnbuffered& operator>>(uint32_t& value) override;
26+
// BinaryIOUnbuffered& operator>>(uint8_t& value) override;
27+
// PacketIO& operator>>(Packet& value) override;
28+
// bool is_complete() override;
29+
// virtual void _flush() override;
30+
31+
// protected:
32+
// Stream& _io;
33+
// uint8_t remaining = 0;
34+
// };

‎src/encoders/calibrated/CalibratedSensor.cpp‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,8 @@
55
// n_lut - number of samples in the LUT
66
CalibratedSensor::CalibratedSensor(Sensor &wrapped, int n_lut, uint16_t *lut)
77
: _wrapped(wrapped), n_lut(n_lut), allocated(false), calibrationLut(lut) {
8-
lut_resolution = n_lut / _2PI;
9-
lut_resolution_inv = 1/ lut_resolution;
8+
lut_resolution = _2PI / n_lut;
9+
lut_resolution_inv = 1.0f / lut_resolution;
1010
};
1111

1212
CalibratedSensor::~CalibratedSensor() {
@@ -57,8 +57,10 @@ float CalibratedSensor::getSensorAngle()
5757
// Linearly interpolate between lower and higher LUT entries
5858
float correction_offset = (1.0f - distance_lower) * lut_entry_lower + distance_lower * lut_entry_higher;
5959

60-
// Calculate the calibrated angle
61-
return raw_angle - correction_offset;
60+
// Calculate and normalize the calibrated angle to keep it in [0, 2PI)
61+
float calibrated_angle = raw_angle - correction_offset;
62+
if (calibrated_angle < 0 || calibrated_angle >= _2PI) calibrated_angle = _normalizeAngle(calibrated_angle);
63+
return calibrated_angle;
6264
}
6365

6466
// Perform filtering to linearize position sensor eccentricity

0 commit comments

Comments
 (0)