From 4a4dd1a1fc498f80dc0f808eac5c2b1d1816a491 Mon Sep 17 00:00:00 2001 From: Erki Date: Thu, 1 Apr 2021 14:47:13 +0300 Subject: [PATCH] Refactor peripherals library to rely solely on templates for customizeability with HAL --- Peripherals/Inc/peripherals_config.hpp | 17 -- Peripherals/Inc/peripherals_hal_st.hpp | 224 ++++++++++++++++ Peripherals/Inc/peripherals_imu.hpp | 3 - Peripherals/Inc/peripherals_imu_icm.hpp | 254 ++++++++++++++++-- Peripherals/Inc/peripherals_io.hpp | 38 --- Peripherals/Inc/peripherals_motors.hpp | 90 +++++-- Peripherals/Inc/peripherals_pwm_channel.hpp | 4 +- Peripherals/Inc/peripherals_rgb.hpp | 2 - Peripherals/Inc/peripherals_serial.hpp | 35 --- Peripherals/Inc/peripherals_spi.hpp | 43 --- Peripherals/Inc/peripherals_utility.hpp | 8 - Peripherals/Src/peripherals_imu_icm.cpp | 274 -------------------- Peripherals/Src/peripherals_io.cpp | 44 ---- Peripherals/Src/peripherals_motors.cpp | 94 ------- Peripherals/Src/peripherals_pwm_channel.cpp | 63 ----- Peripherals/Src/peripherals_spi.cpp | 104 -------- Peripherals/Src/peripherals_utility.cpp | 34 --- 17 files changed, 528 insertions(+), 803 deletions(-) delete mode 100644 Peripherals/Inc/peripherals_config.hpp create mode 100644 Peripherals/Inc/peripherals_hal_st.hpp delete mode 100644 Peripherals/Inc/peripherals_io.hpp delete mode 100644 Peripherals/Inc/peripherals_serial.hpp delete mode 100644 Peripherals/Inc/peripherals_spi.hpp delete mode 100644 Peripherals/Src/peripherals_imu_icm.cpp delete mode 100644 Peripherals/Src/peripherals_io.cpp delete mode 100644 Peripherals/Src/peripherals_motors.cpp delete mode 100644 Peripherals/Src/peripherals_pwm_channel.cpp delete mode 100644 Peripherals/Src/peripherals_spi.cpp delete mode 100644 Peripherals/Src/peripherals_utility.cpp diff --git a/Peripherals/Inc/peripherals_config.hpp b/Peripherals/Inc/peripherals_config.hpp deleted file mode 100644 index 87a1b97..0000000 --- a/Peripherals/Inc/peripherals_config.hpp +++ /dev/null @@ -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_ */ diff --git a/Peripherals/Inc/peripherals_hal_st.hpp b/Peripherals/Inc/peripherals_hal_st.hpp new file mode 100644 index 0000000..40080b4 --- /dev/null +++ b/Peripherals/Inc/peripherals_hal_st.hpp @@ -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 + +#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 +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; + +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(®, 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(®, 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(®, 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(®, 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; + +#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_ */ diff --git a/Peripherals/Inc/peripherals_imu.hpp b/Peripherals/Inc/peripherals_imu.hpp index 059891c..b50d204 100644 --- a/Peripherals/Inc/peripherals_imu.hpp +++ b/Peripherals/Inc/peripherals_imu.hpp @@ -31,9 +31,6 @@ public: virtual void ReadAccelerometer(float* 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; }; } diff --git a/Peripherals/Inc/peripherals_imu_icm.hpp b/Peripherals/Inc/peripherals_imu_icm.hpp index 693d36e..a3affb8 100644 --- a/Peripherals/Inc/peripherals_imu_icm.hpp +++ b/Peripherals/Inc/peripherals_imu_icm.hpp @@ -9,18 +9,23 @@ #define PERIPHERALS_IMU_ICM_HPP_ #include +#include #include "spi.h" #include "peripherals_imu.hpp" -#include "peripherals_spi.hpp" +#include "peripherals_utility.hpp" namespace Peripherals { +template class ImuIcm : public IImu { public: + using registers_handle = T; + using hal = HAL; + enum class GyroScale : std::uint32_t { DPS_250 = 0, @@ -37,44 +42,251 @@ public: G16 }; + registers_handle registers; + ImuIcm() = delete; - ImuIcm(SPI_HandleTypeDef* spi, const IO& cs); - ImuIcm(const Spi& spi); + ImuIcm(const registers_handle& registers) + : 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); - void SetSpiTxCallback(pSPI_CallbackTypeDef cb); + registers.WriteRegister(_Registers::USER_CTRL & _Registers::WRITE_MASK, 0b00010000); + hal::Delay(10); -#endif + registers.WriteRegister(_Registers::PWR_MGMT_1 & _Registers::WRITE_MASK, 0b00000000); + hal::Delay(10); - void Setup() override; - void Calibrate(const std::uint32_t samples) override; + registers.WriteRegister(_Registers::PWR_MGMT_1 & _Registers::WRITE_MASK, 0b00000001); + hal::Delay(10); - void SetGyroscopeScale(const GyroScale scale); - void SetAccelerometerScale(const AccelerometerScale scale); + 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); - void ReadGyro(float* output) override; - void ReadGyroRaw(std::int16_t* output) override; + SetGyroscopeScale(_scale_gyro); - void ReadAccelerometer(float* output) override; - void ReadAccelerometerRaw(std::int16_t* output) override; + SetAccelerometerScale(_scale_accel); - std::int16_t AccelerometerReadingToRaw(const float& fs) const; - std::int16_t GyroReadingToRaw(const float& fs) const; - float AccelerometerRawToReading(const std::int16_t bit) const; - float GyroRawToReading(const std::int16_t bit) const; + // 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); + + // 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 avg_gyro; + std::array avg_accel; + + for (std::uint32_t i = 0; i < samples; i++) + { + std::array raw; + auto add_to_avg = [&raw](std::array& 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::max(); + const std::int32_t min = std::numeric_limits::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(&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(&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(&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(&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: - Spi _spi; - GyroScale _scale_gyro = GyroScale::DPS_2000; AccelerometerScale _scale_accel = AccelerometerScale::G16; std::array _bias_gyro; std::array _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 +constexpr float ImuIcm::_accel_fs_to_bit_constants[4]; + +template +constexpr float ImuIcm::_gyro_fs_to_bit_constants[4]; + } #endif /* PERIPHERALS_IMU_ICM_HPP_ */ diff --git a/Peripherals/Inc/peripherals_io.hpp b/Peripherals/Inc/peripherals_io.hpp deleted file mode 100644 index de285f3..0000000 --- a/Peripherals/Inc/peripherals_io.hpp +++ /dev/null @@ -1,38 +0,0 @@ -/* - * IO.h - * - * Created on: Feb 24, 2021 - * Author: erki - */ - -#ifndef PERIPHERALS_IO_HPP_ -#define PERIPHERALS_IO_HPP_ - -#include -#include - -#include - -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_ */ diff --git a/Peripherals/Inc/peripherals_motors.hpp b/Peripherals/Inc/peripherals_motors.hpp index 7646a59..4a215e4 100644 --- a/Peripherals/Inc/peripherals_motors.hpp +++ b/Peripherals/Inc/peripherals_motors.hpp @@ -8,44 +8,92 @@ #ifndef PERIPHERALS_MOTORS_HPP_ #define PERIPHERALS_MOTORS_HPP_ -#include "peripherals_io.hpp" - -#include - -#include "peripherals_pwm_channel.hpp" - namespace Peripherals { +template +struct TwoChannelMotorData +{ + using pwm_channel = T; + pwm_channel forward; + pwm_channel backward; +}; + class IMotors { public: - struct TwoChannelMotorData - { - PwmChannel forward; - PwmChannel backward; - }; - virtual void Set(const std::int16_t left, const std::int16_t right) = 0; virtual void Coast() = 0; virtual void Break() = 0; - virtual void Unbreak() = 0; }; +template class DualDrvMotors : public IMotors { public: - DualDrvMotors(const IMotors::TwoChannelMotorData& left, const IMotors::TwoChannelMotorData& right, const IO& sleep_pin); + using single_motor = TwoChannelMotorData; + using gpio = I; - virtual void Set(const std::int16_t left, const std::int16_t right) override; - virtual void Coast() override; - virtual void Break() override; - virtual void Unbreak() override; + 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(); + + 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: - IMotors::TwoChannelMotorData _left; - IMotors::TwoChannelMotorData _right; - IO _sleep_pin; + single_motor _left; + single_motor _right; + gpio _sleep_pin; }; } diff --git a/Peripherals/Inc/peripherals_pwm_channel.hpp b/Peripherals/Inc/peripherals_pwm_channel.hpp index 48da844..7ec7a19 100644 --- a/Peripherals/Inc/peripherals_pwm_channel.hpp +++ b/Peripherals/Inc/peripherals_pwm_channel.hpp @@ -22,14 +22,14 @@ struct PwmChannel TIM_HandleTypeDef* timer; std::uint32_t channel; std::uint32_t timer_code; - IO pin; + Io pin; PwmChannel() = delete; PwmChannel(TIM_HandleTypeDef* timer, const std::uint32_t channel, const std::uint32_t timer_code, - const IO& pin); + const Io& pin); void PinToPwm(); void PinToGpio(); diff --git a/Peripherals/Inc/peripherals_rgb.hpp b/Peripherals/Inc/peripherals_rgb.hpp index 3546d4c..5f614c2 100644 --- a/Peripherals/Inc/peripherals_rgb.hpp +++ b/Peripherals/Inc/peripherals_rgb.hpp @@ -8,8 +8,6 @@ #ifndef PERIPHERALS_INC_PERIPHERALS_RGB_HPP_ #define PERIPHERALS_INC_PERIPHERALS_RGB_HPP_ -#include "peripherals_io.hpp" - namespace Peripherals { diff --git a/Peripherals/Inc/peripherals_serial.hpp b/Peripherals/Inc/peripherals_serial.hpp deleted file mode 100644 index 30115f4..0000000 --- a/Peripherals/Inc/peripherals_serial.hpp +++ /dev/null @@ -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 - -namespace Peripherals -{ - -template -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_ */ diff --git a/Peripherals/Inc/peripherals_spi.hpp b/Peripherals/Inc/peripherals_spi.hpp deleted file mode 100644 index bc63f50..0000000 --- a/Peripherals/Inc/peripherals_spi.hpp +++ /dev/null @@ -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_ */ diff --git a/Peripherals/Inc/peripherals_utility.hpp b/Peripherals/Inc/peripherals_utility.hpp index 06a9e2e..e8aa3b8 100644 --- a/Peripherals/Inc/peripherals_utility.hpp +++ b/Peripherals/Inc/peripherals_utility.hpp @@ -10,17 +10,9 @@ #include -#include "peripherals_config.hpp" - namespace Peripherals { -void Initialize(); - -#ifdef PERIPHERALS_USE_DELAY_US -void DelayUs(const std::uint32_t micros); -#endif - template constexpr const T& Clamp(const T& v, const T& lo, const T& hi) { diff --git a/Peripherals/Src/peripherals_imu_icm.cpp b/Peripherals/Src/peripherals_imu_icm.cpp deleted file mode 100644 index aac7fb2..0000000 --- a/Peripherals/Src/peripherals_imu_icm.cpp +++ /dev/null @@ -1,274 +0,0 @@ -/* - * peripherals_imu_icm.cpp - * - * Created on: Mar 5, 2021 - * Author: erki - */ - -#include "peripherals_imu_icm.hpp" - -#include - -#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 avg_gyro; - std::array avg_accel; - - for (std::uint32_t i = 0; i < samples; i++) - { - std::array raw; - auto add_to_avg = [&raw](std::array& 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::max(); - const std::int32_t min = std::numeric_limits::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(&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(&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(&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(&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)]; -} - -} diff --git a/Peripherals/Src/peripherals_io.cpp b/Peripherals/Src/peripherals_io.cpp deleted file mode 100644 index aa84e0d..0000000 --- a/Peripherals/Src/peripherals_io.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/* - * IO.cpp - * - * Created on: Feb 24, 2021 - * Author: erki - */ - -#include - -#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 */ diff --git a/Peripherals/Src/peripherals_motors.cpp b/Peripherals/Src/peripherals_motors.cpp deleted file mode 100644 index e0b751c..0000000 --- a/Peripherals/Src/peripherals_motors.cpp +++ /dev/null @@ -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(); -} - -} diff --git a/Peripherals/Src/peripherals_pwm_channel.cpp b/Peripherals/Src/peripherals_pwm_channel.cpp deleted file mode 100644 index ddd4f41..0000000 --- a/Peripherals/Src/peripherals_pwm_channel.cpp +++ /dev/null @@ -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); -} - -} diff --git a/Peripherals/Src/peripherals_spi.cpp b/Peripherals/Src/peripherals_spi.cpp deleted file mode 100644 index 2d6c584..0000000 --- a/Peripherals/Src/peripherals_spi.cpp +++ /dev/null @@ -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, ®, 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, ®, 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, ®, 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, ®, 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); -} - -} diff --git a/Peripherals/Src/peripherals_utility.cpp b/Peripherals/Src/peripherals_utility.cpp deleted file mode 100644 index be2156b..0000000 --- a/Peripherals/Src/peripherals_utility.cpp +++ /dev/null @@ -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 - -}