Compare commits
No commits in common. "55a8efa57913b21389e873a6949cc723cb74beeb" and "ce4f8eb8f5c02e2832ad3f396a0d84822e2b04ad" have entirely different histories.
55a8efa579
...
ce4f8eb8f5
@ -1,67 +0,0 @@
|
||||
# Generated from CLion C/C++ Code Style settings
|
||||
BasedOnStyle: LLVM
|
||||
AccessModifierOffset: -2
|
||||
AlignAfterOpenBracket: Align
|
||||
AlignConsecutiveAssignments: false
|
||||
AlignOperands: true
|
||||
AllowAllArgumentsOnNextLine: false
|
||||
AllowAllConstructorInitializersOnNextLine: false
|
||||
AllowAllParametersOfDeclarationOnNextLine: false
|
||||
AllowShortBlocksOnASingleLine: Always
|
||||
AllowShortCaseLabelsOnASingleLine: false
|
||||
AllowShortFunctionsOnASingleLine: All
|
||||
AllowShortIfStatementsOnASingleLine: Always
|
||||
AllowShortLambdasOnASingleLine: All
|
||||
AllowShortLoopsOnASingleLine: true
|
||||
AlwaysBreakAfterReturnType: None
|
||||
AlwaysBreakTemplateDeclarations: Yes
|
||||
BreakBeforeBraces: Custom
|
||||
BraceWrapping:
|
||||
AfterCaseLabel: false
|
||||
AfterClass: true
|
||||
AfterControlStatement: Always
|
||||
AfterEnum: true
|
||||
AfterFunction: true
|
||||
AfterNamespace: true
|
||||
AfterUnion: true
|
||||
AfterStruct: true
|
||||
BeforeCatch: false
|
||||
BeforeElse: false
|
||||
IndentBraces: false
|
||||
SplitEmptyFunction: false
|
||||
SplitEmptyRecord: true
|
||||
BreakBeforeBinaryOperators: None
|
||||
BreakBeforeTernaryOperators: true
|
||||
BreakConstructorInitializers: BeforeColon
|
||||
BreakInheritanceList: BeforeComma
|
||||
ColumnLimit: 0
|
||||
CompactNamespaces: false
|
||||
ContinuationIndentWidth: 8
|
||||
IndentCaseLabels: true
|
||||
IndentPPDirectives: None
|
||||
IndentWidth: 2
|
||||
KeepEmptyLinesAtTheStartOfBlocks: true
|
||||
MaxEmptyLinesToKeep: 2
|
||||
NamespaceIndentation: None
|
||||
ObjCSpaceAfterProperty: false
|
||||
ObjCSpaceBeforeProtocolList: true
|
||||
PointerAlignment: Left
|
||||
ReflowComments: false
|
||||
SpaceAfterCStyleCast: true
|
||||
SpaceAfterLogicalNot: false
|
||||
SpaceAfterTemplateKeyword: false
|
||||
SpaceBeforeAssignmentOperators: true
|
||||
SpaceBeforeCpp11BracedList: false
|
||||
SpaceBeforeCtorInitializerColon: true
|
||||
SpaceBeforeInheritanceColon: true
|
||||
SpaceBeforeParens: ControlStatements
|
||||
SpaceBeforeRangeBasedForLoopColon: true
|
||||
SpaceInEmptyParentheses: false
|
||||
SpacesBeforeTrailingComments: 0
|
||||
SpacesInAngles: false
|
||||
SpacesInCStyleCastParentheses: false
|
||||
SpacesInContainerLiterals: false
|
||||
SpacesInParentheses: false
|
||||
SpacesInSquareBrackets: false
|
||||
TabWidth: 2
|
||||
UseTab: Never
|
||||
@ -5,9 +5,9 @@ add_library(messaging INTERFACE)
|
||||
add_library(skullc::messaging ALIAS messaging)
|
||||
|
||||
target_include_directories(messaging
|
||||
INTERFACE
|
||||
INTERFACE
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/Inc>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
)
|
||||
|
||||
skullc_install_packages(skullc messaging ${version})
|
||||
|
||||
@ -8,9 +8,9 @@
|
||||
#ifndef MESSAGING_INC_MESSAGING_PACKET_HPP_
|
||||
#define MESSAGING_INC_MESSAGING_PACKET_HPP_
|
||||
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <cstdint>
|
||||
#include <array>
|
||||
|
||||
namespace Messaging
|
||||
{
|
||||
@ -20,8 +20,8 @@ struct Packet
|
||||
{
|
||||
using length_type = std::uint32_t;
|
||||
|
||||
static constexpr std::uint8_t preamble[] = {'A', 'A'};
|
||||
std::array<std::uint8_t, N> data = {0};
|
||||
static constexpr std::uint8_t preamble[] = { 'A', 'A' };
|
||||
std::array<std::uint8_t, N> data = { 0 };
|
||||
std::uint32_t data_length = 0;
|
||||
|
||||
const std::uint32_t max_data_length = N;
|
||||
@ -34,16 +34,13 @@ struct Packet
|
||||
}
|
||||
|
||||
Packet() = default;
|
||||
|
||||
Packet(const Packet&) = default;
|
||||
|
||||
Packet(Packet&&) noexcept = default;
|
||||
|
||||
template<std::size_t data_in_length>
|
||||
void copy_data_in(const std::uint8_t (&data_in)[data_in_length])
|
||||
{
|
||||
const std::uint32_t to_copy_length =
|
||||
std::min(std::uint32_t(data_in_length), max_data_length);
|
||||
const std::uint32_t to_copy_length = std::min(std::uint32_t(data_in_length), max_data_length);
|
||||
|
||||
std::memcpy(data.data(), data_in, to_copy_length);
|
||||
data_length = to_copy_length;
|
||||
@ -52,8 +49,7 @@ struct Packet
|
||||
template<std::size_t data_in_length>
|
||||
void copy_data_in(const std::array<std::uint8_t, data_in_length>& data_in)
|
||||
{
|
||||
const std::uint32_t to_copy_length =
|
||||
std::min(std::uint32_t(data_in_length), max_data_length);
|
||||
const std::uint32_t to_copy_length = std::min(std::uint32_t(data_in_length), max_data_length);
|
||||
|
||||
std::memcpy(data.data(), data_in.data(), to_copy_length);
|
||||
data_length = to_copy_length;
|
||||
@ -61,8 +57,7 @@ struct Packet
|
||||
|
||||
bool serialize(std::uint8_t* buffer, const std::uint32_t max_length)
|
||||
{
|
||||
const std::uint32_t required_size =
|
||||
preamble_length + data_length_length + data_length;
|
||||
const std::uint32_t required_size = preamble_length + data_length_length + data_length;
|
||||
|
||||
if (max_length < required_size)
|
||||
return false;
|
||||
@ -101,9 +96,10 @@ struct Packet
|
||||
}
|
||||
};
|
||||
|
||||
template<std::size_t N>
|
||||
template <std::size_t N>
|
||||
constexpr std::uint8_t Packet<N>::preamble[2];
|
||||
|
||||
}// namespace Messaging
|
||||
}
|
||||
|
||||
|
||||
#endif /* MESSAGING_INC_MESSAGING_PACKET_HPP_ */
|
||||
|
||||
@ -5,9 +5,9 @@
|
||||
#ifndef SKULLC_MESSAGING_PARSER_HPP
|
||||
#define SKULLC_MESSAGING_PARSER_HPP
|
||||
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <array>
|
||||
|
||||
namespace Messaging
|
||||
{
|
||||
@ -19,10 +19,11 @@ public:
|
||||
using Packet = P;
|
||||
const std::size_t buffer_length = N;
|
||||
|
||||
Parser() { reset(); }
|
||||
|
||||
Parser()
|
||||
{
|
||||
reset();
|
||||
}
|
||||
Parser(const Parser&) = delete;
|
||||
|
||||
Parser(Parser&&) = delete;
|
||||
|
||||
void reset()
|
||||
@ -42,20 +43,20 @@ public:
|
||||
|
||||
switch (_state)
|
||||
{
|
||||
case _State::Preamble:
|
||||
if (byte != P::preamble[_current_pos])
|
||||
{
|
||||
reset();
|
||||
return;
|
||||
}
|
||||
[[fallthrough]];
|
||||
case _State::Length:
|
||||
case _State::Body:
|
||||
_buffer[buffer_loc] = byte;
|
||||
_current_pos++;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
case _State::Preamble:
|
||||
if (byte != P::preamble[_current_pos])
|
||||
{
|
||||
reset();
|
||||
return;
|
||||
}
|
||||
[[fallthrough]];
|
||||
case _State::Length:
|
||||
case _State::Body:
|
||||
_buffer[buffer_loc] = byte;
|
||||
_current_pos++;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (_current_pos == _expected)
|
||||
@ -64,7 +65,10 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
bool packetReady() const { return _state == _State::Done; }
|
||||
bool packetReady() const
|
||||
{
|
||||
return _state == _State::Done;
|
||||
}
|
||||
|
||||
bool getPacket(Packet& packet) const
|
||||
{
|
||||
@ -102,19 +106,19 @@ private:
|
||||
{
|
||||
switch (_state)
|
||||
{
|
||||
case _State::Preamble:
|
||||
_state = _State::Length;
|
||||
_expected = sizeof(typename P::length_type);
|
||||
break;
|
||||
case _State::Length:
|
||||
_state = _State::Body;
|
||||
_expected = _deserializeLength<typename P::length_type>(_current_offset);
|
||||
break;
|
||||
case _State::Body:
|
||||
_state = _State::Done;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
case _State::Preamble:
|
||||
_state = _State::Length;
|
||||
_expected = sizeof(typename P::length_type);
|
||||
break;
|
||||
case _State::Length:
|
||||
_state = _State::Body;
|
||||
_expected = _deserializeLength<typename P::length_type>(_current_offset);
|
||||
break;
|
||||
case _State::Body:
|
||||
_state = _State::Done;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
_current_offset += _current_pos;
|
||||
@ -122,6 +126,6 @@ private:
|
||||
}
|
||||
};
|
||||
|
||||
}// namespace Messaging
|
||||
}
|
||||
|
||||
#endif// SKULLC_MESSAGING_PARSER_HPP
|
||||
#endif //SKULLC_MESSAGING_PARSER_HPP
|
||||
|
||||
@ -8,8 +8,8 @@
|
||||
#ifndef PERIPHERALS_INC_PERIPHERALS_ADC_HPP_
|
||||
#define PERIPHERALS_INC_PERIPHERALS_ADC_HPP_
|
||||
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <array>
|
||||
|
||||
#include <adc.h>
|
||||
|
||||
@ -19,14 +19,19 @@ namespace Peripherals
|
||||
template<std::size_t N>
|
||||
struct Adc
|
||||
{
|
||||
std::array<std::uint16_t, N> readings = {0};
|
||||
std::array<std::uint16_t, N> readings = { 0 };
|
||||
ADC_HandleTypeDef* hadc = nullptr;
|
||||
|
||||
static constexpr std::uint32_t readingsCount() { return N; }
|
||||
static constexpr std::uint32_t readingsCount()
|
||||
{
|
||||
return N;
|
||||
}
|
||||
|
||||
Adc() = delete;
|
||||
|
||||
explicit Adc(ADC_HandleTypeDef* hadc) : hadc(hadc) {}
|
||||
explicit Adc(ADC_HandleTypeDef* hadc)
|
||||
: hadc(hadc)
|
||||
{ }
|
||||
|
||||
Adc(const Adc&) = delete;
|
||||
Adc(Adc&&) = delete;
|
||||
@ -51,13 +56,16 @@ struct Adc
|
||||
|
||||
void startDma()
|
||||
{
|
||||
HAL_ADC_Start_DMA(hadc, reinterpret_cast<std::uint32_t*>(readings.data()),
|
||||
readingsCount());
|
||||
HAL_ADC_Start_DMA(hadc, reinterpret_cast<std::uint32_t*>(readings.data()), readingsCount());
|
||||
}
|
||||
|
||||
void stopDma() { HAL_ADC_Stop_DMA(hadc); }
|
||||
void stopDma()
|
||||
{
|
||||
HAL_ADC_Stop_DMA(hadc);
|
||||
}
|
||||
};
|
||||
|
||||
}// namespace Peripherals
|
||||
}
|
||||
|
||||
|
||||
#endif /* PERIPHERALS_INC_PERIPHERALS_ADC_HPP_ */
|
||||
|
||||
@ -22,14 +22,19 @@ namespace St
|
||||
{
|
||||
|
||||
template<typename Origin>
|
||||
using IsrCallbackFn = void (*)(Origin*);
|
||||
using IsrCallbackFn = void(*)(Origin*);
|
||||
|
||||
template<typename Origin, typename Handler, void (Handler::*func)(),
|
||||
typename Tag>
|
||||
template<typename Origin, typename Handler, void (Handler::*func)(), typename Tag>
|
||||
IsrCallbackFn<Origin> createCallback(Handler& h_in)
|
||||
{
|
||||
static Handler* h = &h_in;
|
||||
return +[](Origin*) { (h->*func)(); };
|
||||
printf("Set handler: %p\n", h);
|
||||
return +[](Origin*)
|
||||
{
|
||||
printf("Into callback we go.\n");
|
||||
(h->*func)();
|
||||
printf("And we're out.\n");
|
||||
};
|
||||
}
|
||||
|
||||
struct StaticHal
|
||||
@ -51,19 +56,24 @@ struct StaticHal
|
||||
static void DelayUs(const std::uint32_t micros)
|
||||
{
|
||||
#ifdef USE_DELAY_US
|
||||
const std::uint32_t tick_start = DWT->CYCCNT;
|
||||
const std::uint32_t ticks_delay = micros * (SystemCoreClock / 1'000'000);
|
||||
const std::uint32_t tick_start = DWT->CYCCNT;
|
||||
const std::uint32_t ticks_delay = micros * (SystemCoreClock / 1'000'000);
|
||||
|
||||
while (DWT->CYCCNT - tick_start < ticks_delay)
|
||||
;
|
||||
while (DWT->CYCCNT - tick_start < ticks_delay);
|
||||
#else
|
||||
(void) micros;
|
||||
(void)micros;
|
||||
#endif
|
||||
}
|
||||
|
||||
static void enableInterrupts() { __enable_irq(); }
|
||||
static void enableInterrupts()
|
||||
{
|
||||
__enable_irq();
|
||||
}
|
||||
|
||||
static void disableInterrupts() { __disable_irq(); }
|
||||
static void disableInterrupts()
|
||||
{
|
||||
__disable_irq();
|
||||
}
|
||||
};
|
||||
|
||||
#ifdef HAL_GPIO_MODULE_ENABLED
|
||||
@ -75,33 +85,40 @@ struct Gpio
|
||||
|
||||
Gpio() = delete;
|
||||
explicit Gpio(GPIO_TypeDef* port, const std::uint16_t pin)
|
||||
: port(port), pin(pin) {}
|
||||
: port(port)
|
||||
, pin(pin)
|
||||
{ }
|
||||
|
||||
void Set(const bool& state)
|
||||
{
|
||||
HAL_GPIO_WritePin(port, pin, GPIO_PinState(state));
|
||||
}
|
||||
|
||||
void Toggle() { HAL_GPIO_TogglePin(port, pin); }
|
||||
void Toggle()
|
||||
{
|
||||
HAL_GPIO_TogglePin(port, pin);
|
||||
}
|
||||
|
||||
bool Read() const { return HAL_GPIO_ReadPin(port, pin); }
|
||||
bool Read() const
|
||||
{
|
||||
return HAL_GPIO_ReadPin(port, pin);
|
||||
}
|
||||
};
|
||||
|
||||
#endif// HAL_GPIO_MODULE_ENABLED
|
||||
#endif // HAL_GPIO_MODULE_ENABLED
|
||||
|
||||
template<
|
||||
typename T,
|
||||
HAL_StatusTypeDef (*transmit)(
|
||||
T*, std::uint8_t* data, std::uint16_t data_len, std::uint32_t timeout),
|
||||
HAL_StatusTypeDef (*receive)(T*, std::uint8_t* data,
|
||||
std::uint16_t data_len, std::uint32_t timeout)>
|
||||
template<typename T,
|
||||
HAL_StatusTypeDef (*transmit)(T*, std::uint8_t* data, std::uint16_t data_len, std::uint32_t timeout),
|
||||
HAL_StatusTypeDef (*receive)(T*, std::uint8_t* data, std::uint16_t data_len, std::uint32_t timeout)>
|
||||
struct SerialInterface
|
||||
{
|
||||
using underlying_handle_type = T;
|
||||
underlying_handle_type* handle;
|
||||
|
||||
SerialInterface() = delete;
|
||||
explicit SerialInterface(underlying_handle_type* handle) : handle(handle) {}
|
||||
explicit SerialInterface(underlying_handle_type* handle)
|
||||
: handle(handle)
|
||||
{ }
|
||||
|
||||
bool Transmit(std::uint8_t* data, const std::uint32_t data_len)
|
||||
{
|
||||
@ -115,10 +132,8 @@ struct SerialInterface
|
||||
};
|
||||
|
||||
template<typename T,
|
||||
HAL_StatusTypeDef (*transmit)(T*, std::uint8_t* data,
|
||||
std::uint16_t data_len),
|
||||
HAL_StatusTypeDef (*receive)(T*, std::uint8_t* data,
|
||||
std::uint16_t data_len)>
|
||||
HAL_StatusTypeDef (*transmit)(T*, std::uint8_t* data, std::uint16_t data_len),
|
||||
HAL_StatusTypeDef (*receive)(T*, std::uint8_t* data, std::uint16_t data_len)>
|
||||
struct SerialInterfaceAsync
|
||||
{
|
||||
using underlying_handle_type = T;
|
||||
@ -126,7 +141,8 @@ struct SerialInterfaceAsync
|
||||
|
||||
SerialInterfaceAsync() = delete;
|
||||
explicit SerialInterfaceAsync(underlying_handle_type* handle)
|
||||
: handle(handle) {}
|
||||
: handle(handle)
|
||||
{ }
|
||||
|
||||
bool Transmit(std::uint8_t* data, const std::uint32_t data_len)
|
||||
{
|
||||
@ -141,8 +157,7 @@ struct SerialInterfaceAsync
|
||||
|
||||
#ifdef HAL_SPI_MODULE_ENABLED
|
||||
|
||||
using SpiInterface =
|
||||
SerialInterface<SPI_HandleTypeDef, HAL_SPI_Transmit, HAL_SPI_Receive>;
|
||||
using SpiInterface = SerialInterface<SPI_HandleTypeDef, HAL_SPI_Transmit, HAL_SPI_Receive>;
|
||||
|
||||
struct SpiRegisters
|
||||
{
|
||||
@ -151,7 +166,9 @@ struct SpiRegisters
|
||||
|
||||
SpiRegisters() = delete;
|
||||
explicit SpiRegisters(const SpiInterface& handle, const Gpio& chip_select)
|
||||
: handle(handle), chip_select(chip_select) {}
|
||||
: handle(handle)
|
||||
, chip_select(chip_select)
|
||||
{ }
|
||||
|
||||
void WriteRegister(std::uint8_t reg, uint8_t data)
|
||||
{
|
||||
@ -163,8 +180,7 @@ struct SpiRegisters
|
||||
chip_select.Set(true);
|
||||
}
|
||||
|
||||
void WriteRegisterMultibyte(std::uint8_t reg, std::uint8_t* data,
|
||||
const std::uint32_t len)
|
||||
void WriteRegisterMultibyte(std::uint8_t reg, std::uint8_t* data, const std::uint32_t len)
|
||||
{
|
||||
chip_select.Set(false);
|
||||
|
||||
@ -174,8 +190,7 @@ struct SpiRegisters
|
||||
chip_select.Set(true);
|
||||
}
|
||||
|
||||
std::uint8_t ReadRegister(std::uint8_t reg,
|
||||
const std::uint32_t read_delay = 0)
|
||||
std::uint8_t ReadRegister(std::uint8_t reg, const std::uint32_t read_delay = 0)
|
||||
{
|
||||
chip_select.Set(false);
|
||||
|
||||
@ -193,9 +208,7 @@ struct SpiRegisters
|
||||
return output;
|
||||
}
|
||||
|
||||
void ReadRegisterMultibyte(std::uint8_t reg, std::uint8_t* data,
|
||||
const std::uint32_t len,
|
||||
const std::uint32_t read_delay = 0)
|
||||
void ReadRegisterMultibyte(std::uint8_t reg, std::uint8_t* data, const std::uint32_t len, const std::uint32_t read_delay = 0)
|
||||
{
|
||||
chip_select.Set(false);
|
||||
|
||||
@ -210,17 +223,14 @@ struct SpiRegisters
|
||||
}
|
||||
};
|
||||
|
||||
#endif// HAL_SPI_MODULE_ENABLED
|
||||
#endif // HAL_SPI_MODULE_ENABLED
|
||||
|
||||
#ifdef HAL_UART_MODULE_ENABLED
|
||||
|
||||
using UartInterface =
|
||||
SerialInterface<UART_HandleTypeDef, HAL_UART_Transmit, HAL_UART_Receive>;
|
||||
using UartInterfaceDMA =
|
||||
SerialInterfaceAsync<UART_HandleTypeDef, HAL_UART_Transmit_DMA,
|
||||
HAL_UART_Receive_DMA>;
|
||||
using UartInterface = SerialInterface<UART_HandleTypeDef, HAL_UART_Transmit, HAL_UART_Receive>;
|
||||
using UartInterfaceDMA = SerialInterfaceAsync<UART_HandleTypeDef, HAL_UART_Transmit_DMA, HAL_UART_Receive_DMA>;
|
||||
|
||||
#endif// HAL_UART_MODULE_ENABLED
|
||||
#endif // HAL_UART_MODULE_ENABLED
|
||||
|
||||
#ifdef HAL_TIM_MODULE_ENABLED
|
||||
|
||||
@ -231,21 +241,32 @@ struct PwmChannel
|
||||
|
||||
PwmChannel() = delete;
|
||||
explicit PwmChannel(TIM_HandleTypeDef* handle, const std::uint32_t channel)
|
||||
: handle(handle), channel(channel) {}
|
||||
: handle(handle)
|
||||
, channel(channel)
|
||||
{ }
|
||||
|
||||
void Enable() { HAL_TIM_PWM_Start(handle, channel); }
|
||||
void Enable()
|
||||
{
|
||||
HAL_TIM_PWM_Start(handle, channel);
|
||||
}
|
||||
|
||||
void Disable() { HAL_TIM_PWM_Stop(handle, channel); }
|
||||
void Disable()
|
||||
{
|
||||
HAL_TIM_PWM_Stop(handle, channel);
|
||||
}
|
||||
|
||||
void SetCompare(const std::uint32_t compare)
|
||||
{
|
||||
__HAL_TIM_SET_COMPARE(handle, channel, compare);
|
||||
}
|
||||
|
||||
std::uint32_t MaxValue() { return handle->Init.Period; }
|
||||
std::uint32_t MaxValue()
|
||||
{
|
||||
return handle->Init.Period;
|
||||
}
|
||||
};
|
||||
|
||||
#endif// HAL_TIM_MODULE_ENABLED
|
||||
#endif // HAL_TIM_MODULE_ENABLED
|
||||
|
||||
struct ItmSerialInterface
|
||||
{
|
||||
@ -259,8 +280,8 @@ struct ItmSerialInterface
|
||||
}
|
||||
};
|
||||
|
||||
}// namespace St
|
||||
}// namespace Hal
|
||||
}// namespace Peripherals
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* PERIPHERALS_INC_PERIPHERALS_HAL_ST_HPP_ */
|
||||
|
||||
@ -16,23 +16,24 @@ namespace Peripherals
|
||||
class IImu
|
||||
{
|
||||
public:
|
||||
enum class Axis : std::uint32_t
|
||||
{
|
||||
X = 0,
|
||||
Y,
|
||||
Z
|
||||
};
|
||||
enum class Axis : std::uint32_t
|
||||
{
|
||||
X = 0,
|
||||
Y,
|
||||
Z
|
||||
};
|
||||
|
||||
virtual void Setup() = 0;
|
||||
virtual void Calibrate(const std::uint32_t samples) = 0;
|
||||
virtual void Setup() = 0;
|
||||
virtual void Calibrate(const std::uint32_t samples) = 0;
|
||||
|
||||
virtual void ReadGyro(float* output) = 0;
|
||||
virtual void ReadGyroRaw(std::int16_t* output) = 0;
|
||||
virtual void ReadGyro(float* output) = 0;
|
||||
virtual void ReadGyroRaw(std::int16_t* output) = 0;
|
||||
|
||||
virtual void ReadAccelerometer(float* output) = 0;
|
||||
virtual void ReadAccelerometerRaw(std::int16_t* output) = 0;
|
||||
virtual void ReadAccelerometer(float* output) = 0;
|
||||
virtual void ReadAccelerometerRaw(std::int16_t* output) = 0;
|
||||
};
|
||||
|
||||
}// namespace Peripherals
|
||||
}
|
||||
|
||||
|
||||
#endif /* PERIPHERALS_IMU_HPP_ */
|
||||
|
||||
@ -24,269 +24,259 @@ public:
|
||||
using registers_handle = T;
|
||||
using hal = HAL;
|
||||
|
||||
enum class GyroScale : std::uint32_t
|
||||
{
|
||||
DPS_250 = 0,
|
||||
DPS_500,
|
||||
DPS_1000,
|
||||
DPS_2000
|
||||
};
|
||||
enum class GyroScale : std::uint32_t
|
||||
{
|
||||
DPS_250 = 0,
|
||||
DPS_500,
|
||||
DPS_1000,
|
||||
DPS_2000
|
||||
};
|
||||
|
||||
enum class AccelerometerScale : std::uint32_t
|
||||
{
|
||||
G2 = 0,
|
||||
G4,
|
||||
G8,
|
||||
G16
|
||||
};
|
||||
enum class AccelerometerScale : std::uint32_t
|
||||
{
|
||||
G2 = 0,
|
||||
G4,
|
||||
G8,
|
||||
G16
|
||||
};
|
||||
|
||||
registers_handle registers;
|
||||
registers_handle registers;
|
||||
|
||||
ImuIcm() = delete;
|
||||
ImuIcm(const registers_handle& registers) : registers(registers) {}
|
||||
ImuIcm() = delete;
|
||||
ImuIcm(const registers_handle& registers)
|
||||
: registers(registers)
|
||||
{ }
|
||||
|
||||
void Setup() override
|
||||
{
|
||||
registers.WriteRegister(_Registers::PWR_MGMT_1 & _Registers::WRITE_MASK,
|
||||
0b10000000);
|
||||
hal::Delay(10);
|
||||
void Setup() override
|
||||
{
|
||||
registers.WriteRegister(_Registers::PWR_MGMT_1 & _Registers::WRITE_MASK, 0b10000000);
|
||||
hal::Delay(10);
|
||||
|
||||
registers.WriteRegister(_Registers::USER_CTRL & _Registers::WRITE_MASK,
|
||||
0b00010000);
|
||||
hal::Delay(10);
|
||||
registers.WriteRegister(_Registers::USER_CTRL & _Registers::WRITE_MASK, 0b00010000);
|
||||
hal::Delay(10);
|
||||
|
||||
registers.WriteRegister(_Registers::PWR_MGMT_1 & _Registers::WRITE_MASK,
|
||||
0b00000000);
|
||||
hal::Delay(10);
|
||||
registers.WriteRegister(_Registers::PWR_MGMT_1 & _Registers::WRITE_MASK, 0b00000000);
|
||||
hal::Delay(10);
|
||||
|
||||
registers.WriteRegister(_Registers::PWR_MGMT_1 & _Registers::WRITE_MASK,
|
||||
0b00000001);
|
||||
hal::Delay(10);
|
||||
registers.WriteRegister(_Registers::PWR_MGMT_1 & _Registers::WRITE_MASK, 0b00000001);
|
||||
hal::Delay(10);
|
||||
|
||||
registers.WriteRegister(_Registers::CONFIG & _Registers::WRITE_MASK,
|
||||
0x03);// DLPF_CFG = 3, gyro filter = 41/59.0, gyro
|
||||
// rate = 1KHz, temp filter = 42
|
||||
hal::Delay(10);
|
||||
registers.WriteRegister(_Registers::CONFIG & _Registers::WRITE_MASK, 0x03); // DLPF_CFG = 3, gyro filter = 41/59.0, gyro rate = 1KHz, temp filter = 42
|
||||
hal::Delay(10);
|
||||
|
||||
SetGyroscopeScale(_scale_gyro);
|
||||
SetGyroscopeScale(_scale_gyro);
|
||||
|
||||
SetAccelerometerScale(_scale_accel);
|
||||
SetAccelerometerScale(_scale_accel);
|
||||
|
||||
// ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz
|
||||
registers.WriteRegister(_Registers::ACCEL_CONFIG2 & _Registers::WRITE_MASK,
|
||||
0x03);
|
||||
hal::Delay(10);
|
||||
// ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz
|
||||
registers.WriteRegister(_Registers::ACCEL_CONFIG2 & _Registers::WRITE_MASK, 0x03);
|
||||
hal::Delay(10);
|
||||
|
||||
// SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV) Where
|
||||
// INTERNAL_SAMPLE_RATE = 1kHz
|
||||
registers.WriteRegister(_Registers::SMPLRT_DIV & _Registers::WRITE_MASK, 0);
|
||||
hal::Delay(10);
|
||||
// SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV) Where INTERNAL_SAMPLE_RATE = 1kHz
|
||||
registers.WriteRegister(_Registers::SMPLRT_DIV & _Registers::WRITE_MASK, 0);
|
||||
hal::Delay(10);
|
||||
|
||||
// Enable interrupt
|
||||
// Enable interrupt
|
||||
|
||||
// The logic level for INT/DRDY pin is active high.
|
||||
// INT/DRDY pin is configured as push-pull.
|
||||
// INT/DRDY pin indicates interrupt pulse's width is 50us.
|
||||
// Interrupt status is cleared only by reading INT_STATUS register
|
||||
registers.WriteRegister(_Registers::INT_PIN_CFG & _Registers::WRITE_MASK,
|
||||
0);
|
||||
hal::Delay(10);
|
||||
// The logic level for INT/DRDY pin is active high.
|
||||
// INT/DRDY pin is configured as push-pull.
|
||||
// INT/DRDY pin indicates interrupt pulse's width is 50us.
|
||||
// Interrupt status is cleared only by reading INT_STATUS register
|
||||
registers.WriteRegister(_Registers::INT_PIN_CFG & _Registers::WRITE_MASK, 0);
|
||||
hal::Delay(10);
|
||||
|
||||
registers.WriteRegister(_Registers::INT_ENABLE & _Registers::WRITE_MASK, 1);
|
||||
hal::Delay(10);
|
||||
}
|
||||
registers.WriteRegister(_Registers::INT_ENABLE & _Registers::WRITE_MASK, 1);
|
||||
hal::Delay(10);
|
||||
}
|
||||
|
||||
void Calibrate(const std::uint32_t samples) override
|
||||
{
|
||||
std::array<std::int32_t, 3> avg_gyro;
|
||||
std::array<std::int32_t, 3> avg_accel;
|
||||
void Calibrate(const std::uint32_t samples) override
|
||||
{
|
||||
std::array<std::int32_t, 3> avg_gyro;
|
||||
std::array<std::int32_t, 3> avg_accel;
|
||||
|
||||
for (std::uint32_t i = 0; i < samples; i++)
|
||||
{
|
||||
std::array<std::int16_t, 3> raw;
|
||||
auto add_to_avg = [&raw](std::array<std::int32_t, 3>& out) {
|
||||
for (std::uint32_t j = 0; j < 3; j++)
|
||||
out[j] += raw[j];
|
||||
};
|
||||
for (std::uint32_t i = 0; i < samples; i++)
|
||||
{
|
||||
std::array<std::int16_t, 3> raw;
|
||||
auto add_to_avg = [&raw](std::array<std::int32_t, 3>& out)
|
||||
{
|
||||
for (std::uint32_t j = 0; j < 3; j++)
|
||||
out[j] += raw[j];
|
||||
};
|
||||
|
||||
ReadGyroRaw(raw.data());
|
||||
add_to_avg(avg_gyro);
|
||||
ReadGyroRaw(raw.data());
|
||||
add_to_avg(avg_gyro);
|
||||
|
||||
ReadAccelerometerRaw(raw.data());
|
||||
add_to_avg(avg_accel);
|
||||
}
|
||||
ReadAccelerometerRaw(raw.data());
|
||||
add_to_avg(avg_accel);
|
||||
}
|
||||
|
||||
for (std::uint32_t i = 0; i < 3; i++)
|
||||
{
|
||||
const std::int32_t max = std::numeric_limits<std::int16_t>::max();
|
||||
const std::int32_t min = std::numeric_limits<std::int16_t>::min();
|
||||
for (std::uint32_t i = 0; i < 3; i++)
|
||||
{
|
||||
const std::int32_t max = std::numeric_limits<std::int16_t>::max();
|
||||
const std::int32_t min = std::numeric_limits<std::int16_t>::min();
|
||||
|
||||
_bias_gyro[i] = Clamp(avg_gyro[i], max, min);
|
||||
_bias_accel[i] = Clamp(avg_accel[i], max, min);
|
||||
}
|
||||
_bias_gyro[i] = Clamp(avg_gyro[i], max, min);
|
||||
_bias_accel[i] = Clamp(avg_accel[i], max, min);
|
||||
}
|
||||
|
||||
_bias_accel[2] -= AccelerometerReadingToRaw(1);
|
||||
}
|
||||
_bias_accel[2] -= AccelerometerReadingToRaw(1);
|
||||
}
|
||||
|
||||
void SetGyroscopeScale(const GyroScale scale)
|
||||
{
|
||||
const std::uint8_t current_config =
|
||||
registers.ReadRegister(_Registers::GYRO_CONFIG | _Registers::READ_MASK);
|
||||
const std::uint8_t new_config =
|
||||
(current_config & 0xE7) | (std::uint8_t(scale) << 3);
|
||||
registers.WriteRegister(_Registers::GYRO_CONFIG & _Registers::WRITE_MASK,
|
||||
new_config);
|
||||
void SetGyroscopeScale(const GyroScale scale)
|
||||
{
|
||||
const std::uint8_t current_config = registers.ReadRegister(_Registers::GYRO_CONFIG | _Registers::READ_MASK);
|
||||
const std::uint8_t new_config = (current_config & 0xE7) | (std::uint8_t(scale) << 3);
|
||||
registers.WriteRegister(_Registers::GYRO_CONFIG & _Registers::WRITE_MASK, new_config);
|
||||
|
||||
_scale_gyro = scale;
|
||||
}
|
||||
_scale_gyro = scale;
|
||||
}
|
||||
|
||||
void SetAccelerometerScale(const AccelerometerScale scale)
|
||||
{
|
||||
const std::uint8_t current_config = registers.ReadRegister(
|
||||
_Registers::ACCEL_CONFIG | _Registers::READ_MASK);
|
||||
const std::uint8_t new_config =
|
||||
(current_config & 0xE7) | (std::uint8_t(scale) << 3);
|
||||
registers.WriteRegister(_Registers::ACCEL_CONFIG & _Registers::WRITE_MASK,
|
||||
new_config);
|
||||
void SetAccelerometerScale(const AccelerometerScale scale)
|
||||
{
|
||||
const std::uint8_t current_config = registers.ReadRegister(_Registers::ACCEL_CONFIG | _Registers::READ_MASK);
|
||||
const std::uint8_t new_config = (current_config & 0xE7) | (std::uint8_t(scale) << 3);
|
||||
registers.WriteRegister(_Registers::ACCEL_CONFIG & _Registers::WRITE_MASK, new_config);
|
||||
|
||||
_scale_accel = scale;
|
||||
}
|
||||
_scale_accel = scale;
|
||||
}
|
||||
|
||||
void ReadGyro(float* output) override
|
||||
{
|
||||
uint8_t data[6] = {0};
|
||||
registers.ReadRegisterMultibyte(
|
||||
_Registers::GYRO_XOUT_H | _Registers::READ_MASK, data, 6);
|
||||
void ReadGyro(float* output) override
|
||||
{
|
||||
uint8_t data[6] = { 0 };
|
||||
registers.ReadRegisterMultibyte(_Registers::GYRO_XOUT_H | _Registers::READ_MASK, data, 6);
|
||||
|
||||
for (std::uint32_t i = 0; i < 3; i++)
|
||||
{
|
||||
const std::int16_t bit = ByteToTypeBE<std::int16_t, 2>(&data[i * 2]);
|
||||
output[i] = GyroRawToReading(bit);
|
||||
}
|
||||
}
|
||||
for (std::uint32_t i = 0; i < 3; i++)
|
||||
{
|
||||
const std::int16_t bit = ByteToTypeBE<std::int16_t, 2>(&data[i * 2]);
|
||||
output[i] = GyroRawToReading(bit);
|
||||
}
|
||||
}
|
||||
|
||||
void ReadGyroRaw(std::int16_t* output) override
|
||||
{
|
||||
uint8_t data[6] = {0};
|
||||
registers.ReadRegisterMultibyte(
|
||||
_Registers::GYRO_XOUT_H | _Registers::READ_MASK, data, 6);
|
||||
void ReadGyroRaw(std::int16_t* output) override
|
||||
{
|
||||
uint8_t data[6] = { 0 };
|
||||
registers.ReadRegisterMultibyte(_Registers::GYRO_XOUT_H | _Registers::READ_MASK, data, 6);
|
||||
|
||||
for (std::uint32_t i = 0; i < 3; i++)
|
||||
{
|
||||
output[i] = ByteToTypeBE<std::int16_t, 2>(&data[i * 2]);
|
||||
}
|
||||
}
|
||||
for (std::uint32_t i = 0; i < 3; i++)
|
||||
{
|
||||
output[i] = ByteToTypeBE<std::int16_t, 2>(&data[i * 2]);
|
||||
}
|
||||
}
|
||||
|
||||
void ReadAccelerometer(float* output) override
|
||||
{
|
||||
uint8_t data[6] = {0};
|
||||
registers.ReadRegisterMultibyte(
|
||||
_Registers::ACCEL_XOUT_H | _Registers::READ_MASK, data, 6);
|
||||
void ReadAccelerometer(float* output) override
|
||||
{
|
||||
uint8_t data[6] = { 0 };
|
||||
registers.ReadRegisterMultibyte(_Registers::ACCEL_XOUT_H | _Registers::READ_MASK, data, 6);
|
||||
|
||||
for (std::uint32_t i = 0; i < 3; i++)
|
||||
{
|
||||
const std::int16_t bit = ByteToTypeBE<std::int16_t, 2>(&data[i * 2]);
|
||||
output[i] = AccelerometerRawToReading(bit);
|
||||
}
|
||||
}
|
||||
for (std::uint32_t i = 0; i < 3; i++)
|
||||
{
|
||||
const std::int16_t bit = ByteToTypeBE<std::int16_t, 2>(&data[i * 2]);
|
||||
output[i] = AccelerometerRawToReading(bit);
|
||||
}
|
||||
}
|
||||
|
||||
void ReadAccelerometerRaw(std::int16_t* output) override
|
||||
{
|
||||
uint8_t data[6] = {0};
|
||||
registers.ReadRegisterMultibyte(
|
||||
_Registers::ACCEL_XOUT_H | _Registers::READ_MASK, data, 6);
|
||||
void ReadAccelerometerRaw(std::int16_t* output) override
|
||||
{
|
||||
uint8_t data[6] = { 0 };
|
||||
registers.ReadRegisterMultibyte(_Registers::ACCEL_XOUT_H | _Registers::READ_MASK, data, 6);
|
||||
|
||||
for (std::uint32_t i = 0; i < 3; i++)
|
||||
{
|
||||
output[i] = ByteToTypeBE<std::int16_t, 2>(&data[i * 2]);
|
||||
}
|
||||
}
|
||||
for (std::uint32_t i = 0; i < 3; i++)
|
||||
{
|
||||
output[i] = ByteToTypeBE<std::int16_t, 2>(&data[i * 2]);
|
||||
}
|
||||
}
|
||||
|
||||
std::int16_t AccelerometerReadingToRaw(const float& fs) const
|
||||
{
|
||||
return fs / _accel_fs_to_bit_constants[std::uint32_t(_scale_accel)];
|
||||
}
|
||||
std::int16_t AccelerometerReadingToRaw(const float& fs) const
|
||||
{
|
||||
return fs / _accel_fs_to_bit_constants[std::uint32_t(_scale_accel)];
|
||||
}
|
||||
|
||||
std::int16_t GyroReadingToRaw(const float& fs) const
|
||||
{
|
||||
return fs / _gyro_fs_to_bit_constants[std::uint32_t(_scale_gyro)];
|
||||
}
|
||||
std::int16_t GyroReadingToRaw(const float& fs) const
|
||||
{
|
||||
return fs / _gyro_fs_to_bit_constants[std::uint32_t(_scale_gyro)];
|
||||
}
|
||||
|
||||
float AccelerometerRawToReading(const std::int16_t bit) const
|
||||
{
|
||||
return float(bit) * _accel_fs_to_bit_constants[std::uint32_t(_scale_accel)];
|
||||
}
|
||||
float AccelerometerRawToReading(const std::int16_t bit) const
|
||||
{
|
||||
return float(bit) * _accel_fs_to_bit_constants[std::uint32_t(_scale_accel)];
|
||||
}
|
||||
|
||||
float GyroRawToReading(const std::int16_t bit) const
|
||||
{
|
||||
return float(bit) * _gyro_fs_to_bit_constants[std::uint32_t(_scale_gyro)];
|
||||
}
|
||||
float GyroRawToReading(const std::int16_t bit) const
|
||||
{
|
||||
return float(bit) * _gyro_fs_to_bit_constants[std::uint32_t(_scale_gyro)];
|
||||
}
|
||||
|
||||
private:
|
||||
GyroScale _scale_gyro = GyroScale::DPS_2000;
|
||||
AccelerometerScale _scale_accel = AccelerometerScale::G16;
|
||||
GyroScale _scale_gyro = GyroScale::DPS_2000;
|
||||
AccelerometerScale _scale_accel = AccelerometerScale::G16;
|
||||
|
||||
std::array<std::int16_t, 3> _bias_gyro;
|
||||
std::array<std::int16_t, 3> _bias_accel;
|
||||
std::array<std::int16_t, 3> _bias_gyro;
|
||||
std::array<std::int16_t, 3> _bias_accel;
|
||||
|
||||
static constexpr float _accel_fs_to_bit_constants[4] = {
|
||||
(2.0f / 32768.0f), (4.0f / 32768.0f), (8.0f / 32768.0f),
|
||||
(16.0f / 32768.0f)};
|
||||
static constexpr float _accel_fs_to_bit_constants[4] = {
|
||||
(2.0f / 32768.0f),
|
||||
(4.0f / 32768.0f),
|
||||
(8.0f / 32768.0f),
|
||||
(16.0f / 32768.0f)
|
||||
};
|
||||
|
||||
static constexpr float _gyro_fs_to_bit_constants[4] = {
|
||||
(250.0f / 32768.0f), (500.0f / 32768.0f), (1000.0f / 32768.0f),
|
||||
(2000.0f / 32768.0f)};
|
||||
static constexpr float _gyro_fs_to_bit_constants[4] = {
|
||||
(250.0f / 32768.0f),
|
||||
(500.0f / 32768.0f),
|
||||
(1000.0f / 32768.0f),
|
||||
(2000.0f / 32768.0f)
|
||||
};
|
||||
|
||||
struct _Registers
|
||||
{
|
||||
static constexpr std::uint32_t ICM20689_ID = 0x98;
|
||||
struct _Registers
|
||||
{
|
||||
static constexpr std::uint32_t ICM20689_ID = 0x98;
|
||||
|
||||
static constexpr std::uint8_t READ_MASK = 0x80;
|
||||
static constexpr std::uint8_t WRITE_MASK = 0x7F;
|
||||
static constexpr std::uint8_t READ_MASK = 0x80;
|
||||
static constexpr std::uint8_t WRITE_MASK = 0x7F;
|
||||
|
||||
static constexpr std::uint8_t SMPLRT_DIV = 0x19;
|
||||
static constexpr std::uint8_t CONFIG = 0x1A;
|
||||
static constexpr std::uint8_t GYRO_CONFIG = 0x1B;
|
||||
static constexpr std::uint8_t ACCEL_CONFIG = 0x1C;
|
||||
static constexpr std::uint8_t ACCEL_CONFIG2 = 0x1D;
|
||||
static constexpr std::uint8_t SMPLRT_DIV = 0x19;
|
||||
static constexpr std::uint8_t CONFIG = 0x1A;
|
||||
static constexpr std::uint8_t GYRO_CONFIG = 0x1B;
|
||||
static constexpr std::uint8_t ACCEL_CONFIG =0x1C;
|
||||
static constexpr std::uint8_t ACCEL_CONFIG2=0x1D;
|
||||
|
||||
static constexpr std::uint8_t INT_PIN_CFG = 0x37;
|
||||
static constexpr std::uint8_t INT_ENABLE = 0x38;
|
||||
static constexpr std::uint8_t INT_STATUS = 0x3A;
|
||||
static constexpr std::uint8_t INT_PIN_CFG = 0x37;
|
||||
static constexpr std::uint8_t INT_ENABLE = 0x38;
|
||||
static constexpr std::uint8_t INT_STATUS = 0x3A;
|
||||
|
||||
static constexpr std::uint8_t GYRO_XOUT_H = 0x43;
|
||||
static constexpr std::uint8_t GYRO_XOUT_L = 0x44;
|
||||
static constexpr std::uint8_t GYRO_YOUT_H = 0x45;
|
||||
static constexpr std::uint8_t GYRO_YOUT_L = 0x46;
|
||||
static constexpr std::uint8_t GYRO_ZOUT_H = 0x47;
|
||||
static constexpr std::uint8_t GYRO_ZOUT_L = 0x48;
|
||||
static constexpr std::uint8_t GYRO_XOUT_H = 0x43;
|
||||
static constexpr std::uint8_t GYRO_XOUT_L = 0x44;
|
||||
static constexpr std::uint8_t GYRO_YOUT_H = 0x45;
|
||||
static constexpr std::uint8_t GYRO_YOUT_L = 0x46;
|
||||
static constexpr std::uint8_t GYRO_ZOUT_H = 0x47;
|
||||
static constexpr std::uint8_t GYRO_ZOUT_L = 0x48;
|
||||
|
||||
static constexpr std::uint8_t ACCEL_XOUT_H = 0x3B;
|
||||
static constexpr std::uint8_t ACCEL_XOUT_L = 0x3C;
|
||||
static constexpr std::uint8_t ACCEL_YOUT_H = 0x3D;
|
||||
static constexpr std::uint8_t ACCEL_YOUT_L = 0x3E;
|
||||
static constexpr std::uint8_t ACCEL_ZOUT_H = 0x3F;
|
||||
static constexpr std::uint8_t ACCEL_ZOUT_L = 0x40;
|
||||
static constexpr std::uint8_t ACCEL_XOUT_H =0x3B;
|
||||
static constexpr std::uint8_t ACCEL_XOUT_L =0x3C;
|
||||
static constexpr std::uint8_t ACCEL_YOUT_H =0x3D;
|
||||
static constexpr std::uint8_t ACCEL_YOUT_L =0x3E;
|
||||
static constexpr std::uint8_t ACCEL_ZOUT_H =0x3F;
|
||||
static constexpr std::uint8_t ACCEL_ZOUT_L =0x40;
|
||||
|
||||
static constexpr std::uint8_t USER_CTRL = 0x6A;
|
||||
static constexpr std::uint8_t PWR_MGMT_1 = 0x6B;
|
||||
static constexpr std::uint8_t PWR_MGMT_2 = 0x6C;
|
||||
static constexpr std::uint8_t WHO_AM_I = 0x75;
|
||||
static constexpr std::uint8_t USER_CTRL = 0x6A;
|
||||
static constexpr std::uint8_t PWR_MGMT_1 = 0x6B;
|
||||
static constexpr std::uint8_t PWR_MGMT_2 = 0x6C;
|
||||
static constexpr std::uint8_t WHO_AM_I = 0x75;
|
||||
|
||||
static constexpr std::uint8_t XG_OFFSET_H = 0x13;
|
||||
static constexpr std::uint8_t XG_OFFSET_L = 0x14;
|
||||
static constexpr std::uint8_t YG_OFFSET_H = 0x15;
|
||||
static constexpr std::uint8_t YG_OFFSET_L = 0x16;
|
||||
static constexpr std::uint8_t ZG_OFFSET_H = 0x17;
|
||||
static constexpr std::uint8_t ZG_OFFSET_L = 0x18;
|
||||
static constexpr std::uint8_t XG_OFFSET_H = 0x13;
|
||||
static constexpr std::uint8_t XG_OFFSET_L = 0x14;
|
||||
static constexpr std::uint8_t YG_OFFSET_H = 0x15;
|
||||
static constexpr std::uint8_t YG_OFFSET_L = 0x16;
|
||||
static constexpr std::uint8_t ZG_OFFSET_H = 0x17;
|
||||
static constexpr std::uint8_t ZG_OFFSET_L = 0x18;
|
||||
|
||||
static constexpr std::uint8_t XA_OFFSET_H = 0x77;
|
||||
static constexpr std::uint8_t XA_OFFSET_L = 0x78;
|
||||
static constexpr std::uint8_t YA_OFFSET_H = 0x7A;
|
||||
static constexpr std::uint8_t YA_OFFSET_L = 0x7B;
|
||||
static constexpr std::uint8_t ZA_OFFSET_H = 0x7D;
|
||||
static constexpr std::uint8_t ZA_OFFSET_L = 0x7E;
|
||||
};
|
||||
static constexpr std::uint8_t XA_OFFSET_H = 0x77;
|
||||
static constexpr std::uint8_t XA_OFFSET_L = 0x78;
|
||||
static constexpr std::uint8_t YA_OFFSET_H = 0x7A;
|
||||
static constexpr std::uint8_t YA_OFFSET_L = 0x7B;
|
||||
static constexpr std::uint8_t ZA_OFFSET_H = 0x7D;
|
||||
static constexpr std::uint8_t ZA_OFFSET_L = 0x7E;
|
||||
};
|
||||
};
|
||||
|
||||
template<typename T, typename HAL>
|
||||
@ -295,6 +285,6 @@ constexpr float ImuIcm<T, HAL>::_accel_fs_to_bit_constants[4];
|
||||
template<typename T, typename HAL>
|
||||
constexpr float ImuIcm<T, HAL>::_gyro_fs_to_bit_constants[4];
|
||||
|
||||
}// namespace Peripherals
|
||||
}
|
||||
|
||||
#endif /* PERIPHERALS_IMU_ICM_HPP_ */
|
||||
|
||||
@ -22,9 +22,9 @@ struct TwoChannelMotorData
|
||||
class IMotors
|
||||
{
|
||||
public:
|
||||
virtual void Set(const std::int16_t left, const std::int16_t right) = 0;
|
||||
virtual void Coast() = 0;
|
||||
virtual void Break() = 0;
|
||||
virtual void Set(const std::int16_t left, const std::int16_t right) = 0;
|
||||
virtual void Coast() = 0;
|
||||
virtual void Break() = 0;
|
||||
};
|
||||
|
||||
template<typename T, typename I>
|
||||
@ -34,65 +34,69 @@ public:
|
||||
using single_motor = TwoChannelMotorData<T>;
|
||||
using gpio = I;
|
||||
|
||||
DualDrvMotors(const single_motor& left, const single_motor& right,
|
||||
const gpio& sleep_pin)
|
||||
: _left(left), _right(right), _sleep_pin(sleep_pin)
|
||||
DualDrvMotors(const single_motor& left, const single_motor& right, const gpio& sleep_pin)
|
||||
: _left(left)
|
||||
, _right(right)
|
||||
, _sleep_pin(sleep_pin)
|
||||
{
|
||||
_left.forward.Enable();
|
||||
_left.backward.Enable();
|
||||
_right.forward.Enable();
|
||||
_right.backward.Enable();
|
||||
_left.forward.Enable();
|
||||
_left.backward.Enable();
|
||||
_right.forward.Enable();
|
||||
_right.backward.Enable();
|
||||
|
||||
Set(0, 0);
|
||||
Set(0, 0);
|
||||
}
|
||||
|
||||
virtual void Set(const std::int16_t left, const std::int16_t right) override
|
||||
{
|
||||
if (left > 0)
|
||||
{
|
||||
_left.forward.SetCompare(left);
|
||||
_left.backward.SetCompare(0);
|
||||
} else
|
||||
{
|
||||
_left.forward.SetCompare(0);
|
||||
_left.backward.SetCompare(-1 * left);
|
||||
}
|
||||
virtual void Set(const std::int16_t left, const std::int16_t right) override
|
||||
{
|
||||
if (left > 0)
|
||||
{
|
||||
_left.forward.SetCompare(left);
|
||||
_left.backward.SetCompare(0);
|
||||
}
|
||||
else
|
||||
{
|
||||
_left.forward.SetCompare(0);
|
||||
_left.backward.SetCompare(-1 * left);
|
||||
}
|
||||
|
||||
if (right > 0)
|
||||
{
|
||||
_right.forward.SetCompare(right);
|
||||
_right.backward.SetCompare(0);
|
||||
} else
|
||||
{
|
||||
_right.forward.SetCompare(0);
|
||||
_right.backward.SetCompare(-1 * right);
|
||||
}
|
||||
}
|
||||
if (right > 0)
|
||||
{
|
||||
_right.forward.SetCompare(right);
|
||||
_right.backward.SetCompare(0);
|
||||
}
|
||||
else
|
||||
{
|
||||
_right.forward.SetCompare(0);
|
||||
_right.backward.SetCompare(-1 * right);
|
||||
}
|
||||
}
|
||||
|
||||
virtual void Coast() override
|
||||
{
|
||||
_left.forward.SetCompare(0);
|
||||
_left.backward.SetCompare(0);
|
||||
virtual void Coast() override
|
||||
{
|
||||
_left.forward.SetCompare(0);
|
||||
_left.backward.SetCompare(0);
|
||||
|
||||
_right.forward.SetCompare(0);
|
||||
_right.backward.SetCompare(0);
|
||||
}
|
||||
_right.forward.SetCompare(0);
|
||||
_right.backward.SetCompare(0);
|
||||
}
|
||||
|
||||
virtual void Break() override
|
||||
{
|
||||
virtual void Break() override
|
||||
{
|
||||
_left.forward.SetCompare(_left.forward.MaxValue());
|
||||
_left.backward.SetCompare(_left.backward.MaxValue());
|
||||
|
||||
_right.forward.SetCompare(_right.forward.MaxValue());
|
||||
_right.backward.SetCompare(_right.backward.MaxValue());
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
single_motor _left;
|
||||
single_motor _right;
|
||||
gpio _sleep_pin;
|
||||
single_motor _left;
|
||||
single_motor _right;
|
||||
gpio _sleep_pin;
|
||||
};
|
||||
|
||||
}// namespace Peripherals
|
||||
}
|
||||
|
||||
|
||||
#endif /* PERIPHERALS_MOTORS_HPP_ */
|
||||
|
||||
@ -19,25 +19,28 @@ namespace Peripherals
|
||||
|
||||
struct PwmChannel
|
||||
{
|
||||
TIM_HandleTypeDef* timer;
|
||||
std::uint32_t channel;
|
||||
std::uint32_t timer_code;
|
||||
Io pin;
|
||||
TIM_HandleTypeDef* timer;
|
||||
std::uint32_t channel;
|
||||
std::uint32_t timer_code;
|
||||
Io pin;
|
||||
|
||||
PwmChannel() = delete;
|
||||
PwmChannel() = delete;
|
||||
|
||||
PwmChannel(TIM_HandleTypeDef* timer, const std::uint32_t channel,
|
||||
const std::uint32_t timer_code, const Io& pin);
|
||||
PwmChannel(TIM_HandleTypeDef* timer,
|
||||
const std::uint32_t channel,
|
||||
const std::uint32_t timer_code,
|
||||
const Io& pin);
|
||||
|
||||
void PinToPwm();
|
||||
void PinToGpio();
|
||||
void PinToPwm();
|
||||
void PinToGpio();
|
||||
|
||||
void Enable();
|
||||
void Disable();
|
||||
void Enable();
|
||||
void Disable();
|
||||
|
||||
void SetCompare(const std::uint32_t compare);
|
||||
void SetCompare(const std::uint32_t compare);
|
||||
};
|
||||
|
||||
}// namespace Peripherals
|
||||
}
|
||||
|
||||
|
||||
#endif /* PERIPHERALS_PWM_CHANNEL_HPP_ */
|
||||
|
||||
@ -36,7 +36,9 @@ struct Rgb
|
||||
|
||||
Rgb() = delete;
|
||||
Rgb(const IOType& r, const IOType& g, const IOType& b)
|
||||
: red(r), green(g), blue(b)
|
||||
: red(r)
|
||||
, green(g)
|
||||
, blue(b)
|
||||
{
|
||||
Set(RgbColor::OFF);
|
||||
}
|
||||
@ -47,50 +49,51 @@ struct Rgb
|
||||
|
||||
switch (color)
|
||||
{
|
||||
case RgbColor::OFF:
|
||||
red.Set(false);
|
||||
green.Set(false);
|
||||
blue.Set(false);
|
||||
break;
|
||||
case RgbColor::RED:
|
||||
red.Set(true);
|
||||
green.Set(false);
|
||||
blue.Set(false);
|
||||
break;
|
||||
case RgbColor::GREEN:
|
||||
red.Set(false);
|
||||
green.Set(true);
|
||||
blue.Set(false);
|
||||
break;
|
||||
case RgbColor::BLUE:
|
||||
red.Set(false);
|
||||
green.Set(false);
|
||||
blue.Set(true);
|
||||
break;
|
||||
case RgbColor::CYAN:
|
||||
red.Set(false);
|
||||
green.Set(true);
|
||||
blue.Set(true);
|
||||
break;
|
||||
case RgbColor::PINK:
|
||||
red.Set(true);
|
||||
green.Set(false);
|
||||
blue.Set(true);
|
||||
break;
|
||||
case RgbColor::YELLOW:
|
||||
red.Set(true);
|
||||
green.Set(true);
|
||||
blue.Set(false);
|
||||
break;
|
||||
case RgbColor::WHITE:
|
||||
red.Set(true);
|
||||
green.Set(true);
|
||||
blue.Set(true);
|
||||
break;
|
||||
case RgbColor::OFF:
|
||||
red.Set(false);
|
||||
green.Set(false);
|
||||
blue.Set(false);
|
||||
break;
|
||||
case RgbColor::RED:
|
||||
red.Set(true);
|
||||
green.Set(false);
|
||||
blue.Set(false);
|
||||
break;
|
||||
case RgbColor::GREEN:
|
||||
red.Set(false);
|
||||
green.Set(true);
|
||||
blue.Set(false);
|
||||
break;
|
||||
case RgbColor::BLUE:
|
||||
red.Set(false);
|
||||
green.Set(false);
|
||||
blue.Set(true);
|
||||
break;
|
||||
case RgbColor::CYAN:
|
||||
red.Set(false);
|
||||
green.Set(true);
|
||||
blue.Set(true);
|
||||
break;
|
||||
case RgbColor::PINK:
|
||||
red.Set(true);
|
||||
green.Set(false);
|
||||
blue.Set(true);
|
||||
break;
|
||||
case RgbColor::YELLOW:
|
||||
red.Set(true);
|
||||
green.Set(true);
|
||||
blue.Set(false);
|
||||
break;
|
||||
case RgbColor::WHITE:
|
||||
red.Set(true);
|
||||
green.Set(true);
|
||||
blue.Set(true);
|
||||
break;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
}// namespace Peripherals
|
||||
}
|
||||
|
||||
|
||||
#endif /* PERIPHERALS_INC_PERIPHERALS_RGB_HPP_ */
|
||||
|
||||
@ -13,47 +13,48 @@
|
||||
namespace Peripherals
|
||||
{
|
||||
|
||||
#define SKULLC_CONCAT_IMPL(x, y) x##y
|
||||
#define SKULLC_CONCAT_IMPL(x, y) x ## y
|
||||
#define SKULLC_CONCAT(x, y) SKULLC_CONCAT_IMPL(x, y)
|
||||
#define SKULLC_TAG struct SKULLC_CONCAT(SkullCTag_, __COUNTER__)
|
||||
|
||||
template<typename T>
|
||||
constexpr const T& Clamp(const T& v, const T& lo, const T& hi)
|
||||
{
|
||||
if (v > hi)
|
||||
return hi;
|
||||
else if (v < lo)
|
||||
return lo;
|
||||
else
|
||||
return v;
|
||||
if (v > hi)
|
||||
return hi;
|
||||
else if (v < lo)
|
||||
return lo;
|
||||
else
|
||||
return v;
|
||||
}
|
||||
|
||||
template<typename T, std::size_t N>
|
||||
T ByteToTypeBE(const std::uint8_t a[N])
|
||||
{
|
||||
T t(0);
|
||||
T t(0);
|
||||
|
||||
for (std::size_t i = 0; i < N; i++)
|
||||
{
|
||||
t |= a[i] << (i * 8);
|
||||
}
|
||||
for (std::size_t i = 0; i < N; i++)
|
||||
{
|
||||
t |= a[i] << (i * 8);
|
||||
}
|
||||
|
||||
return t;
|
||||
return t;
|
||||
}
|
||||
|
||||
template<typename T, std::size_t N>
|
||||
T ByteToTypeLE(const std::uint8_t a[N])
|
||||
{
|
||||
T t(0);
|
||||
T t(0);
|
||||
|
||||
for (std::size_t i = N; i >= 0; i--)
|
||||
{
|
||||
t |= a[i] << ((i - 1) * 8);
|
||||
}
|
||||
for (std::size_t i = N; i >= 0; i--)
|
||||
{
|
||||
t |= a[i] << ((i - 1) * 8);
|
||||
}
|
||||
|
||||
return t;
|
||||
}
|
||||
|
||||
return t;
|
||||
}
|
||||
|
||||
}// namespace Peripherals
|
||||
|
||||
#endif /* PERIPHERALS_UTILITY_HPP_ */
|
||||
|
||||
@ -2,6 +2,5 @@
|
||||
// Created by erki on 13.03.21.
|
||||
//
|
||||
|
||||
#define CATCH_CONFIG_MAIN// This tells Catch to provide a main() - only do this \
|
||||
// in one cpp file
|
||||
#define CATCH_CONFIG_MAIN // This tells Catch to provide a main() - only do this in one cpp file
|
||||
#include <catch2/catch.hpp>
|
||||
|
||||
@ -12,7 +12,7 @@ TEST_CASE("Packet copy_data_in copies data in.", "[messaging],[packet]")
|
||||
|
||||
SECTION("Plain C-array data copies properly.")
|
||||
{
|
||||
const std::uint8_t data[2] = {'C', 'D'};
|
||||
const std::uint8_t data[2] = { 'C', 'D' };
|
||||
const std::uint32_t data_size = sizeof(data);
|
||||
|
||||
packet.copy_data_in(data);
|
||||
@ -24,7 +24,7 @@ TEST_CASE("Packet copy_data_in copies data in.", "[messaging],[packet]")
|
||||
|
||||
SECTION("STL data copies properly.")
|
||||
{
|
||||
const std::array<std::uint8_t, 2> data = {'C', 'D'};
|
||||
const std::array<std::uint8_t, 2> data = { 'C', 'D' };
|
||||
|
||||
packet.copy_data_in(data);
|
||||
|
||||
@ -34,12 +34,11 @@ TEST_CASE("Packet copy_data_in copies data in.", "[messaging],[packet]")
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("Packet copying data in cuts off excess bytes.",
|
||||
"[messaging],[packet]")
|
||||
TEST_CASE("Packet copying data in cuts off excess bytes.", "[messaging],[packet]")
|
||||
{
|
||||
Messaging::Packet<2> packet;
|
||||
|
||||
const std::uint8_t data[4] = {'C', 'D', 'E', 'F'};
|
||||
const std::uint8_t data[4] = { 'C', 'D', 'E', 'F' };
|
||||
|
||||
packet.copy_data_in(data);
|
||||
|
||||
@ -52,7 +51,7 @@ TEST_CASE("Packet gets serialized properly.", "[messaging],[packet]")
|
||||
{
|
||||
Messaging::Packet<2> packet;
|
||||
|
||||
const std::uint8_t data[2] = {'C', 'D'};
|
||||
const std::uint8_t data[2] = { 'C', 'D' };
|
||||
const std::uint32_t data_size = sizeof(data);
|
||||
|
||||
packet.copy_data_in(data);
|
||||
@ -83,15 +82,14 @@ TEST_CASE("Packet gets serialized properly.", "[messaging],[packet]")
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("Packet serialization fails if buffer too small.",
|
||||
"[messaging],[packet]")
|
||||
TEST_CASE("Packet serialization fails if buffer too small.", "[messaging],[packet]")
|
||||
{
|
||||
Messaging::Packet<2> packet;
|
||||
|
||||
const std::uint8_t data[2] = {'C', 'D'};
|
||||
const std::uint8_t data[2] = { 'C', 'D' };
|
||||
packet.copy_data_in(data);
|
||||
|
||||
std::uint8_t output[4] = {0};
|
||||
std::uint8_t output[4] = { 0 };
|
||||
REQUIRE(packet.serialize(output, sizeof(output)) == false);
|
||||
|
||||
SECTION("Output buffer is left unmodified.")
|
||||
@ -105,7 +103,9 @@ TEST_CASE("Packet serialization fails if buffer too small.",
|
||||
|
||||
TEST_CASE("Packet deserialization works as expected.", "[messaging],[packet]")
|
||||
{
|
||||
std::uint8_t data[] = {'A', 'A', 0, 0, 0, 0, 'C', 'D'};
|
||||
std::uint8_t data[] = {
|
||||
'A', 'A', 0, 0, 0, 0, 'C', 'D'
|
||||
};
|
||||
|
||||
const std::uint32_t data_length = 2;
|
||||
std::memcpy(data + 2, &data_length, sizeof(data_length));
|
||||
@ -118,10 +118,11 @@ TEST_CASE("Packet deserialization works as expected.", "[messaging],[packet]")
|
||||
REQUIRE(packet.data[1] == 'D');
|
||||
}
|
||||
|
||||
TEST_CASE("Packet deserialization fails with invalid conditions.",
|
||||
"[messaging],[packet]")
|
||||
TEST_CASE("Packet deserialization fails with invalid conditions.", "[messaging],[packet]")
|
||||
{
|
||||
std::uint8_t data[] = {'A', 'A', 0, 0, 0, 0, 'C', 'D'};
|
||||
std::uint8_t data[] = {
|
||||
'A', 'A', 0, 0, 0, 0, 'C', 'D'
|
||||
};
|
||||
|
||||
const std::uint32_t data_length = 2;
|
||||
std::memcpy(data + 2, &data_length, sizeof(data_length));
|
||||
|
||||
@ -4,20 +4,21 @@
|
||||
|
||||
#include <catch2/catch.hpp>
|
||||
|
||||
#include <messaging_packet.hpp>
|
||||
#include <messaging_parser.hpp>
|
||||
#include <messaging_packet.hpp>
|
||||
|
||||
using Packet = Messaging::Packet<2>;
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
std::array<std::uint8_t, 2> test_data = {'C', 'D'};
|
||||
std::array<std::uint8_t, 2> test_data = { 'C', 'D' };
|
||||
|
||||
std::array<std::uint8_t, 8>
|
||||
getRawData(const std::array<std::uint8_t, 2>& data)
|
||||
std::array<std::uint8_t, 8> getRawData(const std::array<std::uint8_t, 2>& data)
|
||||
{
|
||||
std::array<std::uint8_t, 8> raw = {'A', 'A', 0, 0, 0, 0, data[0], data[1]};
|
||||
std::array<std::uint8_t, 8> raw = {
|
||||
'A', 'A', 0, 0, 0, 0, data[0], data[1]
|
||||
};
|
||||
|
||||
const std::uint32_t len = 2;
|
||||
std::memcpy(raw.data() + 2, &len, 4);
|
||||
@ -25,9 +26,12 @@ getRawData(const std::array<std::uint8_t, 2>& data)
|
||||
return raw;
|
||||
}
|
||||
|
||||
std::array<std::uint8_t, 8> getRawData() { return getRawData({'C', 'D'}); }
|
||||
std::array<std::uint8_t, 8> getRawData()
|
||||
{
|
||||
return getRawData({'C', 'D'});
|
||||
}
|
||||
|
||||
}// namespace
|
||||
}
|
||||
|
||||
TEST_CASE("Parser parses raw message successfully.", "[messaging],[parser]")
|
||||
{
|
||||
@ -62,7 +66,7 @@ TEST_CASE("Parser ignores extra bytes when done.", "[messaging],[parser]")
|
||||
|
||||
REQUIRE(parser.packetReady());
|
||||
|
||||
for (const std::uint8_t& byte : getRawData({'E', 'F'}))
|
||||
for (const std::uint8_t& byte : getRawData({ 'E', 'F' }))
|
||||
{
|
||||
parser.pushByte(byte);
|
||||
}
|
||||
@ -78,12 +82,12 @@ TEST_CASE("Parser ignores extra bytes when done.", "[messaging],[parser]")
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("Parser ignores junk data until header is spotted.",
|
||||
"[messaging],[parser]")
|
||||
TEST_CASE("Parser ignores junk data until header is spotted.", "[messaging],[parser]")
|
||||
{
|
||||
Messaging::Parser<Packet, Packet::totalLength()> parser;
|
||||
const std::array<std::uint8_t, 8> junk_data = {'E', 'F', 'A', 'H',
|
||||
'I', 'J', 'K', 'L'};
|
||||
const std::array<std::uint8_t, 8> junk_data = {
|
||||
'E', 'F', 'A', 'H', 'I', 'J', 'K', 'L'
|
||||
};
|
||||
|
||||
for (const std::uint8_t& byte : junk_data)
|
||||
{
|
||||
|
||||
@ -9,7 +9,7 @@
|
||||
template<size_t N>
|
||||
using Ringbuffer = Utility::Ringbuffer<int, N>;
|
||||
|
||||
TEST_CASE("Ringbuffer iterator", "[utility],[ringbuffer]")
|
||||
TEST_CASE("Ringbuffer iterator", "[peripherals],[ringbuffer]")
|
||||
{
|
||||
using iterator = Ringbuffer<10>::iterator;
|
||||
const auto begin = iterator::pointer(10);
|
||||
@ -43,7 +43,7 @@ TEST_CASE("Ringbuffer iterator", "[utility],[ringbuffer]")
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("Ringbuffer iterator at the end", "[utility],[ringbuffer]")
|
||||
TEST_CASE("Ringbuffer iterator at the end", "[peripherals],[ringbuffer]")
|
||||
{
|
||||
using iterator = Ringbuffer<10>::iterator;
|
||||
const auto begin = iterator::pointer(10);
|
||||
@ -71,7 +71,7 @@ TEST_CASE("Ringbuffer iterator at the end", "[utility],[ringbuffer]")
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("Ringbuffer iterator at the beginning", "[utility],[ringbuffer]")
|
||||
TEST_CASE("Ringbuffer iterator at the beginning", "[peripherals],[ringbuffer]")
|
||||
{
|
||||
using iterator = Ringbuffer<10>::iterator;
|
||||
const auto begin = iterator::pointer(10);
|
||||
@ -87,17 +87,16 @@ TEST_CASE("Ringbuffer iterator at the beginning", "[utility],[ringbuffer]")
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("Ringbuffer constructed buffer is empty.", "[utility],[ringbuffer]")
|
||||
TEST_CASE("Constructed buffer is empty.", "[peripherals],[ringbuffer]")
|
||||
{
|
||||
Ringbuffer<10> buffer;
|
||||
|
||||
REQUIRE(buffer.begin() == buffer.end());
|
||||
REQUIRE(buffer.size() == 0);
|
||||
REQUIRE(buffer.empty());
|
||||
REQUIRE(&buffer.front() == &buffer.back());
|
||||
}
|
||||
|
||||
TEST_CASE("Ringbuffer reports size properly.", "[utility],[ringbuffer]")
|
||||
TEST_CASE("Buffer reports size properly.", "[peripherals],[ringbuffer]")
|
||||
{
|
||||
Ringbuffer<3> buffer;
|
||||
|
||||
@ -141,7 +140,7 @@ TEST_CASE("Ringbuffer reports size properly.", "[utility],[ringbuffer]")
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("Ringbuffer adding single element.", "[utility],[ringbuffer]")
|
||||
TEST_CASE("Adding single element.", "[peripherals],[ringbuffer]")
|
||||
{
|
||||
Ringbuffer<10> buffer;
|
||||
const auto old_end = buffer.end();
|
||||
@ -155,9 +154,15 @@ TEST_CASE("Ringbuffer adding single element.", "[utility],[ringbuffer]")
|
||||
REQUIRE(!buffer.empty());
|
||||
}
|
||||
|
||||
SECTION("Updates end() appropriately.") { REQUIRE(old_end != buffer.end()); }
|
||||
SECTION("Updates end() appropriately.")
|
||||
{
|
||||
REQUIRE(old_end != buffer.end());
|
||||
}
|
||||
|
||||
SECTION("begin() remains the same.") { REQUIRE(old_begin == buffer.begin()); }
|
||||
SECTION("begin() remains the same.")
|
||||
{
|
||||
REQUIRE(old_begin == buffer.begin());
|
||||
}
|
||||
|
||||
SECTION("Makes begin() refer to the inserted member.")
|
||||
{
|
||||
@ -186,7 +191,7 @@ TEST_CASE("Ringbuffer adding single element.", "[utility],[ringbuffer]")
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("Ringbuffer adding multiple elements.", "[utility],[ringbuffer]")
|
||||
TEST_CASE("Adding multiple elements.", "[peripherals],[ringbuffer]")
|
||||
{
|
||||
Ringbuffer<10> buffer;
|
||||
const auto old_begin = buffer.begin();
|
||||
@ -218,8 +223,7 @@ TEST_CASE("Ringbuffer adding multiple elements.", "[utility],[ringbuffer]")
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("Ringbuffer removing elements from the ringbuffer.",
|
||||
"[utility],[ringbuffer]")
|
||||
TEST_CASE("Removing elements from the ringbuffer.", "[peripherals],[ringbuffer]")
|
||||
{
|
||||
Ringbuffer<10> buffer;
|
||||
const auto old_begin = buffer.begin();
|
||||
@ -240,25 +244,34 @@ TEST_CASE("Ringbuffer removing elements from the ringbuffer.",
|
||||
REQUIRE(buffer.begin() - old_begin == 1);
|
||||
}
|
||||
|
||||
SECTION("Updates size() appropriately.") { REQUIRE(buffer.size() == 2); }
|
||||
SECTION("Updates size() appropriately.")
|
||||
{
|
||||
REQUIRE(buffer.size() == 2);
|
||||
}
|
||||
|
||||
SECTION("Erasing remaining elements")
|
||||
{
|
||||
buffer.pop_front();
|
||||
buffer.pop_front();
|
||||
|
||||
SECTION("Updates empty() appropriately.") { REQUIRE(buffer.empty()); }
|
||||
SECTION("Updates empty() appropriately.")
|
||||
{
|
||||
REQUIRE(buffer.empty());
|
||||
}
|
||||
|
||||
SECTION("Updates begin() and end() appropriately.")
|
||||
{
|
||||
REQUIRE(buffer.begin() == buffer.end());
|
||||
}
|
||||
|
||||
SECTION("Updates size() appropriately.") { REQUIRE(buffer.size() == 0); }
|
||||
SECTION("Updates size() appropriately.")
|
||||
{
|
||||
REQUIRE(buffer.size() == 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("Ringbuffer clearing a ringbuffer works.", "[utility],[ringbuffer]")
|
||||
TEST_CASE("Clearing a ringbuffer works.", "[peripherals],[ringbuffer]")
|
||||
{
|
||||
Ringbuffer<10> buffer;
|
||||
|
||||
@ -277,12 +290,10 @@ TEST_CASE("Ringbuffer clearing a ringbuffer works.", "[utility],[ringbuffer]")
|
||||
SECTION("Sets begin() and end() pointers appropriately.")
|
||||
{
|
||||
REQUIRE(buffer.begin() == buffer.end());
|
||||
REQUIRE(&buffer.front() == &buffer.back());
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("Ringbuffer manually incrementing tail works.",
|
||||
"[utility],[ringbuffer]")
|
||||
TEST_CASE("Manually incrementing tail works.", "[peripherals],[ringbuffer]")
|
||||
{
|
||||
Ringbuffer<10> buffer;
|
||||
|
||||
@ -301,57 +312,7 @@ TEST_CASE("Ringbuffer manually incrementing tail works.",
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("Ringbuffer manually incrementing tail from empty state works.",
|
||||
"[utility],[ringbuffer]")
|
||||
{
|
||||
Ringbuffer<10> buffer;
|
||||
|
||||
const auto old_end = buffer.end();
|
||||
const auto old_begin = buffer.begin();
|
||||
auto* old_tail_item_ptr = &(*old_end);
|
||||
|
||||
buffer.increment_tail();
|
||||
|
||||
SECTION("Head remains in place.")
|
||||
{
|
||||
const auto new_end = buffer.end();
|
||||
const auto new_begin = buffer.begin();
|
||||
REQUIRE(old_begin == new_begin);
|
||||
REQUIRE(new_end != new_begin);
|
||||
REQUIRE(new_end - new_begin == 1);
|
||||
}
|
||||
|
||||
SECTION("Tail has been incremented.")
|
||||
{
|
||||
const auto new_end = buffer.end();
|
||||
REQUIRE(new_end != old_end);
|
||||
REQUIRE(new_end - old_end == 1);
|
||||
REQUIRE(buffer.size() == 1);
|
||||
}
|
||||
|
||||
SECTION("Item pointer for end is increased appropriately.")
|
||||
{
|
||||
auto* new_tail_item_ptr = &(*buffer.end());
|
||||
REQUIRE(new_tail_item_ptr != old_tail_item_ptr);
|
||||
REQUIRE(new_tail_item_ptr - old_tail_item_ptr == 1);
|
||||
}
|
||||
|
||||
SECTION("Second incrementation works as expected.")
|
||||
{
|
||||
buffer.increment_tail();
|
||||
const auto new_end = buffer.end();
|
||||
const auto new_begin = buffer.begin();
|
||||
auto* new_tail_item_ptr = &(*buffer.end());
|
||||
|
||||
REQUIRE(new_begin == old_begin);
|
||||
REQUIRE(new_end - old_end == 2);
|
||||
REQUIRE(new_tail_item_ptr - old_tail_item_ptr == 2);
|
||||
REQUIRE(buffer.size() == 2);
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("Ringbuffer manually incrementing tail when full deletes data.",
|
||||
"[utility],[ringbuffer]")
|
||||
TEST_CASE("Manually incrementing tail when full deletes data.", "[peripherals],[ringbuffer]")
|
||||
{
|
||||
Ringbuffer<2> buffer;
|
||||
|
||||
|
||||
@ -9,8 +9,8 @@
|
||||
#define UTILITY_INC_UTILITY_ASYNCLOGGER_HPP_
|
||||
|
||||
#include "utility_atomicscopeguard.hpp"
|
||||
#include "utility_ilogger.hpp"
|
||||
#include "utility_ringbuffer.hpp"
|
||||
#include "utility_ilogger.hpp"
|
||||
|
||||
#include <array>
|
||||
#include <cstdarg>
|
||||
@ -19,8 +19,7 @@
|
||||
namespace Utility
|
||||
{
|
||||
|
||||
template<typename T, typename H, std::size_t BufferCount,
|
||||
std::size_t BufferSize>
|
||||
template<typename T, typename H, std::size_t BufferCount, std::size_t BufferSize>
|
||||
class AsyncLogger : public ILogger
|
||||
{
|
||||
public:
|
||||
@ -28,7 +27,9 @@ public:
|
||||
using hal = H;
|
||||
|
||||
AsyncLogger() = delete;
|
||||
explicit AsyncLogger(const serial_interface& serial) : _serial(serial) {}
|
||||
explicit AsyncLogger(const serial_interface& serial)
|
||||
: _serial(serial)
|
||||
{ }
|
||||
|
||||
AsyncLogger(const AsyncLogger&) = delete;
|
||||
AsyncLogger(AsyncLogger&&) = delete;
|
||||
@ -47,9 +48,10 @@ public:
|
||||
std::va_list args;
|
||||
va_start(args, format);
|
||||
|
||||
_Data& tail = (*_buffer_queue.end());
|
||||
tail.length =
|
||||
vsnprintf(tail.buffer.data(), tail.buffer.size(), format, args);
|
||||
auto tail = _buffer_queue.end();
|
||||
std::array<char, 255>& buffer = tail->buffer;
|
||||
|
||||
tail->length = vsnprintf(buffer.data(), buffer.size(), format, args);
|
||||
|
||||
{
|
||||
AtomicScopeGuard<hal> s;
|
||||
@ -58,8 +60,6 @@ public:
|
||||
if (!_in_flight)
|
||||
_sendNextLog();
|
||||
}
|
||||
|
||||
va_end(args);
|
||||
}
|
||||
|
||||
void txCompleteCallback()
|
||||
@ -86,12 +86,12 @@ private:
|
||||
_in_flight = true;
|
||||
|
||||
_Data& head = _buffer_queue.front();
|
||||
_serial.Transmit(reinterpret_cast<uint8_t*>(head.buffer.data()),
|
||||
head.length);
|
||||
_serial.Transmit(reinterpret_cast<uint8_t*>(head.buffer.data()), head.length);
|
||||
_buffer_queue.pop_front();
|
||||
}
|
||||
};
|
||||
|
||||
}// namespace Utility
|
||||
}
|
||||
|
||||
|
||||
#endif /* UTILITY_INC_UTILITY_ASYNCLOGGER_HPP_ */
|
||||
|
||||
@ -36,6 +36,7 @@ struct AtomicScopeGuard
|
||||
hal::enableInterrupts();
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
static std::int32_t _reentrancy_counter;
|
||||
};
|
||||
@ -43,6 +44,6 @@ private:
|
||||
template<typename H>
|
||||
std::int32_t AtomicScopeGuard<H>::_reentrancy_counter = 0;
|
||||
|
||||
}// namespace Utility
|
||||
}
|
||||
|
||||
#endif /* UTILITY_INC_UTILITY_ATOMICSCOPEGUARD_HPP_ */
|
||||
|
||||
@ -26,8 +26,14 @@ public:
|
||||
LOG_FATAL
|
||||
};
|
||||
|
||||
constexpr static const char* level_strs[] = {"DEBUG", "INFO", "NOTICE",
|
||||
"WARNING", "ERROR", "FATAL"};
|
||||
constexpr static const char* level_strs[] = {
|
||||
"DEBUG",
|
||||
"INFO",
|
||||
"NOTICE",
|
||||
"WARNING",
|
||||
"ERROR",
|
||||
"FATAL"
|
||||
};
|
||||
|
||||
ILogger() = default;
|
||||
ILogger(const ILogger&) = delete;
|
||||
@ -38,6 +44,6 @@ public:
|
||||
virtual void log(const char* format, ...) = 0;
|
||||
};
|
||||
|
||||
}// namespace Utility
|
||||
}
|
||||
|
||||
#endif// SKULLC_UTILITY_ILOGGER_HPP_
|
||||
#endif //SKULLC_UTILITY_ILOGGER_HPP_
|
||||
|
||||
@ -7,22 +7,13 @@
|
||||
|
||||
#include "utility_ilogger.hpp"
|
||||
|
||||
#define SKULLC_LOG(sev, msg, ...) \
|
||||
Utility::skullc_logger->log("%s: " msg "\n\r", \
|
||||
Utility::ILogger::level_strs[std::uint8_t(sev)], \
|
||||
##__VA_ARGS__)
|
||||
#define SKULLC_LOG_DEBUG(msg, ...) \
|
||||
SKULLC_LOG(Utility::ILogger::LogLevel::LOG_DEBUG, msg, ##__VA_ARGS__)
|
||||
#define SKULLC_LOG_INFO(msg, ...) \
|
||||
SKULLC_LOG(Utility::ILogger::LogLevel::LOG_INFO, msg, ##__VA_ARGS__)
|
||||
#define SKULLC_LOG_NOTICE(msg, ...) \
|
||||
SKULLC_LOG(Utility::ILogger::LogLevel::LOG_NOTICE, msg, ##__VA_ARGS__)
|
||||
#define SKULLC_LOG_WARNING(msg, ...) \
|
||||
SKULLC_LOG(Utility::ILogger::LogLevel::LOG_WARNING, msg, ##__VA_ARGS__)
|
||||
#define SKULLC_LOG_ERROR(msg, ...) \
|
||||
SKULLC_LOG(Utility::ILogger::LogLevel::LOG_ERROR, msg, ##__VA_ARGS__)
|
||||
#define SKULLC_LOG_FATAL(msg, ...) \
|
||||
SKULLC_LOG(Utility::ILogger::LogLevel::LOG_FATAL, msg, ##__VA_ARGS__)
|
||||
#define SKULLC_LOG(sev, msg, ...) Utility::skullc_logger->log("%s: " msg "\n\r", Utility::ILogger::level_strs[std::uint8_t(sev)], ## __VA_ARGS__)
|
||||
#define SKULLC_LOG_DEBUG(msg, ...) SKULLC_LOG(Utility::ILogger::LogLevel::LOG_DEBUG, msg, ## __VA_ARGS__)
|
||||
#define SKULLC_LOG_INFO(msg, ...) SKULLC_LOG(Utility::ILogger::LogLevel::LOG_INFO, msg, ## __VA_ARGS__)
|
||||
#define SKULLC_LOG_NOTICE(msg, ...) SKULLC_LOG(Utility::ILogger::LogLevel::LOG_NOTICE, msg, ## __VA_ARGS__)
|
||||
#define SKULLC_LOG_WARNING(msg, ...) SKULLC_LOG(Utility::ILogger::LogLevel::LOG_WARNING, msg, ## __VA_ARGS__)
|
||||
#define SKULLC_LOG_ERROR(msg, ...) SKULLC_LOG(Utility::ILogger::LogLevel::LOG_ERROR, msg, ## __VA_ARGS__)
|
||||
#define SKULLC_LOG_FATAL(msg, ...) SKULLC_LOG(Utility::ILogger::LogLevel::LOG_FATAL, msg, ## __VA_ARGS__)
|
||||
|
||||
namespace Utility
|
||||
{
|
||||
@ -32,6 +23,6 @@ extern ILogger* skullc_logger;
|
||||
void setLogger(ILogger* log);
|
||||
void setLogger(ILogger& log);
|
||||
|
||||
}// namespace Utility
|
||||
}
|
||||
|
||||
#endif// UTILITY_LOGGING_HPP_
|
||||
#endif //UTILITY_LOGGING_HPP_
|
||||
|
||||
@ -9,8 +9,8 @@
|
||||
#define UTILITY_RINGBUFFER_HPP_
|
||||
|
||||
#include <array>
|
||||
#include <cstddef>
|
||||
#include <iterator>
|
||||
#include <cstddef>
|
||||
|
||||
namespace Utility
|
||||
{
|
||||
@ -19,176 +19,197 @@ template<typename T, size_t N>
|
||||
class Ringbuffer
|
||||
{
|
||||
public:
|
||||
using value_type = T;
|
||||
using size_type = std::size_t;
|
||||
using difference_type = std::ptrdiff_t;
|
||||
using reference = value_type&;
|
||||
using const_reference = const value_type&;
|
||||
using pointer = value_type*;
|
||||
using const_pointer = const value_type*;
|
||||
using value_type = T;
|
||||
using size_type = std::size_t;
|
||||
using difference_type = std::ptrdiff_t;
|
||||
using reference = value_type&;
|
||||
using const_reference = const value_type&;
|
||||
using pointer = value_type*;
|
||||
using const_pointer = const value_type*;
|
||||
|
||||
struct iterator
|
||||
{
|
||||
using iterator_category = std::forward_iterator_tag;
|
||||
using difference_type = std::ptrdiff_t;
|
||||
using value_type = T;
|
||||
using pointer = value_type*;
|
||||
using reference = value_type&;
|
||||
struct iterator
|
||||
{
|
||||
using iterator_category = std::forward_iterator_tag;
|
||||
using difference_type = std::ptrdiff_t;
|
||||
using value_type = T;
|
||||
using pointer = value_type*;
|
||||
using reference = value_type&;
|
||||
|
||||
iterator(const pointer ptr, const pointer begin, const pointer end)
|
||||
: _ptr(ptr), _arr_begin(begin), _arr_end(end) {}
|
||||
iterator(const iterator&) = default;
|
||||
iterator(iterator&&) noexcept = default;
|
||||
iterator(const pointer ptr, const pointer begin, const pointer end)
|
||||
: _ptr(ptr)
|
||||
, _arr_begin(begin)
|
||||
, _arr_end(end)
|
||||
{ }
|
||||
iterator(const iterator&) = default;
|
||||
iterator(iterator&&) noexcept = default;
|
||||
|
||||
iterator& operator=(const iterator&) = default;
|
||||
iterator& operator=(const iterator&) = default;
|
||||
|
||||
reference operator*() const { return *_ptr; }
|
||||
reference operator*() const
|
||||
{
|
||||
return *_ptr;
|
||||
}
|
||||
|
||||
pointer operator->() { return _ptr; }
|
||||
pointer operator->()
|
||||
{
|
||||
return _ptr;
|
||||
}
|
||||
|
||||
iterator& operator++()
|
||||
{
|
||||
_ptr++;
|
||||
if (_ptr == _arr_end)
|
||||
_ptr = _arr_begin;
|
||||
iterator& operator++()
|
||||
{
|
||||
_ptr++;
|
||||
if (_ptr == _arr_end)
|
||||
_ptr = _arr_begin;
|
||||
|
||||
return *this;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
iterator operator++(int)
|
||||
{
|
||||
iterator tmp = *this;
|
||||
++(*this);
|
||||
return tmp;
|
||||
}
|
||||
iterator operator++(int)
|
||||
{
|
||||
iterator tmp = *this;
|
||||
++(*this);
|
||||
return tmp;
|
||||
}
|
||||
|
||||
iterator operator+(const difference_type n) const
|
||||
{
|
||||
const pointer naive = _ptr + n;
|
||||
const pointer naive = _ptr + n;
|
||||
if (naive < _arr_end)
|
||||
{
|
||||
return iterator(naive, _arr_begin, _arr_end);
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
const pointer remainder = pointer(naive - _arr_end);
|
||||
return iterator(_arr_begin + difference_type(remainder), _arr_begin,
|
||||
_arr_end);
|
||||
return iterator(_arr_begin + difference_type(remainder), _arr_begin, _arr_end);
|
||||
}
|
||||
}
|
||||
|
||||
iterator operator-(const difference_type n) const
|
||||
{
|
||||
const pointer naive = _ptr - n;
|
||||
if (naive >= _arr_begin)
|
||||
const pointer naive = _ptr - n;
|
||||
if (naive >= _arr_begin)
|
||||
{
|
||||
return iterator(naive, _arr_begin, _arr_end);
|
||||
} else
|
||||
return iterator(naive, _arr_begin, _arr_end);
|
||||
}
|
||||
else
|
||||
{
|
||||
const pointer remainder = pointer(_arr_begin - naive);
|
||||
return iterator(_arr_end - difference_type(remainder), _arr_begin,
|
||||
_arr_end);
|
||||
const pointer remainder = pointer(_arr_begin - naive);
|
||||
return iterator(_arr_end - difference_type(remainder), _arr_begin, _arr_end);
|
||||
}
|
||||
}
|
||||
|
||||
friend bool operator==(const iterator& a, const iterator& b)
|
||||
{
|
||||
return a._ptr == b._ptr;
|
||||
}
|
||||
friend bool operator==(const iterator& a, const iterator& b)
|
||||
{
|
||||
return a._ptr == b._ptr;
|
||||
}
|
||||
|
||||
friend bool operator!=(const iterator& a, const iterator& b)
|
||||
{
|
||||
return a._ptr != b._ptr;
|
||||
}
|
||||
friend bool operator!=(const iterator& a, const iterator& b)
|
||||
{
|
||||
return a._ptr != b._ptr;
|
||||
}
|
||||
|
||||
friend difference_type operator-(const iterator& a, const iterator& b)
|
||||
friend difference_type operator-(const iterator& a, const iterator& b)
|
||||
{
|
||||
return a._ptr - b._ptr;
|
||||
return a._ptr - b._ptr;
|
||||
}
|
||||
|
||||
friend difference_type operator+(const iterator& a, const iterator& b)
|
||||
{
|
||||
return a._ptr + b._ptr;
|
||||
return a._ptr + b._ptr;
|
||||
}
|
||||
|
||||
private:
|
||||
pointer _ptr;
|
||||
pointer _arr_begin;
|
||||
pointer _arr_end;
|
||||
};
|
||||
private:
|
||||
pointer _ptr;
|
||||
pointer _arr_begin;
|
||||
pointer _arr_end;
|
||||
};
|
||||
|
||||
Ringbuffer() : _head(&_data[0], &_data[0], &_data[N]), _tail(_head) {}
|
||||
Ringbuffer()
|
||||
: _head(&_data[0], &_data[0], &_data[N])
|
||||
, _tail(_head)
|
||||
{ }
|
||||
|
||||
Ringbuffer(const Ringbuffer&) = delete;
|
||||
Ringbuffer(Ringbuffer&&) noexcept = default;
|
||||
Ringbuffer(const Ringbuffer&) = delete;
|
||||
Ringbuffer(Ringbuffer&&) noexcept = default;
|
||||
|
||||
iterator begin() { return _head; }
|
||||
iterator begin()
|
||||
{
|
||||
return _head;
|
||||
}
|
||||
|
||||
iterator end() { return _tail; }
|
||||
iterator end()
|
||||
{
|
||||
return _tail;
|
||||
}
|
||||
|
||||
void clear() { _head = _tail; }
|
||||
|
||||
void push_back(const T& value)
|
||||
void clear()
|
||||
{
|
||||
if (size() == N)
|
||||
return;
|
||||
|
||||
*_tail = value;
|
||||
++_tail;
|
||||
|
||||
if (_tail == _head)
|
||||
_is_full = true;
|
||||
_head = _tail;
|
||||
}
|
||||
|
||||
template<typename... Args>
|
||||
void emplace_back(Args&&... args)
|
||||
void push_back(const T& value)
|
||||
{
|
||||
if (size() == N)
|
||||
return;
|
||||
|
||||
*_tail = value;
|
||||
++_tail;
|
||||
|
||||
if (_tail == _head)
|
||||
_is_full = true;
|
||||
}
|
||||
|
||||
template<typename... Args>
|
||||
void emplace_back(Args&&... args)
|
||||
{
|
||||
if (size() == N)
|
||||
return;
|
||||
|
||||
new (&*_tail)T(std::forward<Args>(args)...);
|
||||
++_tail;
|
||||
}
|
||||
|
||||
void increment_tail()
|
||||
{
|
||||
if (_tail == _head)
|
||||
++_head;
|
||||
|
||||
++_tail;
|
||||
}
|
||||
|
||||
void pop_front()
|
||||
{
|
||||
if (size() == N)
|
||||
return;
|
||||
|
||||
new (&*_tail) T(std::forward<Args>(args)...);
|
||||
++_tail;
|
||||
}
|
||||
|
||||
void increment_tail()
|
||||
{
|
||||
if (_is_full)
|
||||
++_head;
|
||||
|
||||
++_tail;
|
||||
}
|
||||
|
||||
void pop_front()
|
||||
{
|
||||
if (empty())
|
||||
return;
|
||||
if (empty())
|
||||
return;
|
||||
|
||||
++_head;
|
||||
_is_full = false;
|
||||
}
|
||||
|
||||
reference front() { return *_head; }
|
||||
reference front()
|
||||
{
|
||||
return *_head;
|
||||
}
|
||||
|
||||
const_reference front() const { return *_head; }
|
||||
const_reference front() const
|
||||
{
|
||||
return *_head;
|
||||
}
|
||||
|
||||
reference back()
|
||||
{
|
||||
if (empty())
|
||||
return *_tail;
|
||||
else
|
||||
return *(_tail - 1);
|
||||
return *(_tail - 1);
|
||||
}
|
||||
|
||||
const_reference back() const
|
||||
{
|
||||
if (empty())
|
||||
return *_tail;
|
||||
else
|
||||
return *(_tail - 1);
|
||||
return *(_tail - 1);
|
||||
}
|
||||
|
||||
size_type size() const
|
||||
{
|
||||
if (_head == _tail)
|
||||
if (_head == _tail)
|
||||
{
|
||||
if (_is_full)
|
||||
return N;
|
||||
@ -196,30 +217,37 @@ public:
|
||||
return 0;
|
||||
}
|
||||
|
||||
const typename Ringbuffer<T, N>::iterator::difference_type distance =
|
||||
_tail - _head;
|
||||
const typename Ringbuffer<T, N>::iterator::difference_type distance = _tail - _head;
|
||||
|
||||
if (distance > 0)
|
||||
{
|
||||
return distance;
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
return _head - _tail + 1;
|
||||
}
|
||||
}
|
||||
|
||||
constexpr size_type max_size() const { return N; }
|
||||
constexpr size_type max_size() const
|
||||
{
|
||||
return N;
|
||||
}
|
||||
|
||||
bool empty() const { return size() == 0; }
|
||||
bool empty() const
|
||||
{
|
||||
return size() == 0;
|
||||
}
|
||||
|
||||
private:
|
||||
std::array<T, N> _data;
|
||||
bool _is_full = false;
|
||||
std::array<T, N> _data;
|
||||
bool _is_full = false;
|
||||
|
||||
iterator _head;
|
||||
iterator _tail;
|
||||
iterator _head;
|
||||
iterator _tail;
|
||||
};
|
||||
|
||||
}// namespace Utility
|
||||
}
|
||||
|
||||
|
||||
#endif /* UTILITY_RINGBUFFER_HPP_ */
|
||||
|
||||
@ -24,15 +24,16 @@ public:
|
||||
using serial_interface = T;
|
||||
|
||||
SerialLogger() = delete;
|
||||
explicit SerialLogger(const serial_interface& serial) : _serial(serial) {}
|
||||
explicit SerialLogger(const serial_interface& serial)
|
||||
: _serial(serial)
|
||||
{ }
|
||||
|
||||
void log(const char* format, ...) override
|
||||
{
|
||||
std::va_list args;
|
||||
va_start(args, format);
|
||||
|
||||
const std::int32_t len =
|
||||
vsnprintf(_buffer.data(), _buffer.size(), format, args);
|
||||
const std::int32_t len = vsnprintf(_buffer.data(), _buffer.size(), format, args);
|
||||
|
||||
if (len > 0)
|
||||
_serial.Transmit(reinterpret_cast<std::uint8_t*>(_buffer.data()), len);
|
||||
@ -45,6 +46,7 @@ private:
|
||||
std::array<char, N> _buffer;
|
||||
};
|
||||
|
||||
}// namespace Utility
|
||||
}
|
||||
|
||||
|
||||
#endif /* UTILITY_INC_UTILITY_SERIALLOGGER_HPP_ */
|
||||
|
||||
@ -12,8 +12,14 @@ namespace Utility
|
||||
|
||||
ILogger* skullc_logger = nullptr;
|
||||
|
||||
void setLogger(ILogger* log) { skullc_logger = log; }
|
||||
void setLogger(ILogger* log)
|
||||
{
|
||||
skullc_logger = log;
|
||||
}
|
||||
|
||||
void setLogger(ILogger& log) { skullc_logger = &log; }
|
||||
void setLogger(ILogger& log)
|
||||
{
|
||||
skullc_logger = &log;
|
||||
}
|
||||
|
||||
}// namespace Utility
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user