Feature: Add MSP AOA sensor support for relaxed static stability aircraft control

src/main/CMakeLists.txt

@@ -209,6 +209,10 @@ main_sources(COMMON_SRC
     drivers/opflow/opflow_virtual.c
     drivers/opflow/opflow_virtual.h
 
+    drivers/aoa/aoa.h
+    drivers/aoa/aoa_virtual.c
+    drivers/aoa/aoa_virtual.h
+
     drivers/osd.c
     drivers/osd.h
     drivers/persistent.c
@@ -368,6 +372,8 @@ main_sources(COMMON_SRC
     io/gimbal_serial.h
     io/headtracker_msp.c
     io/headtracker_msp.h
+    io/aoa_msp.c
+
     io/osd_dji_hd.c
     io/osd_dji_hd.h
     io/lights.c
@@ -589,6 +595,9 @@ main_sources(COMMON_SRC
     sensors/barometer.h
     sensors/pitotmeter.c
     sensors/pitotmeter.h
+    sensors/aoa.c
+    sensors/aoa.h
+
     sensors/rangefinder.c
     sensors/rangefinder.h
     sensors/opflow.c

src/main/build/debug.h

@@ -79,6 +79,7 @@ typedef enum {
     DEBUG_GPS,
     DEBUG_LULU,
     DEBUG_SBUS2,
+    DEBUG_AOA,
     DEBUG_COUNT // also update debugModeNames in cli.c
 } debugType_e;
 

src/main/config/parameter_group_ids.h

@@ -132,7 +132,9 @@
 #define PG_GEOZONE_CONFIG 1042
 #define PG_GEOZONES 1043
 #define PG_GEOZONE_VERTICES 1044
-#define PG_INAV_END PG_GEOZONE_VERTICES
+#define PG_AOA_CONFIG 1045
+#define PG_AOA_CONTROL_CONFIG 1046
+#define PG_INAV_END PG_AOA_CONTROL_CONFIG
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/drivers/aoa/aoa.h

@@ -0,0 +1,51 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "common/time.h"
+#include "drivers/io.h"
+#include "drivers/bus.h"
+
+// AOA values are in decidegrees, typical range is -600 to +600 (-60.0° to +60.0°)
+// Use values outside valid range for special states
+#define AOA_NO_NEW_DATA          (INT16_MAX)    // No new data available from sensor
+#define AOA_OUT_OF_RANGE         (1810)         // Measurement out of valid range (181.0°)
+#define AOA_HARDWARE_FAILURE     (1811)         // Hardware failure detected
+
+struct aoaDev_s;
+
+typedef void (*aoaOpInitFuncPtr)(struct aoaDev_s * dev);
+typedef void (*aoaOpStartFuncPtr)(struct aoaDev_s * dev);
+typedef void (*aoaOpReadFuncPtr)(struct aoaDev_s * dev, int16_t * aoa, int16_t * sideslip);
+
+typedef struct aoaDev_s {
+    busDevice_t * busDev;
+    timeMs_t delayMs;
+    aoaOpInitFuncPtr init;
+    aoaOpStartFuncPtr update;
+    aoaOpReadFuncPtr read;
+} aoaDev_t;

src/main/drivers/aoa/aoa_virtual.c

@@ -0,0 +1,71 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <platform.h>
+
+#include "build/build_config.h"
+
+#include "common/utils.h"
+
+#include "drivers/aoa/aoa_virtual.h"
+
+static const virtualAoaVTable_t * highLevelDeviceVTable = NULL;
+
+static void virtualAoaInit(aoaDev_t * dev)
+{
+    UNUSED(dev);
+    return highLevelDeviceVTable->init();
+}
+
+static void virtualAoaUpdate(aoaDev_t * dev)
+{
+    UNUSED(dev);
+    return highLevelDeviceVTable->update();
+}
+
+static void virtualAoaRead(aoaDev_t * dev, int16_t * aoa, int16_t * sideslip)
+{
+    UNUSED(dev);
+    highLevelDeviceVTable->read(aoa, sideslip);
+}
+
+bool virtualAoaDetect(aoaDev_t * dev, const virtualAoaVTable_t * vtable)
+{
+    if (vtable && vtable->detect()) {
+
+        highLevelDeviceVTable = vtable;
+        dev->delayMs = AOA_VIRTUAL_TASK_PERIOD_MS;
+        dev->init = &virtualAoaInit;
+        dev->update = &virtualAoaUpdate;
+        dev->read = &virtualAoaRead;
+
+        return true;
+    }
+
+    return false;
+}

src/main/drivers/aoa/aoa_virtual.h

@@ -0,0 +1,38 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include "drivers/aoa/aoa.h"
+
+#define AOA_VIRTUAL_TASK_PERIOD_MS 30
+
+typedef struct virtualAoaVTable_s {
+    bool (*detect)(void);
+    void (*init)(void);
+    void (*update)(void);
+    void (*read)(int16_t * aoa, int16_t * sideslip);
+} virtualAoaVTable_t;
+
+bool virtualAoaDetect(aoaDev_t * dev, const virtualAoaVTable_t * vtable);

src/main/drivers/osd_symbols.h

@@ -145,6 +145,12 @@
 #define SYM_GFORCE_Y                0xBE  // 190 Gforce Y
 #define SYM_GFORCE_Z                0xBF  // 191 Gforce Z
 
+#ifdef USE_AOA
+#define SYM_AOA_UP                  0x17    //   up
+#define SYM_AOA_DOWN                0x1B  //   down
+#define SYM_AOA                     0x41       // A
+#endif
+
 #define SYM_BARO_TEMP               0xC0  // 192
 #define SYM_IMU_TEMP                0xC1  // 193
 #define SYM_TEMP                    0xC2  // 194 Thermometer icon

src/main/fc/cli.c

@@ -222,7 +222,8 @@ static const char *debugModeNames[DEBUG_COUNT] = {
     "HEADTRACKER",
     "GPS",
     "LULU",
-    "SBUS2"
+    "SBUS2",
+    "AOA"
 };
 
 /* Sensor names (used in lookup tables for *_hardware settings and in status
@@ -234,10 +235,10 @@ static const char *const gyroNames[] = {
 
 // sync this with sensors_e
 static const char * const sensorTypeNames[] = {
-    "GYRO", "ACC", "BARO", "MAG", "RANGEFINDER", "PITOT", "OPFLOW", "GPS", "GPS+MAG", NULL
+    "GYRO", "ACC", "BARO", "MAG", "RANGEFINDER", "PITOT", "OPFLOW", "AOA", "GPS", "GPS+MAG", NULL
 };
 
-#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG | SENSOR_RANGEFINDER | SENSOR_PITOT | SENSOR_OPFLOW)
+#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG | SENSOR_RANGEFINDER | SENSOR_PITOT | SENSOR_OPFLOW | SENSOR_AOA)
 
 static const char * const hardwareSensorStatusNames[] = {
     "NONE", "OK", "UNAVAILABLE", "FAILING"
@@ -271,6 +272,11 @@ static const char * const *sensorHardwareNames[] = {
 #else
         NULL,
 #endif
+#ifdef USE_AOA
+        table_aoa_hardware,
+#else
+        NULL,
+#endif
 };
 
 static void cliPrint(const char *str)
@@ -4142,14 +4148,17 @@ static void cliStatus(char *cmdline)
 #endif // for if at32
 #endif // for SITL
 
-    cliPrintLinef("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, OPFLOW=%s, GPS=%s",
+    cliPrintLinef(
+        "Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, "
+        "OPFLOW=%s, GPS=%s, AOA=%s",
         hardwareSensorStatusNames[getHwGyroStatus()],
         hardwareSensorStatusNames[getHwAccelerometerStatus()],
         hardwareSensorStatusNames[getHwCompassStatus()],
         hardwareSensorStatusNames[getHwBarometerStatus()],
         hardwareSensorStatusNames[getHwRangefinderStatus()],
         hardwareSensorStatusNames[getHwOpticalFlowStatus()],
-        hardwareSensorStatusNames[getHwGPSStatus()]
+        hardwareSensorStatusNames[getHwGPSStatus()],
+        hardwareSensorStatusNames[getHwAoaStatus()]
     );
 
 #ifdef USE_ESC_SENSOR

src/main/fc/fc_msp.c

@@ -100,6 +100,7 @@
 #include "io/vtx_string.h"
 #include "io/gps_private.h"  //for MSP_SIMULATOR
 #include "io/headtracker_msp.h"
+#include "io/aoa.h"
 
 #include "io/osd/custom_elements.h"
 
@@ -129,6 +130,7 @@
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
 #include "sensors/opflow.h"
+#include "sensors/aoa.h"
 #include "sensors/temperature.h"
 #include "sensors/esc_sensor.h"
 
@@ -1396,6 +1398,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 #else
         sbufWriteU8(dst, 0);
 #endif
+#ifdef USE_AOA
+        sbufWriteU8(dst, aoaConfig()->aoa_hardware);
+#else
+        sbufWriteU8(dst, 0);
+#endif
 #ifdef USE_OPFLOW
         sbufWriteU8(dst, opticalFlowConfig()->opflow_hardware);
 #else
@@ -2582,6 +2589,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 #else
             sbufReadU8(src);        // rangefinder hardware
 #endif
+#ifdef USE_AOA
+            aoaConfigMutable()->aoa_hardware = sbufReadU8(src);
+#else
+            sbufReadU8(src);        // aoa hardware
+#endif
 #ifdef USE_OPFLOW
             opticalFlowConfigMutable()->opflow_hardware = sbufReadU8(src);
 #else
@@ -4467,6 +4479,12 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src)
             break;
 #endif
 
+#if defined(USE_AOA_MSP)
+        case MSP2_SENSOR_AOA:
+            mspAoaReceiveNewData(sbufPtr(src));
+            break;
+#endif
+
 #if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_MSP))
         case MSP2_SENSOR_HEADTRACKER:
             mspHeadTrackerReceiverNewData(sbufPtr(src), dataSize);

src/main/fc/fc_tasks.c

@@ -91,6 +91,7 @@
 #include "sensors/pitotmeter.h"
 #include "sensors/rangefinder.h"
 #include "sensors/opflow.h"
+#include "sensors/aoa.h"
 
 #include "telemetry/telemetry.h"
 #include "telemetry/sbus2.h"
@@ -268,6 +269,21 @@ void taskUpdateOpticalFlow(timeUs_t currentTimeUs)
 }
 #endif
 
+#ifdef USE_AOA
+void taskUpdateAoa(timeUs_t currentTimeUs)
+{
+    if (!sensors(SENSOR_AOA))
+        return;
+
+    const uint32_t newDeadline = aoaUpdate();
+    if (newDeadline != 0) {
+        rescheduleTask(TASK_SELF, newDeadline);
+    }
+
+    aoaProcess();
+}
+#endif
+
 #ifdef USE_DASHBOARD
 void taskDashboardUpdate(timeUs_t currentTimeUs)
 {
@@ -414,6 +430,9 @@ void fcTasksInit(void)
 #ifdef USE_OPFLOW
     setTaskEnabled(TASK_OPFLOW, sensors(SENSOR_OPFLOW));
 #endif
+#ifdef USE_AOA
+    setTaskEnabled(TASK_AOA, sensors(SENSOR_AOA));
+#endif
 #ifdef USE_VTX_CONTROL
 #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
     setTaskEnabled(TASK_VTXCTRL, true);
@@ -671,7 +690,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_OPFLOW] = {
         .taskName = "OPFLOW",
         .taskFunc = taskUpdateOpticalFlow,
-        .desiredPeriod = TASK_PERIOD_HZ(100),   // I2C/SPI sensor will work at higher rate and accumulate, UART sensor will work at lower rate w/o accumulation
+        .desiredPeriod = TASK_PERIOD_HZ(100),
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+#ifdef USE_AOA
+    [TASK_AOA] = {
+        .taskName = "AOA",
+        .taskFunc = taskUpdateAoa,
+        .desiredPeriod = TASK_PERIOD_MS(100),
         .staticPriority = TASK_PRIORITY_MEDIUM,
     },
 #endif