From b6f3a898ee0a017ab20492b94e84bf1baaae0559 Mon Sep 17 00:00:00 2001 From: Thomas Sommer Date: Mon, 10 Jun 2024 10:44:33 +0200 Subject: [PATCH 1/2] generalized modm::angle type and renamed some methods --- examples/nucleo_g474re/as5047/main.cpp | 12 ++--- src/modm/driver/encoder/as5047.hpp | 45 +++-------------- src/modm/driver/encoder/as5047.lb | 3 +- src/modm/driver/encoder/as5047_impl.hpp | 8 +-- src/modm/math/geometry/angle_int.hpp | 65 +++++++++++++++++++++++++ 5 files changed, 81 insertions(+), 52 deletions(-) create mode 100644 src/modm/math/geometry/angle_int.hpp diff --git a/examples/nucleo_g474re/as5047/main.cpp b/examples/nucleo_g474re/as5047/main.cpp index d7feb52b31..e56c038f0e 100644 --- a/examples/nucleo_g474re/as5047/main.cpp +++ b/examples/nucleo_g474re/as5047/main.cpp @@ -35,14 +35,14 @@ class EncoderThread : public modm::pt::Protothread while (true) { - PT_CALL(encoder.readout()); + PT_CALL(encoder.read()); MODM_LOG_INFO << "\nNew readout:" << modm::endl; - MODM_LOG_INFO << " angle degree: " << data.getAngleDeg() << " degrees" << modm::endl; - MODM_LOG_INFO << " angle rad: " << data.getAngleRad() << " radians" << modm::endl; - MODM_LOG_INFO << " angle raw: " << data.getAngleRaw() << modm::endl; + MODM_LOG_INFO << " angle degree: " << data.toDegree() << " degrees" << modm::endl; + MODM_LOG_INFO << " angle rad: " << data.toRadian() << " radians" << modm::endl; + MODM_LOG_INFO << " angle raw: " << data.data << modm::endl; - timeout.restart(std::chrono::milliseconds(500)); + timeout.restart(500ms); PT_WAIT_UNTIL(timeout.isExpired()); } @@ -50,7 +50,7 @@ class EncoderThread : public modm::pt::Protothread } private: - modm::as5047::Data data; + modm::as5047::Data data{0}; modm::As5047 encoder; modm::ShortTimeout timeout; diff --git a/src/modm/driver/encoder/as5047.hpp b/src/modm/driver/encoder/as5047.hpp index 0f597a0f01..bb302ccc68 100644 --- a/src/modm/driver/encoder/as5047.hpp +++ b/src/modm/driver/encoder/as5047.hpp @@ -11,15 +11,15 @@ */ // ---------------------------------------------------------------------------- -#ifndef MODM_AS5047_HPP -#define MODM_AS5047_HPP +#pragma once #include +#include #include #include #include #include -#include +#include namespace modm { @@ -81,34 +81,7 @@ struct as5047 }; - struct modm_packed Data - { - /// @return - constexpr float - getAngleRad() const - { - const uint16_t angle = data & 0x3fff; - return static_cast(angle) / 16383.f * 2.f * std::numbers::pi_v; - } - - /// @return - constexpr float - getAngleDeg() const - { - const uint16_t angle = data & 0x3fff; - return static_cast(angle) / 16383.f * 360.f; - } - - /// @return - constexpr uint16_t - getAngleRaw() const - { - const uint16_t angle = data & 0x3fff; - return angle; - } - - uint16_t data; - }; + using Data = modm::IntegerAngle<14>; }; // struct as5047 /** @@ -129,13 +102,9 @@ class As5047 : public as5047, public modm::SpiDevice, protected modm: */ As5047(Data &data); - /// Call this function once before using the device - modm::ResumableResult - initialize(); - /// Read the raw data from the sensor modm::ResumableResult - readout(); + read(); /// Get the data object for this sensor inline Data & @@ -152,6 +121,4 @@ class As5047 : public as5047, public modm::SpiDevice, protected modm: } // namespace modm -#include "as5047_impl.hpp" - -#endif // MODM_AS5047_HPP \ No newline at end of file +#include "as5047_impl.hpp" \ No newline at end of file diff --git a/src/modm/driver/encoder/as5047.lb b/src/modm/driver/encoder/as5047.lb index a556ebdb74..b9a4cbe19b 100644 --- a/src/modm/driver/encoder/as5047.lb +++ b/src/modm/driver/encoder/as5047.lb @@ -22,7 +22,8 @@ def prepare(module, options): module.depends( ":architecture:gpio", ":architecture:spi.device", - ":processing:resumable" + ":processing:resumable", + ":math:geometry" ) return True diff --git a/src/modm/driver/encoder/as5047_impl.hpp b/src/modm/driver/encoder/as5047_impl.hpp index df23d7187e..0107333a53 100644 --- a/src/modm/driver/encoder/as5047_impl.hpp +++ b/src/modm/driver/encoder/as5047_impl.hpp @@ -11,10 +11,6 @@ */ // ---------------------------------------------------------------------------- -#ifndef MODM_AS5047_HPP -#error "Don't include this file directly, use 'as5047.hpp' instead!" -#endif - namespace modm { @@ -27,7 +23,7 @@ As5047::As5047(Data &data) : data(data) template modm::ResumableResult -As5047::readout() +As5047::read() { RF_BEGIN(); @@ -39,7 +35,7 @@ As5047::readout() RF_CALL(SpiMaster::transfer(outBuffer, inBuffer, 2)); Cs::set(); - modm::delay_us(1); + modm::delay(1us); Cs::reset(); outBuffer[1] = 0; diff --git a/src/modm/math/geometry/angle_int.hpp b/src/modm/math/geometry/angle_int.hpp new file mode 100644 index 0000000000..867b87bf19 --- /dev/null +++ b/src/modm/math/geometry/angle_int.hpp @@ -0,0 +1,65 @@ +// coding: utf-8 +// ---------------------------------------------------------------------------- +/* + * Copyright (c) 2024, Thomas Sommer + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#pragma once + +#include +#include +#include + +namespace modm +{ + +/// @brief Represents an absolute angle in a full circle +/// @group modm_math_geometry +template +struct modm_packed IntegerAngle +{ + using T = modm::least_uint; + using DeltaType = std::make_signed_t>; + + T data; + static constexpr T max = std::pow(2, Bits) - 1; + + constexpr DeltaType + getDelta() + { + static DeltaType previous{static_cast(data)}; + DeltaType delta = DeltaType(data) - previous; + + if (delta < -(max / 2)) + { + delta += max; + } else if (delta > (max / 2)) + { + delta -= max; + } + + previous = data; + + return delta; + } + + constexpr float + toDegree() const + { + return float(data) * 360 / max; + } + + constexpr float + toRadian() const + { + return float(data) * 2 * std::numbers::pi_v / max; + } +}; +} // namespace modm \ No newline at end of file From 55c88ba875e20ff3919480d6b5b3a96678761654 Mon Sep 17 00:00:00 2001 From: Thomas Sommer Date: Mon, 10 Jun 2024 10:44:43 +0200 Subject: [PATCH 2/2] added driver/encoder/as5600 --- README.md | 30 +-- src/modm/driver/encoder/as5600.hpp | 332 +++++++++++++++++++++++++++++ src/modm/driver/encoder/as5600.lb | 32 +++ 3 files changed, 380 insertions(+), 14 deletions(-) create mode 100644 src/modm/driver/encoder/as5600.hpp create mode 100644 src/modm/driver/encoder/as5600.lb diff --git a/README.md b/README.md index 3769b3c1f5..32c7f8cc58 100644 --- a/README.md +++ b/README.md @@ -722,104 +722,106 @@ your specific needs. AMS5915 APA102 AS5047 -AT24MAC402 +AS5600 +AT24MAC402 SPI Flash BME280 BMI088 BMP085 BNO055 -CAT24AA +CAT24AA CYCLE-COUNTER DRV832X DS1302 DS1631 DS18B20 -EA-DOG +EA-DOG Encoder Input Encoder Input BitBang Encoder Output BitBang FT245 FT6x06 -Gpio Sampler +Gpio Sampler HCLAx HD44780 HMC58x HMC6343 HX711 -I2C-EEPROM +I2C-EEPROM ILI9341 IS31FL3733 ITG3200 IXM42XXX L3GD20 -LAN8720A +LAN8720A LAWICEL LIS302DL LIS3DSH LIS3MDL LM75 -LP503x +LP503x LSM303A LSM6DS33 LSM6DSO LTC2984 MAX31855 -MAX31865 +MAX31865 MAX6966 MAX7219 MCP23x17 MCP2515 MCP3008 -MCP7941x +MCP7941x MCP990X MMC5603 MS5611 MS5837 NOKIA5110 -NRF24 +NRF24 TFT-DISPLAY PAT9125EL PCA8574 PCA9535 PCA9548A -PCA9685 +PCA9685 QMC5883L SH1106 SIEMENS-S65 SIEMENS-S75 SK6812 -SK9822 +SK9822 SSD1306 ST7586S ST7789 STTS22H STUSB4500 -SX1276 +SX1276 SX128X TCS3414 TCS3472 TLC594x TMP102 -TMP12x +TMP12x TMP175 TOUCH2046 VL53L0 VL6180 WS2812 + diff --git a/src/modm/driver/encoder/as5600.hpp b/src/modm/driver/encoder/as5600.hpp new file mode 100644 index 0000000000..6708d11dfd --- /dev/null +++ b/src/modm/driver/encoder/as5600.hpp @@ -0,0 +1,332 @@ + +// coding: utf-8 +// ---------------------------------------------------------------------------- +/* + * Copyright (c) 2024, Thomas Sommer + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + +#pragma once + +#include +#include +#include + +namespace modm +{ + +/// @ingroup modm_driver_as5600 +struct as5600 +{ + + // @see datasheet page 19 + enum Register : uint8_t + { + ZMCO = 0x00, + ZPOS = 0x01, // two bytes + MPOS = 0x03, // two bytes + MANG = 0x05, // two bytes + CONF = 0x07, // two bytes + ANGLE_RAW = 0x0C, // two bytes + ANGLE = 0x0E, // two bytes + STATUS = 0x0B, + AGC = 0x1A, + MAGNITUDE = 0x1B, // two bytes + BURN = 0xFF, + + // Only AS5600L supports changing the I2c address + I2C_ADDR = 0x20, + I2C_UPDT = 0x21, + }; + + // @see datasheet page 20 + enum class Config : uint16_t + { + PM0 = Bit0, // Power Mode + PM1 = Bit1, + + HYST0 = Bit2, // Hysteresis + HYST1 = Bit3, + + OUTS0 = Bit4, // Output Stage + OUTS1 = Bit5, + + PWM0 = Bit6, // PWM Frequency + PWM1 = Bit7, + + SF0 = Bit8, // Slow Filter + SF1 = Bit9, + + FTH0 = Bit10, // Fast Filter Threshold + FTH1 = Bit11, + FTH2 = Bit12, + + WD = Bit13 // Watchdog 0: Off, 1: On + }; + MODM_FLAGS16(Config); + + enum class PowerMode : uint16_t + { + NOM = 0, + LPM1 = 1, + LPM2 = 2, + LPM3 = 3, + }; + using PowerMode_t = Configuration; + + enum class Hysteresis : uint16_t + { + OFF = 0, + LSB = 1, + LSB2 = 2, + LSB3 = 3 + }; + using Hysteresis_t = Configuration; + + enum class OutputStage : uint16_t + { + ANALOG = 0, + ANALOG_REDUCED = 1, + PWM = 2 + }; + using OutputStage_t = Configuration; + + enum class PWMFrequency : uint16_t + { + Hz115 = 0, + Hz230 = 1, + Hz460 = 2, + Hz920 = 3 + }; + using PWMFrequency_t = Configuration; + + enum class SlowFilter : uint16_t + { + x16 = 0, + x8 = 1, + x4 = 2, + x2 = 3 + }; + using SlowFilter_t = Configuration; + + enum class FastFilterThreshold : uint16_t + { + LSB6 = 0, + LSB7 = 1, + LSB9 = 2, + LSB18 = 3, + LSB21 = 4, + LSB24 = 5, + LSB10 = 6 + }; + using FastFilterThreshold_t = Configuration; + + enum class Burn : uint8_t + { + ANGLE = Bit3, // Burn Angle data + CONFIG = Bit2 // Burn Config data + }; + + // @see datasheet page 21 + enum class Status : uint8_t + { + MagnetTooStrong = Bit3, + MagnetTooWeak = Bit4, + MagnetDetected = Bit5 + }; + MODM_FLAGS8(Status); + + using Data = modm::IntegerAngle<12>; +}; + +/// @ingroup modm_driver_as5600 +template +class As5600 : public as5600, public modm::I2cDevice +{ +public: + /** AS5600 has hardwired address 0x36 + * AS4500L has default address 0x40 but supports programming a different one + */ + As5600(Data &data, uint8_t address = 0x36) : I2cDevice(address), data(data) {} + + /** Reset to Power up state. + * Useful for developement, not required in production. + */ + modm::ResumableResult + reset() + { + RF_BEGIN(); + + bool success = true; + + /// The config registers span from 0x00 to 0x08 + for (uint8_t reg = 0x00; reg < 0x08; reg += 2) + { + buffer[0] = reg; + buffer[1] = 0; + buffer[2] = 0; + + this->transaction.configureWrite(buffer, 3); + + RF_CALL(this->runTransaction()); + success &= this->wasTransactionSuccessful(); + } + + RF_END_RETURN(success); + } + + modm::ResumableResult + configure(Config_t config) + { + return write(Register::CONF, config.value); + } + + modm::ResumableResult + setI2cAddress(uint8_t address) + { + return write(Register::I2C_ADDR, address); + } + + /// Wait 1ms after setting the lower limit + modm::ResumableResult + setLowerLimit(Data data) + { + return write(Register::ZPOS, data.data); + } + + /// Wait 1ms after setting the upper limit + modm::ResumableResult + setUpperLimit(Data data) + { + return write(Register::MPOS, data.data); + } + + modm::ResumableResult + setMaxAngle(Data data) + { + return write(Register::MANG, data.data); + } + + /// Permanently burn configurations + /// @warning As5600 can be burned only 3 times! + modm::ResumableResult + burn(Burn flags) + { + buffer[0] = static_cast(flags); + return write(Register::BURN, buffer, 1); + } + + modm::ResumableResult + getRawValue() + { + RF_BEGIN(); + + const uint16_t result = RF_CALL(read(Register::ANGLE_RAW)); + + // Raw value requires masking + RF_END_RETURN(Data(result & Data::max)); + } + + modm::ResumableResult + getStatus() + { + return static_cast(read(Register::STATUS)); + } + + modm::ResumableResult + getMagnitude() + { + return read(Register::MAGNITUDE); + } + + /** Automated Gain Control + * + * The AS5600 uses Automatic Gain Control in a closed loop to compensate + * for variations of the magnetic field strength due to changes of temperature, + * airgap between IC and magnet, and magnet degradation. + * + * For the most robust performance, the gain value should be in the center of its range. + * The airgap of the physical system can be adjusted to achieve this value. + * + * In 5V operation, range is 0-255 + * In 3.3V operation, range is reduced to 0-128 + */ + modm::ResumableResult + getAgcValue() + { + return read(Register::AGC); + } + + modm::ResumableResult + read() + { + RF_BEGIN(); + + data.data = RF_CALL(read(Register::ANGLE)); + + RF_END_RETURN(this->wasTransactionSuccessful()); + } + + inline Data & + getData() + { + return data; + } + +private: + template + modm::ResumableResult + write(Register reg, T value) + { + RF_BEGIN(); + + buffer[0] = reg; + + if constexpr (std::is_same_v) + { + buffer[1] = value; + } else + { + buffer[1] = value >> 8; + buffer[2] = value; + } + + this->transaction.configureWrite(buffer, 1 + sizeof(T)); + + RF_END_RETURN_CALL(this->runTransaction()); + } + + template + modm::ResumableResult + read(Register reg) + { + RF_BEGIN(); + + buffer[0] = reg; + this->transaction.configureWriteRead(buffer, 1, buffer + 1, sizeof(T)); + + RF_CALL(this->runTransaction()); + + T result; + + if constexpr (std::is_same_v) + { + result = buffer[1]; + } else + { + result = buffer[1] << 8 | buffer[2]; + } + + RF_END_RETURN(result); + } + + Data &data; + uint8_t buffer[3]; +}; +} // namespace modm \ No newline at end of file diff --git a/src/modm/driver/encoder/as5600.lb b/src/modm/driver/encoder/as5600.lb new file mode 100644 index 0000000000..5cf328aecc --- /dev/null +++ b/src/modm/driver/encoder/as5600.lb @@ -0,0 +1,32 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# +# Copyright (c) 2024, Thomas Sommer +# +# This file is part of the modm project. +# +# This Source Code Form is subject to the terms of the Mozilla Public +# License, v. 2.0. If a copy of the MPL was not distributed with this +# file, You can obtain one at http://mozilla.org/MPL/2.0/. +# ----------------------------------------------------------------------------- + +def init(module): + module.name = ":driver:as5600" + module.description = """\ +# AS5600 10 bit absolute encoder SPI driver + +[Datasheet](https://ams.com/documents/20143/36005/AS5600_DS000365_5-00.pdf) +""" + +def prepare(module, options): + module.depends( + ":architecture:gpio", + ":architecture:i2c.device", + ":processing:resumable", + ":math:geometry" + ) + return True + +def build(env): + env.outbasepath = "modm/src/modm/driver/encoder" + env.copy("as5600.hpp")