Merge pull request #9347 from shota3527/sh_mixerprofile_tail

tail sitter vtol support

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

@@ -5782,6 +5782,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

src/main/fc/runtime_config.h

@@ -141,6 +141,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

@@ -1185,6 +1185,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
 
 
 

src/main/flight/imu.c

@@ -700,6 +700,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)
@@ -796,6 +823,7 @@ static void imuCalculateEstimatedAttitude(float dT)
                             useCOG, courseOverGround,
                             accWeight,
                             magWeight);
+    imuUpdateTailSitter();
     imuUpdateEulerAngles();
 }
 

src/main/flight/mixer.c

@@ -161,6 +161,7 @@ void mixerUpdateStateFlags(void)
     DISABLE_STATE(BOAT);
     DISABLE_STATE(AIRPLANE);
     DISABLE_STATE(MOVE_FORWARD_ONLY);
+    DISABLE_STATE(TAILSITTER);
 
     if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) {
         ENABLE_STATE(FIXED_WING_LEGACY);
@@ -186,6 +187,12 @@ void mixerUpdateStateFlags(void)
         ENABLE_STATE(ALTITUDE_CONTROL);
     }
 
+    if (currentMixerConfig.tailsitterOrientationOffset) {
+        ENABLE_STATE(TAILSITTER);
+    } else {
+        DISABLE_STATE(TAILSITTER);
+    }
+
     if (currentMixerConfig.hasFlaps) {
         ENABLE_STATE(FLAPERON_AVAILABLE);
     } else {

src/main/flight/mixer_profile.c

@@ -53,6 +53,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance)
                          .PIDProfileLinking = SETTING_MIXER_PID_PROFILE_LINKING_DEFAULT,
                          .automated_switch = SETTING_MIXER_AUTOMATED_SWITCH_DEFAULT,
                          .switchTransitionTimer =  SETTING_MIXER_SWITCH_TRANS_TIMER_DEFAULT,
+                         .tailsitterOrientationOffset = SETTING_TAILSITTER_ORIENTATION_OFFSET_DEFAULT,
                      });
         for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++)
         {

src/main/flight/mixer_profile.h

@@ -18,6 +18,7 @@ typedef struct mixerConfig_s {
     bool PIDProfileLinking;
     bool automated_switch;
     int16_t switchTransitionTimer;
+    bool tailsitterOrientationOffset;
 } mixerConfig_t;
 typedef struct mixerProfile_s {
     mixerConfig_t mixer_config;

src/main/flight/pid.c

@@ -586,7 +586,11 @@ int16_t angleFreefloatDeadband(int16_t deadband, flight_dynamics_index_t axis)
 
 static float computePidLevelTarget(flight_dynamics_index_t axis) {
     // This is ROLL/PITCH, run ANGLE/HORIZON controllers
+#ifdef USE_PROGRAMMING_FRAMEWORK
+    float angleTarget = pidRcCommandToAngle(getRcCommandOverride(rcCommand, axis), pidProfile()->max_angle_inclination[axis]);
+#else
     float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
+#endif
 
     // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
     if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
@@ -1142,7 +1146,6 @@ void FAST_CODE pidController(float dT)
     }
 
     for (int axis = 0; axis < 3; axis++) {
-        // Step 1: Calculate gyro rates
         pidState[axis].gyroRate = gyro.gyroADCf[axis];
 
         // Step 2: Read target
@@ -1179,6 +1182,11 @@ void FAST_CODE pidController(float dT)
         if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ANGLEHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) {
             // If axis angle override, get the correct angle from Logic Conditions
             float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis));
+
+            //apply 45 deg offset for tailsitter when isMixerTransitionMixing is activated
+            if (STATE(TAILSITTER) && isMixerTransitionMixing && axis == FD_PITCH){
+                angleTarget += DEGREES_TO_DECIDEGREES(45);
+            }
 
             if (STATE(AIRPLANE)) {  // update anglehold mode
                 updateAngleHold(&angleTarget, axis);

src/main/sensors/boardalignment.c

@@ -28,6 +28,7 @@
 
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
+#include "fc/runtime_config.h"
 
 #include "drivers/sensor.h"
 
@@ -45,6 +46,7 @@
 
 static bool standardBoardAlignment = true;     // board orientation correction
 static fpMat3_t boardRotMatrix;
+static fpMat3_t tailRotMatrix;
 
 // no template required since defaults are zero
 PG_REGISTER(boardAlignment_t, boardAlignment, PG_BOARD_ALIGNMENT, 0);
@@ -56,19 +58,19 @@ static bool isBoardAlignmentStandard(const boardAlignment_t *boardAlignment)
 
 void initBoardAlignment(void)
 {
-    if (isBoardAlignmentStandard(boardAlignment())) {
-        standardBoardAlignment = true;
-    } else {
-        fp_angles_t rotationAngles;
-
-        standardBoardAlignment = false;
-
-        rotationAngles.angles.roll  = DECIDEGREES_TO_RADIANS(boardAlignment()->rollDeciDegrees );
-        rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(boardAlignment()->pitchDeciDegrees);
-        rotationAngles.angles.yaw   = DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees  );
-
-        rotationMatrixFromAngles(&boardRotMatrix, &rotationAngles);
-    }
+    standardBoardAlignment=isBoardAlignmentStandard(boardAlignment());
+    fp_angles_t rotationAngles;
+
+    rotationAngles.angles.roll  = DECIDEGREES_TO_RADIANS(boardAlignment()->rollDeciDegrees );
+    rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(boardAlignment()->pitchDeciDegrees);
+    rotationAngles.angles.yaw   = DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees  );
+
+    rotationMatrixFromAngles(&boardRotMatrix, &rotationAngles);
+    fp_angles_t tailSitter_rotationAngles;
+    tailSitter_rotationAngles.angles.roll  = DECIDEGREES_TO_RADIANS(0);
+    tailSitter_rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(900);
+    tailSitter_rotationAngles.angles.yaw   = DECIDEGREES_TO_RADIANS(0);
+    rotationMatrixFromAngles(&tailRotMatrix, &tailSitter_rotationAngles);
 }
 
 void updateBoardAlignment(int16_t roll, int16_t pitch)
