Merge pull request iNavFlight#9355 from error414/adsb

Adsb support pingRX  / Aerobit TT-SC1

Author: DzikuVx

.gitignore

@@ -35,3 +35,4 @@ make/local.mk
 launch.json
 .vscode/tasks.json
 .vscode/c_cpp_properties.json
+/cmake-build-debug/

docs/ADSB.md

@@ -0,0 +1,17 @@
+# ADS-B
+
+[Automatic Dependent Surveillance Broadcast](https://en.wikipedia.org/wiki/Automatic_Dependent_Surveillance%E2%80%93Broadcast)
+is an air traffic surveillance technology that enables aircraft to be accurately tracked by air traffic controllers and other pilots without the need for conventional radar.
+
+## Current state
+
+OSD can be configured to shows the closest aircraft.
+
+## Hardware
+
+All ADSB receivers which can send Mavlink [ADSB_VEHICLE](https://mavlink.io/en/messages/common.html#ADSB_VEHICLE) message are supported 
+
+* [PINGRX](https://uavionix.com/product/pingrx-pro/) (not tested)
+* [TT-SC1](https://www.aerobits.pl/product/aero/) (tested)
+
+

docs/MixerProfile.md

@@ -104,6 +104,9 @@ TailSitter is supported by add a 90deg offset to the board alignment. Set the bo
   - Rate Settings
   - ·······
 
+### TailSitter support
+TailSitter is supported by add a 90deg offset to the board alignment. Set the board aliment normally in the mixer_profile for FW mode(`set platform_type = AIRPLANE`), The motor trust axis should be same direction as the airplane nose. Then, in the mixer_profile for takeoff and landing `set platform_type = TAILSITTER`. The `TAILSITTER` platform type is same as `MULTIROTOR` platform type, expect for a 90 deg board alignment offset. In `TAILSITTER` mixer_profile, when motor trust/airplane nose is pointing to the sky, 'airplane bottom'/'multi rotor front' should facing forward in model preview. Set the motor/servo mixer according to multirotor orientation, Model should roll around geography's longitudinal axis, the roll axis of `TAILSITTER` will be yaw axis of `AIRPLANE`. In addition, When `MIXER TRANSITION` input is activated, a 45deg offset will be add to the target angle for angle mode.
+
 ## Happy flying
 
 Remember that this is currently an emerging capability:

docs/Settings.md

@@ -4032,6 +4032,36 @@ _// TODO_
 
 ---
 
+### osd_adsb_distance_alert
+
+Distance inside which ADSB data flashes for proximity warning
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 3000 | 1 | 64000 |
+
+---
+
+### osd_adsb_distance_warning
+
+Distance in meters of ADSB aircraft that is displayed
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 20000 | 1 | 64000 |
+
+---
+
+### osd_adsb_ignore_plane_above_me_limit
+
+Ignore adsb planes above, limit, 0 disabled (meters)
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 64000 |
+
+---
+
 ### osd_ahi_bordered
 
 Shows a border/corners around the AHI region (pixel OSD only)
@@ -5812,6 +5842,16 @@ Delay before disarming when requested by switch (ms) [0-1000]
 
 ---
 
+### tailsitter_orientation_offset
+
+Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### telemetry_halfduplex
 
 S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details

lib/main/STM32F4/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c

@@ -343,6 +343,12 @@ static int8_t SCSI_ModeSense6 (USBD_HandleTypeDef  *pdev, uint8_t lun, uint8_t *
     len--;
     hmsc->bot_data[len] = MSC_Mode_Sense6_data[len];
   }
+
+  // set bit 7 of the device configuration byte to indicate write protection
+  if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) {
+      hmsc->bot_data[2] = hmsc->bot_data[2] | (1 << 7);
+  }
+
   return 0;
 }
 
@@ -365,6 +371,12 @@ static int8_t SCSI_ModeSense10 (USBD_HandleTypeDef  *pdev, uint8_t lun, uint8_t
     len--;
     hmsc->bot_data[len] = MSC_Mode_Sense10_data[len];
   }
+
+  // set bit 7 of the device configuration byte to indicate write protection
+  if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) {
+      hmsc->bot_data[3] = hmsc->bot_data[3] | (1 << 7);
+  }
+
   return 0;
 }
 

lib/main/STM32F7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c

@@ -347,6 +347,12 @@ static int8_t SCSI_ModeSense6 (USBD_HandleTypeDef  *pdev, uint8_t lun, uint8_t *
     len--;
     hmsc->bot_data[len] = MSC_Mode_Sense6_data[len];
   }
+
+  // set bit 7 of the device configuration byte to indicate write protection
+  if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) {
+      hmsc->bot_data[2] = hmsc->bot_data[2] | (1 << 7);
+  }
+
   return 0;
 }
 
