erki/skullc-peripherals
1
0
Fork 0
Code Issues Pull Requests Actions Projects Releases Wiki Activity

Refactor peripherals library to rely solely on templates for customizeability with HAL

Browse Source
This commit is contained in:
Erki 2021-04-01 14:47:13 +03:00
parent 781e4ba0f5
commit 4a4dd1a1fc
17 changed files with 528 additions and 803 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
Peripherals/Inc/peripherals_config.hpp
View File

@ -1,17 +0,0 @@
/*
* peripherals_config.h
*
* Created on: Feb 24, 2021
* Author: erki
*/
#ifndef PERIPHERALS_CONFIG_HPP_
#define PERIPHERALS_CONFIG_HPP_
#define PERIPHERALS_USE_DELAY_US
#ifndef PERIPHERALS_USE_DELAY_US
# pragma info "Compiled without usecond delay."
#endif
#endif /* PERIPHERALS_CONFIG_HPP_ */

224
Peripherals/Inc/peripherals_hal_st.hpp Normal file
View File

@ -0,0 +1,224 @@
/*
* peripherals_hal_st.hpp
*
* Created on: Mar 30, 2021
* Author: erki
*/
#ifndef PERIPHERALS_INC_PERIPHERALS_HAL_ST_HPP_
#define PERIPHERALS_INC_PERIPHERALS_HAL_ST_HPP_
#include <main.h>
#include "peripherals_utility.hpp"
#define USE_DELAY_US
namespace Peripherals
{
namespace Hal
{
namespace St
{
struct StaticHal
{
static void Initialize()
{
#ifdef USE_DELAY_US
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
DWT->CYCCNT = 0;
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
#endif
}
static void Delay(const std::uint32_t milliseconds)
{
HAL_Delay(milliseconds);
}
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);
while (DWT->CYCCNT - tick_start < ticks_delay);
#else
(void)micros;
#endif
}
};
#ifdef HAL_GPIO_MODULE_ENABLED
struct Gpio
{
GPIO_TypeDef* port;
std::uint16_t pin;
Gpio() = delete;
explicit Gpio(GPIO_TypeDef* port, const std::uint16_t 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);
}
bool Read() const
{
return HAL_GPIO_ReadPin(port, pin);
}
};
#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)>
struct SerialInterface
{
using underlying_handle_type = T;
underlying_handle_type* handle;
SerialInterface() = delete;
explicit SerialInterface(underlying_handle_type* handle)
: handle(handle)
{ }
bool Transmit(std::uint8_t* data, const std::uint32_t data_len)
{
return transmit(handle, data, data_len, 100) == HAL_StatusTypeDef::HAL_OK;
}
bool Receive(std::uint8_t* data, const std::uint32_t data_len)
{
return transmit(handle, data, data_len, 100) == HAL_StatusTypeDef::HAL_OK;
}
};
#ifdef HAL_SPI_MODULE_ENABLED
using SpiInterface = SerialInterface<SPI_HandleTypeDef, HAL_SPI_Transmit, HAL_SPI_Receive>;
struct SpiRegisters
{
SpiInterface handle;
Gpio chip_select;
SpiRegisters() = delete;
explicit SpiRegisters(const SpiInterface& handle, const Gpio& chip_select)
: handle(handle)
, chip_select(chip_select)
{ }
void WriteRegister(std::uint8_t reg, uint8_t data)
{
chip_select.Set(false);
handle.Transmit(&reg, 1);
handle.Transmit(&data, 1);
chip_select.Set(true);
}
void WriteRegisterMultibyte(std::uint8_t reg, std::uint8_t* data, const std::uint32_t len)
{
chip_select.Set(false);
handle.Transmit(&reg, 1);
handle.Transmit(data, len);
chip_select.Set(true);
}
std::uint8_t ReadRegister(std::uint8_t reg, const std::uint32_t read_delay = 0)
{
chip_select.Set(false);
handle.Transmit(&reg, 1);
std::uint8_t output = 255;
if (read_delay)
StaticHal::DelayUs(read_delay);
handle.Receive(&output, 1);
chip_select.Set(true);
return output;
}
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);
handle.Transmit(&reg, 1);
if (read_delay)
StaticHal::DelayUs(read_delay);
handle.Receive(data, len);
chip_select.Set(true);
}
};
#endif // HAL_SPI_MODULE_ENABLED
#ifdef HAL_UART_MODULE_ENABLED
using UartInterface = SerialInterface<UART_HandleTypeDef, HAL_UART_Transmit, HAL_UART_Receive>;
#endif // HAL_UART_MODULE_ENABLED
#ifdef HAL_TIM_MODULE_ENABLED
struct PwmChannel
{
TIM_HandleTypeDef* handle;
std::uint32_t channel;
PwmChannel() = delete;
explicit PwmChannel(TIM_HandleTypeDef* handle, const std::uint32_t channel)
: handle(handle)
, channel(channel)
{ }
void Enable()
{
HAL_TIM_PWM_Start(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;
}
};
#endif // HAL_TIM_MODULE_ENABLED
}
}
}
#endif /* PERIPHERALS_INC_PERIPHERALS_HAL_ST_HPP_ */