@@ -85,15 +87,23 @@ void updateBoardAlignment(int16_t roll, int16_t pitch)
     initBoardAlignment();
 }
 
+void applyTailSitterAlignment(fpVector3_t *fpVec)
+{
+    if (!STATE(TAILSITTER)) {
+        return;
+    }
+    rotationMatrixRotateVector(fpVec, fpVec, &tailRotMatrix);
+}
+
 void applyBoardAlignment(float *vec)
 {
-    if (standardBoardAlignment) {
+    if (standardBoardAlignment && (!STATE(TAILSITTER))) {
         return;
     }
 
     fpVector3_t fpVec = { .v = { vec[X], vec[Y], vec[Z] } };
     rotationMatrixRotateVector(&fpVec, &fpVec, &boardRotMatrix);
-
+    applyTailSitterAlignment(&fpVec);
     vec[X] = lrintf(fpVec.x);
     vec[Y] = lrintf(fpVec.y);
     vec[Z] = lrintf(fpVec.z);

src/main/sensors/boardalignment.h

@@ -18,6 +18,7 @@
 #pragma once
 
 #include "config/parameter_group.h"
+#include "common/vector.h"
 
 typedef struct boardAlignment_s {
     int16_t rollDeciDegrees;
@@ -30,4 +31,5 @@ PG_DECLARE(boardAlignment_t, boardAlignment);
 void initBoardAlignment(void);
 void updateBoardAlignment(int16_t roll, int16_t pitch);
 void applySensorAlignment(float * dest, float * src, uint8_t rotation);
-void applyBoardAlignment(float *vec);
+void applyBoardAlignment(float *vec);
+void applyTailSitterAlignment(fpVector3_t *vec);

src/main/sensors/compass.c

@@ -472,7 +472,7 @@ void compassUpdate(timeUs_t currentTimeUs)
         fpVector3_t rotated;
 
         rotationMatrixRotateVector(&rotated, &v, &mag.dev.magAlign.externalRotation);
-
+        applyTailSitterAlignment(&rotated);
          mag.magADC[X] = rotated.x;
          mag.magADC[Y] = rotated.y;
          mag.magADC[Z] = rotated.z;