@@ -370,6 +376,12 @@ static int8_t SCSI_ModeSense10 (USBD_HandleTypeDef  *pdev, uint8_t lun, uint8_t
     len--;
     hmsc->bot_data[len] = MSC_Mode_Sense10_data[len];
   }
+
+  // set bit 7 of the device configuration byte to indicate write protection
+  if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) {
+      hmsc->bot_data[3] = hmsc->bot_data[3] | (1 << 7);
+  }
+
   return 0;
 }
 

lib/main/STM32H7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c

@@ -346,6 +346,12 @@ static int8_t SCSI_ModeSense6(USBD_HandleTypeDef  *pdev, uint8_t lun, uint8_t *p
     len--;
     hmsc->bot_data[len] = MSC_Mode_Sense6_data[len];
   }
+
+  // set bit 7 of the device configuration byte to indicate write protection
+  if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) {
+      hmsc->bot_data[2] = hmsc->bot_data[2] | (1 << 7);
+  }
+
   return 0;
 }
 
@@ -371,6 +377,11 @@ static int8_t SCSI_ModeSense10(USBD_HandleTypeDef  *pdev, uint8_t lun, uint8_t *
     hmsc->bot_data[len] = MSC_Mode_Sense10_data[len];
   }
 
+  // set bit 7 of the device configuration byte to indicate write protection
+  if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) {
+      hmsc->bot_data[3] = hmsc->bot_data[3] | (1 << 7);
+  }
+
   return 0;
 }
 

src/main/CMakeLists.txt

@@ -344,6 +344,7 @@ main_sources(COMMON_SRC
     flight/ez_tune.c
     flight/ez_tune.h
 
+    io/adsb.c
     io/beeper.c
     io/beeper.h
     io/servo_sbus.c

src/main/drivers/osd_symbols.h

@@ -180,6 +180,8 @@
 #define SYM_CROSS_TRACK_ERROR       0xFC  // 252 Cross track error
 
 
+#define SYM_ADSB                    0xFD // 253 ADSB
+
 #define SYM_LOGO_START              0x101 // 257 to 297, INAV logo
 #define SYM_LOGO_WIDTH              10
 #define SYM_LOGO_HEIGHT             4

src/main/fc/fc_msp.c

@@ -83,6 +83,7 @@
 #include "config/config_eeprom.h"
 #include "config/feature.h"
 
+#include "io/adsb.h"
 #include "io/asyncfatfs/asyncfatfs.h"
 #include "io/flashfs.h"
 #include "io/gps.h"
@@ -948,6 +949,33 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, gpsSol.epv);
         break;
 #endif
+    case MSP2_ADSB_VEHICLE_LIST:
+#ifdef USE_ADSB
+        sbufWriteU8(dst, MAX_ADSB_VEHICLES);
+        sbufWriteU8(dst, ADSB_CALL_SIGN_MAX_LENGTH);
+
+        for(uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++){
+
+            adsbVehicle_t *adsbVehicle = findVehicle(i);
+
+            for(uint8_t ii = 0; ii < ADSB_CALL_SIGN_MAX_LENGTH; ii++){
+                sbufWriteU8(dst, adsbVehicle->vehicleValues.callsign[ii]);
+            }
+
+            sbufWriteU32(dst, adsbVehicle->vehicleValues.icao);
+            sbufWriteU32(dst, adsbVehicle->vehicleValues.lat);
+            sbufWriteU32(dst, adsbVehicle->vehicleValues.lon);
+            sbufWriteU32(dst, adsbVehicle->vehicleValues.alt);
+            sbufWriteU16(dst, (uint16_t)CENTIDEGREES_TO_DEGREES(adsbVehicle->vehicleValues.heading));
+            sbufWriteU8(dst,  adsbVehicle->vehicleValues.tslc);
+            sbufWriteU8(dst,  adsbVehicle->vehicleValues.emitterType);
+            sbufWriteU8(dst,  adsbVehicle->ttl);
+        }
+#else
+        sbufWriteU8(dst, 0);
+        sbufWriteU8(dst, 0);
+#endif
+            break;
     case MSP_DEBUG:
         // output some useful QA statistics
         // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
@@ -1518,6 +1546,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 #else
         sbufWriteU16(dst, 0);
         sbufWriteU16(dst, 0);
+#endif
+#ifdef USE_ADSB
+        sbufWriteU16(dst, osdConfig()->adsb_distance_warning);
+        sbufWriteU16(dst, osdConfig()->adsb_distance_alert);
+#else
+        sbufWriteU16(dst, 0);
+        sbufWriteU16(dst, 0);
 #endif
         break;
 

src/main/fc/fc_tasks.c

@@ -69,6 +69,7 @@
 #include "io/osd_dji_hd.h"
 #include "io/displayport_msp_osd.h"
 #include "io/servo_sbus.h"
+#include "io/adsb.h"
 
 #include "msp/msp_serial.h"
 
@@ -181,6 +182,14 @@ void taskUpdateCompass(timeUs_t currentTimeUs)
 }
 #endif
 