3
Peripherals/Inc/peripherals_imu.hpp
View File

@ -31,9 +31,6 @@ public:
virtual void ReadAccelerometer(float* output) = 0; virtual void ReadAccelerometer(float* output) = 0;
virtual void ReadAccelerometerRaw(std::int16_t* output) = 0; virtual void ReadAccelerometerRaw(std::int16_t* output) = 0;
virtual float ProcessGyroReading(const std::int16_t raw_reading) = 0;
virtual float ProcessAccelerometerReading(const std::int16_t raw_reading) = 0;
}; };
} }

254
Peripherals/Inc/peripherals_imu_icm.hpp
View File

@ -9,18 +9,23 @@
#define PERIPHERALS_IMU_ICM_HPP_ #define PERIPHERALS_IMU_ICM_HPP_
#include <array> #include <array>
#include <limits>
#include "spi.h" #include "spi.h"
#include "peripherals_imu.hpp" #include "peripherals_imu.hpp"
#include "peripherals_spi.hpp" #include "peripherals_utility.hpp"
namespace Peripherals namespace Peripherals
{ {
template<typename T, typename HAL>
class ImuIcm : public IImu class ImuIcm : public IImu
{ {
public: public:
using registers_handle = T;
using hal = HAL;
enum class GyroScale : std::uint32_t enum class GyroScale : std::uint32_t
{ {
DPS_250 = 0, DPS_250 = 0,
@ -37,44 +42,251 @@ public:
G16 G16
}; };
registers_handle registers;
ImuIcm() = delete; ImuIcm() = delete;
ImuIcm(SPI_HandleTypeDef* spi, const IO& cs); ImuIcm(const registers_handle& registers)
ImuIcm(const Spi& spi); : registers(registers)
{ }
#if USE_HAL_SPI_REGISTER_CALLBACKS == 1U void Setup() override
{
registers.WriteRegister(_Registers::PWR_MGMT_1 & _Registers::WRITE_MASK, 0b10000000);
hal::Delay(10);
void SetSpiRxCallback(pSPI_CallbackTypeDef cb); registers.WriteRegister(_Registers::USER_CTRL & _Registers::WRITE_MASK, 0b00010000);
void SetSpiTxCallback(pSPI_CallbackTypeDef cb); hal::Delay(10);
#endif registers.WriteRegister(_Registers::PWR_MGMT_1 & _Registers::WRITE_MASK, 0b00000000);
hal::Delay(10);
void Setup() override; registers.WriteRegister(_Registers::PWR_MGMT_1 & _Registers::WRITE_MASK, 0b00000001);
void Calibrate(const std::uint32_t samples) override; hal::Delay(10);
void SetGyroscopeScale(const GyroScale scale); registers.WriteRegister(_Registers::CONFIG & _Registers::WRITE_MASK, 0x03); // DLPF_CFG = 3, gyro filter = 41/59.0, gyro rate = 1KHz, temp filter = 42
void SetAccelerometerScale(const AccelerometerScale scale); hal::Delay(10);
void ReadGyro(float* output) override; SetGyroscopeScale(_scale_gyro);
void ReadGyroRaw(std::int16_t* output) override;
void ReadAccelerometer(float* output) override; SetAccelerometerScale(_scale_accel);
void ReadAccelerometerRaw(std::int16_t* output) override;
std::int16_t AccelerometerReadingToRaw(const float& fs) const; // ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz
std::int16_t GyroReadingToRaw(const float& fs) const; registers.WriteRegister(_Registers::ACCEL_CONFIG2 & _Registers::WRITE_MASK, 0x03);
float AccelerometerRawToReading(const std::int16_t bit) const; hal::Delay(10);
float GyroRawToReading(const std::int16_t bit) const;
// 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
// 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);
}
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];
};
ReadGyroRaw(raw.data());
add_to_avg(avg_gyro);
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();
_bias_gyro[i] = Clamp(avg_gyro[i], max, min);
_bias_accel[i] = Clamp(avg_accel[i], max, min);
}
_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);
_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);
_scale_accel = scale;
}
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);
}
}
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]);
}
}
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);
}
}
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]);
}
}
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)];
}
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)];
}
private: private:
Spi _spi;
GyroScale _scale_gyro = GyroScale::DPS_2000; GyroScale _scale_gyro = GyroScale::DPS_2000;
AccelerometerScale _scale_accel = AccelerometerScale::G16; AccelerometerScale _scale_accel = AccelerometerScale::G16;
std::array<std::int16_t, 3> _bias_gyro; std::array<std::int16_t, 3> _bias_gyro;
std::array<std::int16_t, 3> _bias_accel; 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 _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;
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 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 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 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;
};
}; };
template<typename T, typename HAL>
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];
} }
#endif /* PERIPHERALS_IMU_ICM_HPP_ */ #endif /* PERIPHERALS_IMU_ICM_HPP_ */

