AP_AngleSensor: add class for handling absolute-angle-measuring sensors

Co-authored-by: colejmero <[email protected]>
ArduPilot · Dec 11, 2024 · cc162dc · cc162dc
1 parent db6323f
commit cc162dc
Show file tree

Hide file tree

Showing 3 changed files with 126 additions and 0 deletions.
diff --git a/libraries/AP_AngleSensor/AP_AngleSensor_config.h b/libraries/AP_AngleSensor/AP_AngleSensor_config.h
@@ -0,0 +1,7 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL_Boards.h>
+
+#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
@@ -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 <AP_HAL/AP_HAL.h>
+#include <AP_Logger/AP_Logger.h>
+
+#include <AP_HAL/utility/sparse-endian.h>
+
+#include <GCS_MAVLink/GCS.h>
+
+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
@@ -0,0 +1,40 @@
+#include "AP_AngleSensor_config.h"
+
+#if AP_ANGLESENSOR_AS5600_ENABLED
+
+#include <AP_HAL/I2CDevice.h>
+#include <unistd.h>
+
+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<AP_HAL::I2CDevice> 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