Plane: add As5600 angle-of-attack sensor

ArduPlane/ArduPlane.cpp

@@ -61,6 +61,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK(read_rangefinder,       50,    100),
     SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100),
     SCHED_TASK_CLASS(Compass,          &plane.compass,              cal_update, 50, 50),
+    SCHED_TASK_CLASS(AS5600_AOA, &plane.aoa_sensor, update, 10, 200),
     SCHED_TASK(accel_cal_update,       10,    50),
 #if OPTFLOW == ENABLED
     SCHED_TASK_CLASS(OpticalFlow, &plane.optflow, update,    50,    50),
@@ -193,6 +194,14 @@ void Plane::update_compass(void)
     }
 }
 
+/*
+ * Read and log Angle of Attack
+ */
+
+//void Plane::read_aoa(void){
+//    aoa_sensor.getRawAngle();
+//}
+
 /*
   do 10Hz logging
  */

ArduPlane/Plane.h

@@ -76,6 +76,7 @@
 #include <AP_OSD/AP_OSD.h>
 
 #include <AP_Rally/AP_Rally.h>
+#include <AP_AS5600_AOA/AS5600_AOA.h>
 
 #include <AP_OpticalFlow/AP_OpticalFlow.h>     // Optical Flow library
 #include <AP_Parachute/AP_Parachute.h>
@@ -196,6 +197,8 @@ class Plane : public AP_Vehicle {
 
     AP_RPM rpm_sensor;
 
+    AS5600_AOA aoa_sensor;
+
     AP_TECS TECS_controller{ahrs, aparm, landing};
     AP_L1_Control L1_controller{ahrs, &TECS_controller};
 
@@ -996,6 +999,7 @@ class Plane : public AP_Vehicle {
     void read_airspeed(void);
     void rpm_update(void);
     void accel_cal_update(void);
+    void read_aoa(void);
 
     // system.cpp
     void init_ardupilot() override;

ArduPlane/system.cpp

@@ -67,6 +67,9 @@ void Plane::init_ardupilot()
 
     rpm_sensor.init();
 
+    //initialise AS5600_AOA sensor
+    aoa_sensor.init();
+
     // setup telem slots with serial ports
     gcs().setup_uarts();
 

ArduPlane/wscript

@@ -29,6 +29,7 @@ def build(bld):
             'AP_Devo_Telem',
             'AP_OSD',
             'AC_AutoTune',
+            'AP_AS5600_AOA',
             'AP_KDECAN',
         ],
     )

libraries/AP_AS5600_AOA/AS5600_AOA.cpp