38
Peripherals/Inc/peripherals_io.hpp
View File

@ -1,38 +0,0 @@
/*
* IO.h
*
* Created on: Feb 24, 2021
* Author: erki
*/
#ifndef PERIPHERALS_IO_HPP_
#define PERIPHERALS_IO_HPP_
#include <peripherals_config.hpp>
#include <cstdint>
#include <gpio.h>
namespace Peripherals
{
struct IO
{
GPIO_TypeDef* port;
std::uint16_t pin;
IO() = delete;
IO(GPIO_TypeDef* port, const unsigned short pin);
void Set(const bool state);
void Toggle();
bool Read();
#ifdef PERIPHERALS_USE_DELAY_US
void SetDelayUs(const bool state, const std::uint32_t micros);
#endif
};
} /* namespace Peripherals */
#endif /* PERIPHERALS_IO_HPP_ */

90
Peripherals/Inc/peripherals_motors.hpp
View File

@ -8,44 +8,92 @@
#ifndef PERIPHERALS_MOTORS_HPP_ #ifndef PERIPHERALS_MOTORS_HPP_
#define PERIPHERALS_MOTORS_HPP_ #define PERIPHERALS_MOTORS_HPP_
#include "peripherals_io.hpp"
#include <tim.h>
#include "peripherals_pwm_channel.hpp"
namespace Peripherals namespace Peripherals
{ {
template<typename T>
struct TwoChannelMotorData
{
using pwm_channel = T;
pwm_channel forward;
pwm_channel backward;
};
class IMotors class IMotors
{ {
public: public:
struct TwoChannelMotorData
{
PwmChannel forward;
PwmChannel backward;
};
virtual void Set(const std::int16_t left, const std::int16_t right) = 0; virtual void Set(const std::int16_t left, const std::int16_t right) = 0;
virtual void Coast() = 0; virtual void Coast() = 0;
virtual void Break() = 0; virtual void Break() = 0;
virtual void Unbreak() = 0;
}; };
template<typename T, typename I>
class DualDrvMotors : public IMotors class DualDrvMotors : public IMotors
{ {
public: public:
DualDrvMotors(const IMotors::TwoChannelMotorData& left, const IMotors::TwoChannelMotorData& right, const IO& sleep_pin); using single_motor = TwoChannelMotorData<T>;
using gpio = I;
virtual void Set(const std::int16_t left, const std::int16_t right) override; DualDrvMotors(const single_motor& left, const single_motor& right, const gpio& sleep_pin)
virtual void Coast() override; : _left(left)
virtual void Break() override; , _right(right)
virtual void Unbreak() override; , _sleep_pin(sleep_pin)
{
_left.forward.Enable();
_left.backward.Enable();
_right.forward.Enable();
_right.backward.Enable();
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);
}
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);
_right.forward.SetCompare(0);
_right.backward.SetCompare(0);
}
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: private:
IMotors::TwoChannelMotorData _left; single_motor _left;
IMotors::TwoChannelMotorData _right; single_motor _right;
IO _sleep_pin; gpio _sleep_pin;
}; };
} }

