diff --git a/libraries/AP_AngleSensor/AP_AngleSensor_config.h b/libraries/AP_AngleSensor/AP_AngleSensor_config.h new file mode 100644 index 00000000000000..66a3ebc46c0ab4 --- /dev/null +++ b/libraries/AP_AngleSensor/AP_AngleSensor_config.h @@ -0,0 +1,7 @@ +#pragma once + +#include + +#ifndef AP_ANGLESENSOR_AS5600_ENABLED +#define AP_ANGLESENSOR_AS5600_ENABLED 1 +#endif diff --git a/libraries/AP_AngleSensor/AS5600.cpp b/libraries/AP_AngleSensor/AS5600.cpp new file mode 100644 index 00000000000000..f63d917ad79488 --- /dev/null +++ b/libraries/AP_AngleSensor/AS5600.cpp @@ -0,0 +1,79 @@ +#include "AP_AngleSensor_config.h" + +#if AP_ANGLESENSOR_AS5600_ENABLED + +// AS5600 angle sensor +// base on code by Cole Mero + +#include "AS5600.h" + +#include +#include + +#include + +#include + +static constexpr uint8_t REG_ZMCO { 0x00 }; +static constexpr uint8_t REG_ZPOS_HI { 0x01 }; +static constexpr uint8_t REG_ZPOS_LO { 0x02 }; +static constexpr uint8_t REG_MPOS_HI { 0x03 }; +static constexpr uint8_t REG_MPOS_LO { 0x04 }; +static constexpr uint8_t REG_MANG_HI { 0x05 }; +static constexpr uint8_t REG_MANG_LO { 0x06 }; +static constexpr uint8_t REG_CONF_HI { 0x07 }; +static constexpr uint8_t REG_CONF_LO { 0x08 }; +static constexpr uint8_t REG_RAW_ANG_HI { 0x0C }; +static constexpr uint8_t REG_RAW_ANG_LO { 0x0D }; +static constexpr uint8_t REG_ANG_HI { 0x0E }; +static constexpr uint8_t REG_ANG_LO { 0x0F }; +static constexpr uint8_t REG_STAT { 0x0B }; +static constexpr uint8_t REG_AGC { 0x1A }; +static constexpr uint8_t REG_MAG_HI { 0x1B }; +static constexpr uint8_t REG_MAG_LO { 0x1C }; +static constexpr uint8_t REG_BURN { 0xFF }; + +extern const AP_HAL::HAL &hal; + +void AP_AngleSensor_AS5600::timer(void) +{ + uint16_t angle; + { + WITH_SEMAPHORE(dev->get_semaphore()); + dev->read_registers(REG_RAW_ANG_HI, (uint8_t*)&angle, 2); + } + { + WITH_SEMAPHORE(readings.sem); + readings.angle = be16toh(angle); + } +} + +void AP_AngleSensor_AS5600::init(void) +{ + dev = hal.i2c_mgr->get_device(bus, address); + + if (!dev) { + return; + } + + // insert some register reads here to try to fingerprint the device + + WITH_SEMAPHORE(dev->get_semaphore()); + dev->register_periodic_callback(25000, FUNCTOR_BIND_MEMBER(&AP_AngleSensor_AS5600::timer, void)); +} + +void AP_AngleSensor_AS5600::update() +{ + { + WITH_SEMAPHORE(readings.sem); + state.angle = readings.angle; + } + + const uint32_t tnow = AP_HAL::millis(); + state.last_reading_ms = tnow; + + AP::logger().Write("AoAR", "TimeUS,Angle", "QH", AP_HAL::micros64(), state.angle); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "angle=%u", state.angle); +} + +#endif diff --git a/libraries/AP_AngleSensor/AS5600.h b/libraries/AP_AngleSensor/AS5600.h new file mode 100644 index 00000000000000..d9260c21d22ceb --- /dev/null +++ b/libraries/AP_AngleSensor/AS5600.h @@ -0,0 +1,40 @@ +#include "AP_AngleSensor_config.h" + +#if AP_ANGLESENSOR_AS5600_ENABLED + +#include +#include + +class AP_AngleSensor_AS5600 { + +public: + + AP_AngleSensor_AS5600(uint8_t _bus, uint8_t _address) : + bus{_bus}, + address{_address} + { } + + void init(void); + void update(void); + +private: + + AP_HAL::OwnPtr dev; + + uint8_t bus; + uint8_t address; + + struct { + HAL_Semaphore sem; + uint16_t angle; + } readings; + + struct { + uint32_t last_reading_ms; + uint16_t angle; + } state; + + void timer(); +}; + +#endif // AP_ANGLESENSOR_AS5600_ENABLED