@@ -0,0 +1,209 @@
+/****************************************************
+ * AS5600 Angle of Attack Class for Ardupilot platform
+ * Author: Cole Mero
+ * Date: 15 Feb 2021
+ * File: AS5600_AOA.cpp
+ * Based on: www.ams.com
+ *
+ * Description:  This class has been designed to
+ * access the AS5600 magnetic encoder sensor to
+ * read the angle of attack and record it for experimental purposes
+ *
+ * It was adapted from the Arduino library available for the AS5600 sensor.
+ *
+***************************************************/
+
+#include <unistd.h>
+#include "AS5600_AOA.h"
+#include <AP_Common/AP_Common.h>
+#include <GCS_MAVLink/GCS.h>
+
+
+extern const AP_HAL::HAL &hal;
+
+/**************************************************
+ * Method: AS5600_AOA
+ * In: none
+ * Out: none
+ * Description: constructor for AS5600_AOA
+***************************************************/
+ AS5600_AOA::AS5600_AOA()
+{
+    bus = 2; //Sets the bus number for the device, unclear what this number should be, trial and error to make it work
+    address = 0x36; //This is the I2C address for the device, it is set by the manufacturer
+}
+
+bool AS5600_AOA::init()
+{
+    dev = hal.i2c_mgr->get_device(bus, address);
+
+    if (!dev){
+        return false;
+    }
+
+    WITH_SEMAPHORE(dev->get_semaphore());
+    dev->set_speed(AP_HAL::Device::SPEED_LOW);
+    dev->set_retries(2);
+
+    dev->register_periodic_callback(
+        50000,
+        FUNCTOR_BIND_MEMBER(&AS5600_AOA::_timer, void));
+
+    return true;
+}
+
+/* mode = 0, output PWM, mode = 1 output analog (full range from 0% to 100% between GND and VDD*/
+//void AS5600_AOA::setOutPut(uint8_t mode){
+//    uint8_t config_status;
+//    if (!dev->read_registers(REG_CONF_LO, &config_status, 1)){
+//        return;
+//    }
+//    if (mode == 1){
+//        config_status = config_status & 0xcf;
+//    }else{
+//        config_status = config_status & 0xef;
+//    }
+//
+//    if (!dev->write_register(REG_CONF_LO, config_status)){
+//        return;
+//    }
+//}
+
+
+/*******************************************************
+ * Method: update
+ * In: none
+ * Out: value of raw angle register
+ * Description: gets raw value of magnet position.
+ * start, end, and max angle settings do not apply
+*******************************************************/
+void AS5600_AOA::update(void)
+{
+  WITH_SEMAPHORE(sem);
+
+  //Gets the two relevant register values and multiplies by conversion factor to get degrees
+  uint16_t angleRaw = ((highByte << 8) | lowByte)*0.087;
+
+  AP::logger().Write("AOAR", "TimeUS, Angle", "QH", AP_HAL::micros64(), angleRaw);
+
+  gcs().send_text(MAV_SEVERITY_INFO, "Angle: %d", angleRaw);
+}
+
+
+void AS5600_AOA::_timer(void)
+{
+    gcs().send_text(MAV_SEVERITY_INFO, "update called - regHiRead: %d regLoRead: %d", regHiRead, regLoRead);
+
+    uint8_t low, high;
+
+    {
+        WITH_SEMAPHORE(dev->get_semaphore());
+
+        if (!dev->read_registers(REG_RAW_ANG_HI, &high, 1)){
+            return;
+        }
+
+        regHiRead = true;
+        gcs().send_text(MAV_SEVERITY_INFO, "REG_RAW_ANG_HI: %d", high);
+
+        if (!dev->read_registers(REG_RAW_ANG_LO, &low, 1)){
+            return;
+        }
+
+        gcs().send_text(MAV_SEVERITY_INFO, "REG_RAW_ANG_LO: %d", low);
+        regLoRead = true;
+    }
+
+    WITH_SEMAPHORE(sem);
+    lowByte = low;
+    highByte = high;
+}
+
+
+/*******************************************************
+ * Method: setMaxAngle
+ * In: new maximum angle to set OR none
+ * Out: value of max angle register
+ * Description: sets a value in maximum angle register.
+ * If no value is provided, method will read position of
+ * magnet. Setting this register zeros out max position
+ * register.
+*******************************************************/
+/*uint16_t AS5600_AOA::setMaxAngle(uint16_t newMaxAngle)
+{
+   if(newMaxAngle == -1){
+    maxAngle = getRawAngle();
+  } else {
+    maxAngle = newMaxAngle;
+  }
+
+  WITH_SEMAPHORE(dev->get_semaphore());
+
+  if (!dev->write_register(REG_MANG_HI, HIGHBYTE(rawStartAngle))){
+        return -1;
+    }
+    if (!dev->write_register(REG_MANG_LO, LOWBYTE(rawStartAngle))){
+        return -1;
+    }
+    if (!dev->read_registers(REG_MANG_HI, &highByte, 1) || !dev->read_registers(REG_MANG_LO, &lowByte, 1)){
+        return -1;
+    }
+
+    maxAngle = ((highByte << 8) | lowByte);
+  return maxAngle;
+}*/
+
+
+/*******************************************************
+ * Method: getMaxAngle
+ * In: none
+ * Out: value of max angle register
+ * Description: gets value of maximum angle register.
+*******************************************************/
+uint16_t AS5600_AOA::getMaxAngle()
+{
+    WITH_SEMAPHORE(dev->get_semaphore());
+
+    if (!dev->read_registers(REG_MANG_HI, &highByte, 1) || !dev->read_registers(REG_MANG_LO, &lowByte, 1)){
+         return -1;
+     }
+
+     maxAngle = ((highByte << 8) | lowByte);
+
+     return maxAngle;
+}
+
+
+/*******************************************************
+ * Method: setStartPosition
+ * In: new start angle position
+ * Out: value of start position register
+ * Description: sets a value in start position register.
+ * If no value is provided, method will read position of
+ * magnet.
+*******************************************************/
+/*uint16_t AS5600_AOA::setStartPosition(uint16_t startAngle = -1)
+{
+  if(startAngle == -1)
+  {
+    rawStartAngle = getRawAngle();
+  } else {
+    rawStartAngle = startAngle;
+  }
+
+  WITH_SEMAPHORE(dev->get_semaphore());
+
+  if (!dev->write_register(REG_ZPOS_HI, HIGHBYTE(rawStartAngle))){
+      return -1;
+  }
+  if (!dev->write_register(REG_ZPOS_LO, LOWBYTE(rawStartAngle))){
+      return -1;
+  }
+  if (!dev->read_registers(REG_ZPOS_HI, &highByte, 1) || !dev->read_registers(REG_ZPOS_LO, &lowByte, 1)){
+      return -1;
+  }
+
+  uint16_t zPosition = ((highByte << 8) | lowByte);
+
+  return(zPosition);
+}*/