4
Peripherals/Inc/peripherals_pwm_channel.hpp
View File

@ -22,14 +22,14 @@ struct PwmChannel
TIM_HandleTypeDef* timer; TIM_HandleTypeDef* timer;
std::uint32_t channel; std::uint32_t channel;
std::uint32_t timer_code; std::uint32_t timer_code;
IO pin; Io pin;
PwmChannel() = delete; PwmChannel() = delete;
PwmChannel(TIM_HandleTypeDef* timer, PwmChannel(TIM_HandleTypeDef* timer,
const std::uint32_t channel, const std::uint32_t channel,
const std::uint32_t timer_code, const std::uint32_t timer_code,
const IO& pin); const Io& pin);
void PinToPwm(); void PinToPwm();
void PinToGpio(); void PinToGpio();

2
Peripherals/Inc/peripherals_rgb.hpp
View File

@ -8,8 +8,6 @@
#ifndef PERIPHERALS_INC_PERIPHERALS_RGB_HPP_ #ifndef PERIPHERALS_INC_PERIPHERALS_RGB_HPP_
#define PERIPHERALS_INC_PERIPHERALS_RGB_HPP_ #define PERIPHERALS_INC_PERIPHERALS_RGB_HPP_
#include "peripherals_io.hpp"
namespace Peripherals namespace Peripherals
{ {

35
Peripherals/Inc/peripherals_serial.hpp
View File

@ -1,35 +0,0 @@
/*
* peripherals_serial.hpp
*
* Created on: Mar 22, 2021
* Author: erki
*/
#ifndef SKULLC_PERIPHERALS_SERIAL_HPP_
#define SKULLC_PERIPHERALS_SERIAL_HPP_
#include <cstdint>
namespace Peripherals
{
template<typename HT, HT* Handle,
void (*tx_function)(HT*, char*, std::uint32_t),
void (*rx_function)(HT*, char*, std::uint32_t)>
struct SerialPeripheral
{
void transmit(char* data, const std::uint32_t data_length)
{
tx_function(handle, data, data_length);
}
void receive(char* output, const std::uint32_t data_length)
{
rx_function(handle, data, data_length);
}
};
}
#endif /* SKULLC_PERIPHERALS_SERIAL_HPP_ */

43
Peripherals/Inc/peripherals_spi.hpp
View File

@ -1,43 +0,0 @@
/*
* peripherals_spi.hpp
*
* Created on: Mar 5, 2021
* Author: erki
*/
#ifndef INC_PERIPHERALS_SPI_HPP_
#define INC_PERIPHERALS_SPI_HPP_
#include "spi.h"
#include "peripherals_config.hpp"
#include "peripherals_io.hpp"
namespace Peripherals
{
struct Spi
{
SPI_HandleTypeDef* hspi;
IO cs;
std::uint32_t timeout = 10;
#ifdef PERIPHERALS_USE_DELAY_US
std::uint32_t cs_set_delay = 0;
#endif
Spi() = delete;
Spi(SPI_HandleTypeDef* hspi, const IO& cs);
void WriteRegister(std::uint8_t reg, uint8_t data);
void WriteRegisterMultibyte(std::uint8_t reg, std::uint8_t* data, const std::uint32_t len);
std::uint8_t ReadRegister(std::uint8_t reg, 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);
};
}
#endif /* INC_PERIPHERALS_SPI_HPP_ */

8
Peripherals/Inc/peripherals_utility.hpp
View File

@ -10,17 +10,9 @@
#include <cstdint> #include <cstdint>
#include "peripherals_config.hpp"
namespace Peripherals namespace Peripherals
{ {
void Initialize();
#ifdef PERIPHERALS_USE_DELAY_US
void DelayUs(const std::uint32_t micros);
#endif
template<typename T> template<typename T>
constexpr const T& Clamp(const T& v, const T& lo, const T& hi) constexpr const T& Clamp(const T& v, const T& lo, const T& hi)
{ {

274
Peripherals/Src/peripherals_imu_icm.cpp
View File

@ -1,274 +0,0 @@
/*
* peripherals_imu_icm.cpp
*
* Created on: Mar 5, 2021
* Author: erki
*/
#include "peripherals_imu_icm.hpp"
#include <limits>
#include "peripherals_utility.hpp"
namespace
{
namespace _REGS
{
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 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 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 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 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 const float _accel_fs_to_bit_constants[] = {
(2.0f / 32768.0f),
(4.0f / 32768.0f),
(8.0f / 32768.0f),
(16.0f / 32768.0f)
};
static const float _gyro_fs_to_bit_constants[] = {
(250.0f / 32768.0f),
(500.0f / 32768.0f),
(1000.0f / 32768.0f),
(2000.0f / 32768.0f)
};
}
namespace Peripherals
{
ImuIcm::ImuIcm(SPI_HandleTypeDef* spi, const IO& cs)
: _spi(spi, cs)
{ }
ImuIcm::ImuIcm(const Spi& spi)
: _spi(spi)
{ }
#if USE_HAL_SPI_REGISTER_CALLBACKS == 1U
void ImuIcm::SetSpiRxCallback(pSPI_CallbackTypeDef cb)
{
_spi.hspi->RxCpltCallback = cb;
}
void ImuIcm::SetSpiTxCallback(pSPI_CallbackTypeDef cb)
{
_spi.hspi->TxCpltCallback = cb;
}
#endif
void ImuIcm::Setup()
{
_spi.cs.Set(true);
HAL_Delay(100);
_spi.WriteRegister(_REGS::PWR_MGMT_1 & _REGS::WRITE_MASK, 0b10000000);
HAL_Delay(10);
_spi.WriteRegister(_REGS::USER_CTRL & _REGS::WRITE_MASK, 0b00010000);
HAL_Delay(10);
_spi.WriteRegister(_REGS::PWR_MGMT_1 & _REGS::WRITE_MASK, 0b00000000);
HAL_Delay(10);
_spi.WriteRegister(_REGS::PWR_MGMT_1 & _REGS::WRITE_MASK, 0b00000001);
HAL_Delay(20);
_spi.WriteRegister(_REGS::CONFIG & _REGS::WRITE_MASK, 0x03); // DLPF_CFG = 3, gyro filter = 41/59.0, gyro rate = 1KHz, temp filter = 42
HAL_Delay(10);
SetGyroscopeScale(_scale_gyro);
SetAccelerometerScale(_scale_accel);
// ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz
_spi.WriteRegister(_REGS::ACCEL_CONFIG2 & _REGS::WRITE_MASK, 0x03);
HAL_Delay(10);
// SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV) Where INTERNAL_SAMPLE_RATE = 1kHz
_spi.WriteRegister(_REGS::SMPLRT_DIV & _REGS::WRITE_MASK, 0);
HAL_Delay(10);
// 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
_spi.WriteRegister(_REGS::INT_PIN_CFG & _REGS::WRITE_MASK, 0);
HAL_Delay(10);
_spi.WriteRegister(_REGS::INT_ENABLE & _REGS::WRITE_MASK, 1);
HAL_Delay(10);
}
void ImuIcm::Calibrate(const std::uint32_t samples)
{
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];
};
ReadGyroRaw(raw.data());
add_to_avg(avg_gyro);
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();
_bias_gyro[i] = Clamp(avg_gyro[i], max, min);
_bias_accel[i] = Clamp(avg_accel[i], max, min);
}
_bias_accel[2] -= AccelerometerReadingToRaw(1);
}
void ImuIcm::SetGyroscopeScale(const ImuIcm::GyroScale scale)
{
const std::uint8_t current_config = _spi.ReadRegister(_REGS::GYRO_CONFIG | _REGS::READ_MASK);
const std::uint8_t new_config = (current_config & 0xE7) | (std::uint8_t(scale) << 3);
_spi.WriteRegister(_REGS::GYRO_CONFIG & _REGS::WRITE_MASK, new_config);
_scale_gyro = scale;
}
void ImuIcm::SetAccelerometerScale(const ImuIcm::AccelerometerScale scale)
{
const std::uint8_t current_config = _spi.ReadRegister(_REGS::ACCEL_CONFIG | _REGS::READ_MASK);
const std::uint8_t new_config = (current_config & 0xE7) | (std::uint8_t(scale) << 3);
_spi.WriteRegister(_REGS::ACCEL_CONFIG & _REGS::WRITE_MASK, new_config);
_scale_accel = scale;
}
void ImuIcm::ReadGyro(float* output)
{
uint8_t data[6] = { 0 };
_spi.ReadRegisterMultibyte(_REGS::GYRO_XOUT_H | _REGS::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);
}
}
void ImuIcm::ReadGyroRaw(std::int16_t* output)
{
uint8_t data[6] = { 0 };
_spi.ReadRegisterMultibyte(_REGS::GYRO_XOUT_H | _REGS::READ_MASK, data, 6);
for (std::uint32_t i = 0; i < 3; i++)
{
output[i] = ByteToTypeBE<std::int16_t, 2>(&data[i * 2]);
}
}
void ImuIcm::ReadAccelerometer(float* output)
{
uint8_t data[6] = { 0 };
_spi.ReadRegisterMultibyte(_REGS::ACCEL_XOUT_H | _REGS::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);
}
}
void ImuIcm::ReadAccelerometerRaw(std::int16_t* output)
{
uint8_t data[6] = { 0 };
_spi.ReadRegisterMultibyte(_REGS::ACCEL_XOUT_H | _REGS::READ_MASK, data, 6);
for (std::uint32_t i = 0; i < 3; i++)
{
output[i] = ByteToTypeBE<std::int16_t, 2>(&data[i * 2]);
}
}
std::int16_t ImuIcm::AccelerometerReadingToRaw(const float& fs) const
{
return fs / _accel_fs_to_bit_constants[std::uint32_t(_scale_accel)];
}
std::int16_t ImuIcm::GyroReadingToRaw(const float& fs) const
{
return fs / _gyro_fs_to_bit_constants[std::uint32_t(_scale_gyro)];
}
float ImuIcm::AccelerometerRawToReading(const std::int16_t bit) const
{
return float(bit) * _accel_fs_to_bit_constants[std::uint32_t(_scale_accel)];
}
float ImuIcm::GyroRawToReading(const std::int16_t bit) const
{
return float(bit) * _gyro_fs_to_bit_constants[std::uint32_t(_scale_gyro)];
}
}

44
Peripherals/Src/peripherals_io.cpp
View File

@ -1,44 +0,0 @@
/*
* IO.cpp
*
* Created on: Feb 24, 2021
* Author: erki
*/
#include <peripherals_io.hpp>
#include "peripherals_utility.hpp"
namespace Peripherals
{
IO::IO(GPIO_TypeDef* port, const unsigned short pin)
: port(port)
, pin(std::uint16_t(pin))
{ }
void IO::Set(const bool state)
{
HAL_GPIO_WritePin(port, pin, GPIO_PinState(state));
}
void IO::Toggle()
{
HAL_GPIO_TogglePin(port, pin);
}
bool IO::Read()
{
return bool(HAL_GPIO_ReadPin(port, pin));
}
#ifdef PERIPHERALS_USE_DELAY_US
void IO::SetDelayUs(const bool state, const std::uint32_t micros)
{
HAL_GPIO_WritePin(port, pin, GPIO_PinState(state));
DelayUs(micros);
}
#endif
} /* namespace Peripherals */

94
Peripherals/Src/peripherals_motors.cpp
View File

@ -1,94 +0,0 @@
/*
* peripherals_motors.cpp
*
* Created on: Feb 24, 2021
* Author: erki
*/
#include "peripherals_motors.hpp"
namespace Peripherals
{
DualDrvMotors::DualDrvMotors(const IMotors::TwoChannelMotorData& left,
const IMotors::TwoChannelMotorData& right, const IO& sleep_pin)
: _left(left)
, _right(right)
, _sleep_pin(sleep_pin)
{
_left.forward.Enable();
_left.backward.Enable();
_right.forward.Enable();
_right.backward.Enable();
Set(0, 0);
}
void DualDrvMotors::Set(const std::int16_t left, const std::int16_t right)
{
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);
}
}
void DualDrvMotors::Coast()
{
_left.forward.SetCompare(0);
_left.backward.SetCompare(0);
_right.forward.SetCompare(0);
_right.backward.SetCompare(0);
}
void DualDrvMotors::Break()
{
Set(0, 0);
_left.forward.Disable();
_left.backward.Disable();
_right.forward.Disable();
_right.backward.Disable();
_left.forward.PinToGpio();
_left.backward.PinToGpio();
_right.forward.PinToGpio();
_right.backward.PinToGpio();
_left.forward.pin.Set(true);
_left.backward.pin.Set(true);
_right.forward.pin.Set(true);
_right.backward.pin.Set(true);
}
void DualDrvMotors::Unbreak()
{
_left.forward.PinToPwm();
_left.backward.PinToPwm();
_right.forward.PinToPwm();
_right.backward.PinToPwm();
_left.forward.Enable();
_left.backward.Enable();
_right.forward.Enable();
_right.backward.Enable();
}
}

