From a0af2252a7cf70b54119faa8973d876000813315 Mon Sep 17 00:00:00 2001
From: colejmero <57465269+colejmero@users.noreply.github.com>
Date: Thu, 18 May 2023 10:17:17 +1000
Subject: [PATCH] ArduPlane: Plane: add test bits for AOA_SSA

Some changes.
---
 ArduPlane/ArduPlane.cpp | 2 ++
 ArduPlane/Plane.h       | 5 +++++
 ArduPlane/system.cpp    | 3 +++
 ArduPlane/wscript       | 1 +
 4 files changed, 11 insertions(+)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 2c68fa97ed8cf..dbff212dd97d3 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -70,6 +70,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK(update_compass,         10,    200,  39),
     SCHED_TASK(calc_airspeed_errors,   10,    100,  42),
     SCHED_TASK(update_alt,             10,    200,  45),
+    SCHED_TASK_CLASS(AP_AngleSensor_AS5600,        &plane.aoa_sensor, update, 10,    200,  46),
     SCHED_TASK(adjust_altitude_target, 10,    200,  48),
 #if AP_ADVANCEDFAILSAFE_ENABLED
     SCHED_TASK(afs_fs_check,           10,    100,  51),
@@ -226,6 +227,7 @@ void Plane::update_compass(void)
     compass.read();
 }
 
+
 /*
   do 10Hz logging
  */
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 39bae83e5530d..87a6201373ec4 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -75,6 +75,7 @@
 #include <AP_OSD/AP_OSD.h>
 
 #include <AP_Rally/AP_Rally.h>
+#include <AP_AngleSensor/AS5600.h> //Magnetic Encoder library, line added by Cole
 
 #include <AP_OpticalFlow/AP_OpticalFlow.h>     // Optical Flow library
 #include <AP_Parachute/AP_Parachute.h>
@@ -89,6 +90,8 @@
 #include "quadplane.h"
 #include "tuning.h"
 
+
+
 // Configuration
 #include "config.h"
 
@@ -198,6 +201,8 @@ class Plane : public AP_Vehicle {
     AP_RPM rpm_sensor;
 #endif
 
+    AP_AngleSensor_AS5600 aoa_sensor{2, 0x36};                                             //This is code added by Cole
+
     AP_TECS TECS_controller{ahrs, aparm, landing, MASK_LOG_TECS};
     AP_L1_Control L1_controller{ahrs, &TECS_controller};
 
diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index e0ae140fd31f4..3a9b9fa6d33f1 100644
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@@ -155,6 +155,9 @@ void Plane::init_ardupilot()
 #if AP_GRIPPER_ENABLED
     g2.gripper.init();
 #endif
+
+    //Code added by Cole, initialize AS5600 sensor
+    aoa_sensor.init();
 }
 
 //********************************************************************************
diff --git a/ArduPlane/wscript b/ArduPlane/wscript
index 4dd670ccc34a5..529f783ec9667 100644
--- a/ArduPlane/wscript
+++ b/ArduPlane/wscript
@@ -28,6 +28,7 @@ def build(bld):
             'AP_Devo_Telem',
             'AP_OSD',
             'AC_AutoTune',
+            'AP_AngleSensor',
             'AP_Follow',
         ],
     )