libraries/AP_AS5600_AOA/AS5600_AOA.h

@@ -0,0 +1,87 @@
+#include <cstdint>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/utility/OwnPtr.h>
+#include <AP_HAL/I2CDevice.h>
+#include <unistd.h>
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Logger/AP_Logger.h>
+
+
+class AS5600_AOA {
+
+public:
+
+    AS5600_AOA(void);
+
+    bool init();
+
+    void update(void);
+
+    uint16_t setMaxAngle(uint16_t newMaxAngle = -1);
+    uint16_t getMaxAngle();
+    uint16_t setStartPosition(uint16_t startAngle = -1);
+    uint16_t getStartPosition();
+    uint16_t setEndPosition(uint16_t endAngle = -1);
+    uint16_t getEndPosition();
+    uint16_t getScaledAngle();
+
+    int  detectMagnet();
+    int  getMagnetStrength();
+    int  getAgc();
+    uint16_t getMagnitude();
+
+    int  getBurnCount();
+    int  burnAngle();
+    int  burnMaxAngleAndConfig();
+    void setOutPut(uint8_t mode);
+
+    bool regHiRead = false;
+    bool regLoRead = false;
+
+private:
+
+    HAL_Semaphore sem;
+
+    AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
+
+    uint8_t  bus;
+    uint8_t  address;
+
+    uint16_t rawStartAngle;
+    uint16_t zPosition;
+    uint16_t rawEndAngle;
+    uint16_t mPosition;
+    uint16_t maxAngle;
+
+    /* Registers */
+
+    static const uint8_t REG_ZMCO       = 0x00;
+    static const uint8_t REG_ZPOS_HI    = 0x01;
+    static const uint8_t REG_ZPOS_LO    = 0x02;
+    static const uint8_t REG_MPOS_HI    = 0x03;
+    static const uint8_t REG_MPOS_LO    = 0x04;
+    static const uint8_t REG_MANG_HI    = 0x05;
+    static const uint8_t REG_MANG_LO    = 0x06;
+    static const uint8_t REG_CONF_HI    = 0x07;
+    static const uint8_t REG_CONF_LO    = 0x08;
+    static const uint8_t REG_RAW_ANG_HI = 0x0c;
+    static const uint8_t REG_RAW_ANG_LO = 0x0d;
+    static const uint8_t REG_ANG_HI     = 0x0e;
+    static const uint8_t REG_ANG_LO     = 0x0f;
+    static const uint8_t REG_STAT       = 0x0b;
+    static const uint8_t REG_AGC        = 0x1a;
+    static const uint8_t REG_MAG_HI     = 0x1b;
+    static const uint8_t REG_MAG_LO     = 0x1c;
+    static const uint8_t REG_BURN       = 0xff;
+
+    int readOneByte(uint8_t in_adr);
+    int readTwoBytes(uint8_t in_adr_hi, uint8_t in_adr_lo);
+    int writeOneByte(uint8_t adr_in, uint8_t dat_in);
+
+    uint8_t lowByte;
+    uint8_t highByte;
+
+
+    void _timer(void);
+};

libraries/SITL/SIM_AS5600.cpp

@@ -0,0 +1,9 @@
+#include "SIM_AS5600.h"
+
+#include <stdio.h>
+
+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);
+}