63
Peripherals/Src/peripherals_pwm_channel.cpp
View File

@ -1,63 +0,0 @@
/*
* peripherals_pwm_channel.cpp
*
* Created on: Feb 24, 2021
* Author: erki
*/
#include "peripherals_pwm_channel.hpp"
namespace Peripherals
{
PwmChannel::PwmChannel(TIM_HandleTypeDef* timer,
const std::uint32_t channel,
const std::uint32_t timer_code,
const IO& pin)
: timer(timer)
, channel(channel)
, timer_code(timer_code)
, pin(pin)
{ }
void PwmChannel::PinToPwm()
{
GPIO_InitTypeDef gpio_init = { 0 };
gpio_init.Pin = pin.pin;
gpio_init.Mode = GPIO_MODE_AF_PP;
gpio_init.Pull = GPIO_NOPULL;
gpio_init.Speed = GPIO_SPEED_FREQ_LOW;
gpio_init.Alternate = timer_code;
HAL_GPIO_Init(pin.port, &gpio_init);
}
void PwmChannel::PinToGpio()
{
GPIO_InitTypeDef gpio_config = { 0 };
gpio_config.Pin = pin.pin;
gpio_config.Mode = GPIO_MODE_OUTPUT_PP;
gpio_config.Pull = GPIO_NOPULL;
gpio_config.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(pin.port, &gpio_config);
}
void PwmChannel::Enable()
{
HAL_TIM_PWM_Start(timer, channel);
}
void PwmChannel::Disable()
{
HAL_TIM_PWM_Stop(timer, channel);
}
void PwmChannel::SetCompare(const std::uint32_t compare)
{
__HAL_TIM_SET_COMPARE(timer, channel, compare);
}
}

