From ec18cf361408b8d06ec79eb731a454cde02ba600 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 21 Jan 2021 16:15:18 +1100
Subject: [PATCH] SITL: add simulated AS5600 sensor

---
 libraries/SITL/SIM_AS5600.cpp | 33 ++++++++++++++++++++
 libraries/SITL/SIM_AS5600.h   | 57 +++++++++++++++++++++++++++++++++++
 libraries/SITL/SIM_I2C.cpp    |  7 +++++
 libraries/SITL/SIM_config.h   |  4 +++
 4 files changed, 101 insertions(+)
 create mode 100644 libraries/SITL/SIM_AS5600.cpp
 create mode 100644 libraries/SITL/SIM_AS5600.h

diff --git a/libraries/SITL/SIM_AS5600.cpp b/libraries/SITL/SIM_AS5600.cpp
new file mode 100644
index 0000000000..23a019e5ec
--- /dev/null
+++ b/libraries/SITL/SIM_AS5600.cpp
@@ -0,0 +1,33 @@
+#include "SIM_config.h"
+
+#if AP_SIM_AS5600_ENABLED
+
+#include "SIM_AS5600.h"
+
+#include <stdio.h>
+
+void SITL::AS5600::init()
+{
+    add_register("ZMCO", AS5600DevReg::ZMCO, I2CRegisters::RegMode::RDONLY);
+    set_register(AS5600DevReg::ZMCO, (uint8_t)1U);
+
+    add_register("RAH", AS5600DevReg::RAW_ANGLE_HIGH, I2CRegisters::RegMode::RDONLY);
+    set_register(AS5600DevReg::RAW_ANGLE_HIGH, (uint8_t)0U);
+    add_register("RAL", AS5600DevReg::RAW_ANGLE_LOW, I2CRegisters::RegMode::RDONLY);
+    set_register(AS5600DevReg::RAW_ANGLE_LOW, (uint8_t)0U);
+}
+
+void SITL::AS5600::update(const class Aircraft &aircraft)
+{
+
+    // gcs().send_text(MAV_SEVERITY_INFO, "SIM_AS5600: PWM0=%u PWM1=%u PWM2=%u ENABLE=%u", last_print_pwm0, last_print_pwm1, last_print_pwm2, last_print_enable);
+    Quaternion attitude;
+    aircraft.get_attitude(attitude);
+    const float pitch = degrees(attitude.get_euler_pitch());
+    uint16_t pitch_180 = pitch + 90;
+
+    set_register(AS5600DevReg::RAW_ANGLE_HIGH, uint8_t(pitch_180 >> 8));
+    set_register(AS5600DevReg::RAW_ANGLE_LOW, uint8_t(pitch_180 & 0xff));
+}
+
+#endif
diff --git a/libraries/SITL/SIM_AS5600.h b/libraries/SITL/SIM_AS5600.h
new file mode 100644
index 0000000000..6b8e41d879
--- /dev/null
+++ b/libraries/SITL/SIM_AS5600.h
@@ -0,0 +1,57 @@
+#include "SIM_config.h"
+
+#if AP_SIM_AS5600_ENABLED
+
+#include "SIM_I2CDevice.h"
+
+namespace SITL {
+
+// datasheet: https://ams.com/documents/20143/36005/AS5600_DS000365_5-00.pdf
+
+class AS5600DevReg : public I2CRegEnum {
+public:
+    // low
+    static const uint8_t ZMCO           = 0x00;
+    static const uint8_t ZPOS_HIGH      = 0x01;
+    static const uint8_t ZPOS_LOW       = 0x02;
+    static const uint8_t MPOS_HIGH      = 0x03;
+    static const uint8_t MPOS_LOW       = 0x04;
+    static const uint8_t MANG_HIGH      = 0x05;
+    static const uint8_t MANG_LOW       = 0x06;
+    static const uint8_t CONF_HIGH      = 0x07;
+    static const uint8_t CONF_LOW       = 0x08;
+
+    // output registers
+    static const uint8_t RAW_ANGLE_HIGH = 0x0C;
+    static const uint8_t RAW_ANGLE_LOW  = 0x0D;
+    static const uint8_t ANGLE_HIGH     = 0x0E;
+    static const uint8_t ANGLE_LOW      = 0x0F;
+
+    // status registers
+    static const uint8_t STATUS         = 0x0B;
+    static const uint8_t AGC            = 0x1A;
+    static const uint8_t MAGNITUDE_HIGH = 0x1B;
+    static const uint8_t MAGNITUDE_LOW  = 0x1C;
+
+    // Burn commands
+    static const uint8_t BURN           = 0xFF;
+};
+
+class AS5600 : public I2CDevice, protected I2CRegisters_8Bit
+{
+public:
+    void init() override;
+
+    void update(const class Aircraft &aircraft) override;
+
+    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override {
+        return I2CRegisters_8Bit::rdwr(data);
+    }
+
+private:
+
+};
+
+} // namespace SITL
+
+#endif  // AP_SIM_AS5600_ENABLED
diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp
index 40e1d1a89b..54095b2313 100644
--- a/libraries/SITL/SIM_I2C.cpp
+++ b/libraries/SITL/SIM_I2C.cpp
@@ -24,6 +24,7 @@
 #include "SIM_I2C.h"
 
 #include "SIM_Airspeed_DLVR.h"
+#include "SIM_AS5600.h"
 #include "SIM_BattMonitor_SMBus_Generic.h"
 #include "SIM_BattMonitor_SMBus_Maxell.h"
 #include "SIM_BattMonitor_SMBus_Rotoye.h"
@@ -117,6 +118,9 @@ static INA3221 ina3221;
 #if AP_SIM_TERARANGERI2C_ENABLED
 static TeraRangerI2C terarangeri2c;
 #endif
+#if AP_SIM_AS5600_ENABLED
+static AS5600 as5600;  // AoA sensor
+#endif  // AP_SIM_AS5600_ENABLED
 
 struct i2c_device_at_address {
     uint8_t bus;
@@ -146,6 +150,9 @@ struct i2c_device_at_address {
 #endif
     { 1, 0x38, ignored }, // NCP5623
     { 1, 0x39, ignored }, // NCP5623C
+#if AP_SIM_AS5600_ENABLED
+    { 1, 0x36, as5600 },
+#endif
     { 1, 0x40, ignored }, // KellerLD
 #if AP_SIM_MS5525_ENABLED
     { 1, 0x76, ms5525 },  // MS5525: ARSPD_TYPE = 4
diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h
index d13bcc454d..a028740f37 100644
--- a/libraries/SITL/SIM_config.h
+++ b/libraries/SITL/SIM_config.h
@@ -13,6 +13,10 @@
 #define HAL_SIM_SERIALPROXIMITYSENSOR_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif
 
+#ifndef AP_SIM_AS5600_ENABLED
+#define AP_SIM_AS5600_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#endif
+
 #ifndef HAL_SIM_PS_RPLIDARA1_ENABLED
 #define HAL_SIM_PS_RPLIDARA1_ENABLED HAL_SIM_SERIALPROXIMITYSENSOR_ENABLED
 #endif