+#ifdef USE_ADSB
+void taskAdsb(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    adsbTtlClean(currentTimeUs);
+}
+#endif
+
 #ifdef USE_BARO
 void taskUpdateBaro(timeUs_t currentTimeUs)
 {
@@ -360,6 +369,9 @@ void fcTasksInit(void)
 #ifdef USE_PITOT
     setTaskEnabled(TASK_PITOT, sensors(SENSOR_PITOT));
 #endif
+#ifdef USE_ADSB
+    setTaskEnabled(TASK_ADSB, true);
+#endif
 #ifdef USE_RANGEFINDER
     setTaskEnabled(TASK_RANGEFINDER, sensors(SENSOR_RANGEFINDER));
 #endif
@@ -495,6 +507,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#ifdef USE_ADSB
+        [TASK_ADSB] = {
+        .taskName = "ADSB",
+        .taskFunc = taskAdsb,
+        .desiredPeriod = TASK_PERIOD_HZ(1),      // ADSB is updated at 1 Hz
+        .staticPriority = TASK_PRIORITY_IDLE,
+    },
+#endif
+
 #ifdef USE_BARO
     [TASK_BARO] = {
         .taskName = "BARO",

src/main/fc/runtime_config.h

@@ -144,6 +144,7 @@ typedef enum {
     ANTI_WINDUP_DEACTIVATED             = (1 << 25),
     LANDING_DETECTED                    = (1 << 26),
     IN_FLIGHT_EMERG_REARM               = (1 << 27),
+    TAILSITTER                          = (1 << 28), //offset the pitch angle by 90 degrees
 } stateFlags_t;
 
 #define DISABLE_STATE(mask) (stateFlags &= ~(mask))

src/main/fc/settings.yaml

@@ -1191,6 +1191,11 @@ groups:
         field: mixer_config.switchTransitionTimer
         min: 0
         max: 200
+      - name: tailsitter_orientation_offset
+        description: "Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode"
+        default_value: OFF
+        field: mixer_config.tailsitterOrientationOffset
+        type: bool
 
 
 
@@ -3457,6 +3462,31 @@ groups:
         max: 11
         default_value: 9
 
+      - name: osd_adsb_distance_warning
+        description: "Distance in meters of ADSB aircraft that is displayed"
+        default_value: 20000
+        condition: USE_ADSB
+        field: adsb_distance_warning
+        min: 1
+        max: 64000
+        type: uint16_t
+      - name: osd_adsb_distance_alert
+        description: "Distance inside which ADSB data flashes for proximity warning"
+        default_value: 3000
+        condition: USE_ADSB
+        field: adsb_distance_alert
+        min: 1
+        max: 64000
+        type: uint16_t
+      - name: osd_adsb_ignore_plane_above_me_limit
+        description: "Ignore adsb planes above, limit, 0 disabled (meters)"
+        default_value: 0
+        condition: USE_ADSB
+        field: adsb_ignore_plane_above_me_limit
+        min: 0
+        max: 64000
+        type: uint16_t
+
       - name: osd_estimations_wind_compensation
         description: "Use wind estimation for remaining flight time/distance estimation"
         default_value: ON

src/main/flight/imu.c

@@ -704,6 +704,33 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF
     lastspeed = currentspeed;
 }
 
+fpQuaternion_t* getTailSitterQuaternion(bool normal2tail){
+    static bool firstRun = true;
+    static fpQuaternion_t qNormal2Tail;
+    static fpQuaternion_t qTail2Normal;
+    if(firstRun){
+        fpAxisAngle_t axisAngle;
+        axisAngle.axis.x = 0;
+        axisAngle.axis.y = 1;
+        axisAngle.axis.z = 0;
+        axisAngle.angle = DEGREES_TO_RADIANS(-90);
+        axisAngleToQuaternion(&qNormal2Tail, &axisAngle);
+        quaternionConjugate(&qTail2Normal, &qNormal2Tail);
+        firstRun = false;
+    }
+    return normal2tail ? &qNormal2Tail : &qTail2Normal;
+}
+
+void imuUpdateTailSitter(void)
+{
+    static bool lastTailSitter=false;
+    if (((bool)STATE(TAILSITTER)) != lastTailSitter){
+        fpQuaternion_t* rotation_for_tailsitter= getTailSitterQuaternion(STATE(TAILSITTER));
+        quaternionMultiply(&orientation, &orientation, rotation_for_tailsitter);
+    }
+    lastTailSitter = STATE(TAILSITTER);
+}
+
 static void imuCalculateEstimatedAttitude(float dT)
 {
 #if defined(USE_MAG)
@@ -800,6 +827,7 @@ static void imuCalculateEstimatedAttitude(float dT)
                             useCOG, courseOverGround,
                             accWeight,
                             magWeight);
+    imuUpdateTailSitter();
     imuUpdateEulerAngles();
 }