104
Peripherals/Src/peripherals_spi.cpp
View File

@ -1,104 +0,0 @@
/*
* peripherals_spi.cpp
*
* Created on: Mar 5, 2021
* Author: erki
*/
#include "peripherals_spi.hpp"
#include "peripherals_utility.hpp"
#ifdef PERIPHERALS_USE_DELAY_US
# define CS_SET() do { cs.SetDelayUs(false, cs_set_delay); } while (0)
#else
# define CS_SET() do { cs.Set(false); } while (0)
#endif
#define CS_UNSET() do { cs.Set(true); } while (0)
namespace
{
struct _CsGuard
{
_CsGuard(Peripherals::Spi& spi)
: _spi(spi)
{
#ifdef PERIPHERALS_USE_DELAY_US
_spi.cs.SetDelayUs(false, _spi.cs_set_delay);
#else
_spi.cs.Set(false);
#endif
}
~_CsGuard()
{
_spi.cs.Set(true);
}
private:
Peripherals::Spi& _spi;
};
}
namespace Peripherals
{
Spi::Spi(SPI_HandleTypeDef* hspi, const IO& cs)
: hspi(hspi)
, cs(cs)
{ }
void Spi::WriteRegister(std::uint8_t reg, uint8_t data)
{
_CsGuard guard(*this);
HAL_SPI_Transmit(hspi, &reg, 1, timeout);
HAL_SPI_Transmit(hspi, &data, 1, timeout);
}
void Spi::WriteRegisterMultibyte(std::uint8_t reg, std::uint8_t* data, const std::uint32_t len)
{
_CsGuard guard(*this);
HAL_SPI_Transmit(hspi, &reg, 1, timeout);
HAL_SPI_Transmit(hspi, data, len, timeout);
}
std::uint8_t Spi::ReadRegister(std::uint8_t reg, const std::uint32_t read_delay)
{
_CsGuard guard(*this);
HAL_SPI_Transmit(hspi, &reg, 1, timeout);
#ifdef PERIPHERALS_USE_DELAY_US
if (read_delay)
DelayUs(read_delay);
#else
(void)read_delay;
#endif
std::uint8_t output = 0;
HAL_SPI_Receive(hspi, &output, 1, timeout);
return output;
}
void Spi::ReadRegisterMultibyte(std::uint8_t reg, std::uint8_t* data, const std::uint32_t len, const std::uint32_t read_delay)
{
_CsGuard guard(*this);
HAL_SPI_Transmit(hspi, &reg, 1, timeout);
#ifdef PERIPHERALS_USE_DELAY_US
if (read_delay)
DelayUs(read_delay);
#else
(void)read_delay;
#endif
HAL_SPI_Receive(hspi, data, len, timeout);
}
}

34
Peripherals/Src/peripherals_utility.cpp
View File

@ -1,34 +0,0 @@
/*
* peripherals_utility.cpp
*
* Created on: Feb 24, 2021
* Author: erki
*/
#include "peripherals_utility.hpp"
#include "main.h"
namespace Peripherals
{
void Initialize()
{
#ifdef PERIPHERALS_USE_DELAY_US
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
DWT->CYCCNT = 0;
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
#endif
}
#ifdef PERIPHERALS_USE_DELAY_US
void DelayUs(const std::uint32_t micros)
{
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);
}
#endif
}