From 380ef16f799aca27b819aa9fc8f2a26ebac56d39 Mon Sep 17 00:00:00 2001 From: shota Date: Thu, 10 Aug 2023 01:07:48 +0900 Subject: [PATCH 01/14] trial --- src/main/drivers/pwm_mapping.c | 2 +- src/main/fc/runtime_config.h | 1 + src/main/fc/settings.yaml | 2 +- src/main/flight/mixer.c | 5 +++++ src/main/flight/mixer.h | 3 ++- src/main/flight/mixer_profile.c | 4 ++-- src/main/flight/pid.c | 12 +++++++++++- src/main/flight/servos.c | 2 +- src/main/telemetry/mavlink.c | 3 +++ 9 files changed, 27 insertions(+), 7 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index a40faf965cc..c8121830307 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -250,7 +250,7 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU } // Determine if timer belongs to motor/servo - if (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER) { + if (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER || currentMixerConfig.platformType == PLATFORM_TAILSITTER) { // Multicopter // Make sure first motorCount outputs get assigned to motor diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 16d1b411df8..8764ea9f824 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -139,6 +139,7 @@ typedef enum { FW_HEADING_USE_YAW = (1 << 24), ANTI_WINDUP_DEACTIVATED = (1 << 25), LANDING_DETECTED = (1 << 26), + TAILSITTER = (1 << 27), //offset the pitch angle and swap roll/yaw controls } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 80540cc3968..3eb8ac78630 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -111,7 +111,7 @@ tables: values: ["PERCENT", "MAH", "MWH"] enum: smartportFuelUnit_e - name: platform_type - values: ["MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT"] + values: ["MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT", "TAILSITTER"] - name: output_mode values: ["AUTO", "MOTORS", "SERVOS"] enum: outputMode_e diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index ab8a701bc74..a377b381bda 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -152,6 +152,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); @@ -172,6 +173,10 @@ void mixerUpdateStateFlags(void) } else if (currentMixerConfig.platformType == PLATFORM_TRICOPTER) { ENABLE_STATE(MULTIROTOR); ENABLE_STATE(ALTITUDE_CONTROL); + } else if (currentMixerConfig.platformType == PLATFORM_TAILSITTER) { + ENABLE_STATE(MULTIROTOR); + ENABLE_STATE(ALTITUDE_CONTROL); + ENABLE_STATE(TAILSITTER); } else if (currentMixerConfig.platformType == PLATFORM_HELICOPTER) { ENABLE_STATE(MULTIROTOR); ENABLE_STATE(ALTITUDE_CONTROL); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 871332447ef..ee7f29686ce 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -39,7 +39,8 @@ typedef enum { PLATFORM_TRICOPTER = 3, PLATFORM_ROVER = 4, PLATFORM_BOAT = 5, - PLATFORM_OTHER = 6 + PLATFORM_OTHER = 6, + PLATFORM_TAILSITTER = 7, } flyingPlatformType_e; diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 044cb027abd..9afd5686355 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -200,8 +200,8 @@ bool checkMixerProfileHotSwitchAvalibility(void) #endif uint8_t platform_type0 = mixerConfigByIndex(0)->platformType; uint8_t platform_type1 = mixerConfigByIndex(1)->platformType; - bool platform_type_mc0 = (platform_type0 == PLATFORM_MULTIROTOR) || (platform_type0 == PLATFORM_TRICOPTER); - bool platform_type_mc1 = (platform_type1 == PLATFORM_MULTIROTOR) || (platform_type1 == PLATFORM_TRICOPTER); + bool platform_type_mc0 = (platform_type0 == PLATFORM_MULTIROTOR) || (platform_type0 == PLATFORM_TRICOPTER || (platform_type0 == PLATFORM_TAILSITTER)); + bool platform_type_mc1 = (platform_type1 == PLATFORM_MULTIROTOR) || (platform_type1 == PLATFORM_TRICOPTER || (platform_type1 == PLATFORM_TAILSITTER)); bool is_mcfw_switching = platform_type_mc0 ^ platform_type_mc1; if ((!MCFW_hotswap_available) && is_mcfw_switching) { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f8728a3cefd..6d0931742fa 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1085,8 +1085,18 @@ void FAST_CODE pidController(float dT) } for (int axis = 0; axis < 3; axis++) { + int gyro_axis = axis; + // float noinvert=1.0f; + // if (STATE(TAILSITTER)){ + // if (axis == FD_ROLL) { + // gyro_axis = FD_YAW; + // } else if (axis == FD_YAW) { + // gyro_axis = FD_ROLL; + // noinvert=-1.0f; + // } + // } // Step 1: Calculate gyro rates - pidState[axis].gyroRate = gyro.gyroADCf[axis]; + pidState[axis].gyroRate = gyro.gyroADCf[gyro_axis] * noinvert; // Step 2: Read target float rateTarget; diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 4d4bb497d19..485f8d3684b 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -264,7 +264,7 @@ void servoMixer(float dT) // Reverse yaw servo when inverted in 3D mode only for multirotor and tricopter if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < PWM_RANGE_MIDDLE) && - (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER)) { + (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER || currentMixerConfig.platformType == PLATFORM_TAILSITTER)) { input[INPUT_STABILIZED_YAW] *= -1; } } diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 30237c28552..a611e6def9d 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -684,6 +684,9 @@ void mavlinkSendHUDAndHeartbeat(void) case PLATFORM_TRICOPTER: mavSystemType = MAV_TYPE_TRICOPTER; break; + case PLATFORM_TAILSITTER: + mavSystemType = MAV_TYPE_QUADROTOR; + break; case PLATFORM_AIRPLANE: mavSystemType = MAV_TYPE_FIXED_WING; break; From 20bfb88094637d75c6690a5f3ee36ee6803614ba Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 11 Aug 2023 02:09:09 +0900 Subject: [PATCH 02/14] initial cut on tail sitter support --- src/main/fc/runtime_config.h | 2 +- src/main/fc/settings.yaml | 2 +- src/main/flight/imu.c | 28 ++++++++++++++++++++++++++++ src/main/flight/mixer.h | 4 ++-- src/main/flight/pid.c | 13 +------------ src/main/sensors/boardalignment.c | 8 +++++++- 6 files changed, 40 insertions(+), 17 deletions(-) diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 8764ea9f824..94eb23bd6a6 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -139,7 +139,7 @@ typedef enum { FW_HEADING_USE_YAW = (1 << 24), ANTI_WINDUP_DEACTIVATED = (1 << 25), LANDING_DETECTED = (1 << 26), - TAILSITTER = (1 << 27), //offset the pitch angle and swap roll/yaw controls + TAILSITTER = (1 << 27), //offset the pitch angle by 90 degrees } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 3eb8ac78630..0f1ccc22922 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1142,7 +1142,7 @@ groups: field: mixer_config.motorDirectionInverted type: bool - name: platform_type - description: "Defines UAV platform type. Allowed values: \"MULTIROTOR\", \"AIRPLANE\", \"HELICOPTER\", \"TRICOPTER\", \"ROVER\", \"BOAT\". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented" + description: "Defines UAV platform type. Allowed values: \"MULTIROTOR\", \"AIRPLANE\", \"HELICOPTER\", \"TRICOPTER\", \"ROVER\", \"BOAT\", \"TAILSITTER\". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented" default_value: "MULTIROTOR" field: mixer_config.platformType type: uint8_t diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index c9d0f0aacbf..3ee0d303b6a 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -660,6 +660,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) @@ -766,6 +793,7 @@ static void imuCalculateEstimatedAttitude(float dT) useCOG, courseOverGround, accWeight, magWeight); + imuUpdateTailSitter(); imuUpdateEulerAngles(); } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index ee7f29686ce..10c1878b3a2 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -39,8 +39,8 @@ typedef enum { PLATFORM_TRICOPTER = 3, PLATFORM_ROVER = 4, PLATFORM_BOAT = 5, - PLATFORM_OTHER = 6, - PLATFORM_TAILSITTER = 7, + PLATFORM_TAILSITTER = 6, + PLATFORM_OTHER = 7, } flyingPlatformType_e; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6d0931742fa..0a3bcde4b82 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1085,18 +1085,7 @@ void FAST_CODE pidController(float dT) } for (int axis = 0; axis < 3; axis++) { - int gyro_axis = axis; - // float noinvert=1.0f; - // if (STATE(TAILSITTER)){ - // if (axis == FD_ROLL) { - // gyro_axis = FD_YAW; - // } else if (axis == FD_YAW) { - // gyro_axis = FD_ROLL; - // noinvert=-1.0f; - // } - // } - // Step 1: Calculate gyro rates - pidState[axis].gyroRate = gyro.gyroADCf[gyro_axis] * noinvert; + pidState[axis].gyroRate = gyro.gyroADCf[axis]; // Step 2: Read target float rateTarget; diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index 86e41880f89..d23293dcce6 100644 --- a/src/main/sensors/boardalignment.c +++ b/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" @@ -87,7 +88,7 @@ void updateBoardAlignment(int16_t roll, int16_t pitch) void applyBoardAlignment(float *vec) { - if (standardBoardAlignment) { + if (standardBoardAlignment && (!STATE(TAILSITTER))) { return; } @@ -97,6 +98,11 @@ void applyBoardAlignment(float *vec) vec[X] = lrintf(fpVec.x); vec[Y] = lrintf(fpVec.y); vec[Z] = lrintf(fpVec.z); + + if (STATE(TAILSITTER)) { + vec[X] = lrintf(fpVec.z); + vec[Z] = -lrintf(fpVec.x); + } } void FAST_CODE applySensorAlignment(float * dest, float * src, uint8_t rotation) From 34f6690e903d48c10888d785f04eb54e7f19f84f Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 11 Aug 2023 13:21:10 +0900 Subject: [PATCH 03/14] tailsitter support --- src/main/flight/imu.c | 2 +- src/main/flight/pid.c | 5 +++++ src/main/sensors/boardalignment.c | 15 +++++++++------ 3 files changed, 15 insertions(+), 7 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 3ee0d303b6a..939f9c4ee7e 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -669,7 +669,7 @@ fpQuaternion_t* getTailSitterQuaternion(bool normal2tail){ axisAngle.axis.x = 0; axisAngle.axis.y = 1; axisAngle.axis.z = 0; - axisAngle.angle = DEGREES_TO_RADIANS(90); + axisAngle.angle = DEGREES_TO_RADIANS(-90); axisAngleToQuaternion(&qNormal2Tail, &axisAngle); quaternionConjugate(&qTail2Normal, &qNormal2Tail); firstRun = false; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0a3bcde4b82..db24091acb6 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1119,6 +1119,11 @@ void FAST_CODE pidController(float dT) if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_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); + } //Apply the Level PID controller pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT); diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index d23293dcce6..ccad08163af 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -46,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); @@ -70,6 +71,11 @@ void initBoardAlignment(void) rotationMatrixFromAngles(&boardRotMatrix, &rotationAngles); } + fp_angles_t rotationAngles; + rotationAngles.angles.roll = DECIDEGREES_TO_RADIANS(0); + rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(900); + rotationAngles.angles.yaw = DECIDEGREES_TO_RADIANS(0); + rotationMatrixFromAngles(&tailRotMatrix, &rotationAngles); } void updateBoardAlignment(int16_t roll, int16_t pitch) @@ -94,15 +100,12 @@ void applyBoardAlignment(float *vec) fpVector3_t fpVec = { .v = { vec[X], vec[Y], vec[Z] } }; rotationMatrixRotateVector(&fpVec, &fpVec, &boardRotMatrix); - + if (STATE(TAILSITTER)) { + rotationMatrixRotateVector(&fpVec, &fpVec, &tailRotMatrix); + } vec[X] = lrintf(fpVec.x); vec[Y] = lrintf(fpVec.y); vec[Z] = lrintf(fpVec.z); - - if (STATE(TAILSITTER)) { - vec[X] = lrintf(fpVec.z); - vec[Z] = -lrintf(fpVec.x); - } } void FAST_CODE applySensorAlignment(float * dest, float * src, uint8_t rotation) From 084020335c8c4763c8f360982a9898bf0ef2bee0 Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 12 Aug 2023 15:41:10 +0900 Subject: [PATCH 04/14] update tailsitter documents --- docs/MixerProfile.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index b30753e631a..432861009bf 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -204,6 +204,9 @@ save When `mixer_switch_on_rth`:`OFF` and `mixer_switch_on_land`:`OFF` is set for all mixer_profiles(defaults). Model will not perform automated transition at all. +### 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: From 378664c322f5eb225d96127b3e67111adc5dd5fe Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 29 Sep 2023 00:09:07 +0900 Subject: [PATCH 05/14] add compass changing aliment --- src/main/sensors/boardalignment.c | 12 +++++++++--- src/main/sensors/boardalignment.h | 4 +++- src/main/sensors/compass.c | 2 +- 3 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index ccad08163af..360ddec4a16 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -92,6 +92,14 @@ 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 && (!STATE(TAILSITTER))) { @@ -100,9 +108,7 @@ void applyBoardAlignment(float *vec) fpVector3_t fpVec = { .v = { vec[X], vec[Y], vec[Z] } }; rotationMatrixRotateVector(&fpVec, &fpVec, &boardRotMatrix); - if (STATE(TAILSITTER)) { - rotationMatrixRotateVector(&fpVec, &fpVec, &tailRotMatrix); - } + applyTailSitterAlignment(&fpVec); vec[X] = lrintf(fpVec.x); vec[Y] = lrintf(fpVec.y); vec[Z] = lrintf(fpVec.z); diff --git a/src/main/sensors/boardalignment.h b/src/main/sensors/boardalignment.h index 6bf01272650..17cd08e5ff3 100644 --- a/src/main/sensors/boardalignment.h +++ b/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); \ No newline at end of file +void applyBoardAlignment(float *vec); +void applyTailSitterAlignment(fpVector3_t *vec); \ No newline at end of file diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 23d45ed7879..734a1e98224 100644 --- a/src/main/sensors/compass.c +++ b/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; From 1ea7aa7e48b5c9cc059855f44755b1978801b670 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 8 Oct 2023 00:59:46 +0900 Subject: [PATCH 06/14] moved tailsitter platform type to tailsitter_board_orientation --- src/main/fc/settings.yaml | 9 +++++++-- src/main/flight/mixer.c | 10 ++++++---- src/main/flight/mixer.h | 3 +-- src/main/flight/mixer_profile.c | 1 + src/main/flight/mixer_profile.h | 1 + src/main/flight/servos.c | 2 +- src/main/telemetry/mavlink.c | 3 --- 7 files changed, 17 insertions(+), 12 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index bee6149147b..d3723d05af7 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -111,7 +111,7 @@ tables: values: ["PERCENT", "MAH", "MWH"] enum: smartportFuelUnit_e - name: platform_type - values: ["MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT", "TAILSITTER"] + values: ["MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT"] - name: tz_automatic_dst values: ["OFF", "EU", "USA"] enum: tz_automatic_dst_e @@ -1145,7 +1145,7 @@ groups: field: mixer_config.motorDirectionInverted type: bool - name: platform_type - description: "Defines UAV platform type. Allowed values: \"MULTIROTOR\", \"AIRPLANE\", \"HELICOPTER\", \"TRICOPTER\", \"ROVER\", \"BOAT\", \"TAILSITTER\". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented" + description: "Defines UAV platform type. Allowed values: \"MULTIROTOR\", \"AIRPLANE\", \"HELICOPTER\", \"TRICOPTER\", \"ROVER\", \"BOAT\". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented" default_value: "MULTIROTOR" field: mixer_config.platformType type: uint8_t @@ -1182,6 +1182,11 @@ groups: field: mixer_config.switchTransitionTimer min: 0 max: 200 + - name: tailsitter_board_orientation + description: "If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch." + default_value: OFF + field: mixer_config.tailsitterBoardOrientation + type: bool diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d0d93a3348a..874f48c5c70 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -182,15 +182,17 @@ void mixerUpdateStateFlags(void) } else if (currentMixerConfig.platformType == PLATFORM_TRICOPTER) { ENABLE_STATE(MULTIROTOR); ENABLE_STATE(ALTITUDE_CONTROL); - } else if (currentMixerConfig.platformType == PLATFORM_TAILSITTER) { - ENABLE_STATE(MULTIROTOR); - ENABLE_STATE(ALTITUDE_CONTROL); - ENABLE_STATE(TAILSITTER); } else if (currentMixerConfig.platformType == PLATFORM_HELICOPTER) { ENABLE_STATE(MULTIROTOR); ENABLE_STATE(ALTITUDE_CONTROL); } + if (currentMixerConfig.tailsitterBoardOrientation) { + ENABLE_STATE(TAILSITTER); + } else { + DISABLE_STATE(TAILSITTER); + } + if (currentMixerConfig.hasFlaps) { ENABLE_STATE(FLAPERON_AVAILABLE); } else { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index ba0a856be6e..9ee6a20654e 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -41,8 +41,7 @@ typedef enum { PLATFORM_TRICOPTER = 3, PLATFORM_ROVER = 4, PLATFORM_BOAT = 5, - PLATFORM_TAILSITTER = 6, - PLATFORM_OTHER = 7, + PLATFORM_OTHER = 6 } flyingPlatformType_e; diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index de1cdecb3bd..de347feed3e 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -52,6 +52,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, + .tailsitterBoardOrientation = SETTING_TAILSITTER_BOARD_ORIENTATION_DEFAULT, }); for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) { diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index bf52d45c3d9..073e256eb1e 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -18,6 +18,7 @@ typedef struct mixerConfig_s { bool PIDProfileLinking; bool automated_switch; int16_t switchTransitionTimer; + bool tailsitterBoardOrientation; } mixerConfig_t; typedef struct mixerProfile_s { mixerConfig_t mixer_config; diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 485f8d3684b..4d4bb497d19 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -264,7 +264,7 @@ void servoMixer(float dT) // Reverse yaw servo when inverted in 3D mode only for multirotor and tricopter if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < PWM_RANGE_MIDDLE) && - (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER || currentMixerConfig.platformType == PLATFORM_TAILSITTER)) { + (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER)) { input[INPUT_STABILIZED_YAW] *= -1; } } diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index a611e6def9d..30237c28552 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -684,9 +684,6 @@ void mavlinkSendHUDAndHeartbeat(void) case PLATFORM_TRICOPTER: mavSystemType = MAV_TYPE_TRICOPTER; break; - case PLATFORM_TAILSITTER: - mavSystemType = MAV_TYPE_QUADROTOR; - break; case PLATFORM_AIRPLANE: mavSystemType = MAV_TYPE_FIXED_WING; break; From 4fd677245844eef0bd3bb2153dce8ed3f3af81a1 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 8 Oct 2023 01:35:46 +0900 Subject: [PATCH 07/14] bug fixes --- docs/Settings.md | 12 +++++++++++- src/main/sensors/boardalignment.c | 29 ++++++++++++----------------- 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index bb26b36f5fd..74cd5d52b8c 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4894,7 +4894,7 @@ _// TODO_ ### platform_type -Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT", "TAILSITTER". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented +Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented | Default | Min | Max | | --- | --- | --- | @@ -5572,6 +5572,16 @@ Delay before disarming when requested by switch (ms) [0-1000] --- +### tailsitter_board_orientation + +If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch. + +| 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 diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index 360ddec4a16..1f149faeff6 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -58,24 +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(0); - rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(900); - rotationAngles.angles.yaw = DECIDEGREES_TO_RADIANS(0); - rotationMatrixFromAngles(&tailRotMatrix, &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) From b5e3cb28d4a606ac18a9f7d2ab2e74e4ee0c9699 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 8 Oct 2023 01:38:16 +0900 Subject: [PATCH 08/14] update docs --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 74cd5d52b8c..67e4b554652 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5574,7 +5574,7 @@ Delay before disarming when requested by switch (ms) [0-1000] ### tailsitter_board_orientation -If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch. +Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d3723d05af7..10d601d8712 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1183,7 +1183,7 @@ groups: min: 0 max: 200 - name: tailsitter_board_orientation - description: "If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch." + description: "Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode" default_value: OFF field: mixer_config.tailsitterBoardOrientation type: bool From cfb63dd56a424a27fc5e5939228036aea0387e57 Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 16 Oct 2023 10:52:02 +0900 Subject: [PATCH 09/14] parameter rename --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 4 ++-- src/main/flight/mixer.c | 2 +- src/main/flight/mixer_profile.c | 2 +- src/main/flight/mixer_profile.h | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 67e4b554652..968f2e49f5a 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5572,7 +5572,7 @@ Delay before disarming when requested by switch (ms) [0-1000] --- -### tailsitter_board_orientation +### tailsitter_orientation_offset Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 10d601d8712..bc5429b3384 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1182,10 +1182,10 @@ groups: field: mixer_config.switchTransitionTimer min: 0 max: 200 - - name: tailsitter_board_orientation + - name: tailsitter_orientation_offset description: "Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode" default_value: OFF - field: mixer_config.tailsitterBoardOrientation + field: mixer_config.tailsitterOrientationOffset type: bool diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 874f48c5c70..834841e6580 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -187,7 +187,7 @@ void mixerUpdateStateFlags(void) ENABLE_STATE(ALTITUDE_CONTROL); } - if (currentMixerConfig.tailsitterBoardOrientation) { + if (currentMixerConfig.tailsitterOrientationOffset) { ENABLE_STATE(TAILSITTER); } else { DISABLE_STATE(TAILSITTER); diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index de347feed3e..421dff17639 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -52,7 +52,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, - .tailsitterBoardOrientation = SETTING_TAILSITTER_BOARD_ORIENTATION_DEFAULT, + .tailsitterOrientationOffset = SETTING_TAILSITTER_ORIENTATION_OFFSET_DEFAULT, }); for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) { diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 073e256eb1e..a709309685f 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -18,7 +18,7 @@ typedef struct mixerConfig_s { bool PIDProfileLinking; bool automated_switch; int16_t switchTransitionTimer; - bool tailsitterBoardOrientation; + bool tailsitterOrientationOffset; } mixerConfig_t; typedef struct mixerProfile_s { mixerConfig_t mixer_config; From 0349bd70fe5164e1cb6299b3b2005a73f40ef55a Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 20 Oct 2023 09:15:03 +0900 Subject: [PATCH 10/14] logic condition RCcommand overide also works for angle mode --- src/main/flight/pid.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5e7f1edb132..fdea10dc435 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -584,7 +584,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()) { From d1658da5e84662106b08f42856a02dade6e355b8 Mon Sep 17 00:00:00 2001 From: druckgott Date: Sat, 18 Nov 2023 20:01:24 +0100 Subject: [PATCH 11/14] add some icons for O3 with integra for ESP32 Radar instead of ? Add some icons for ESP32Radar instead of have ?. Update displayport_msp_bf_compat.c fix some ; --- src/main/io/displayport_msp_bf_compat.c | 42 +++++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c index 4229e035b03..dd120a2295d 100644 --- a/src/main/io/displayport_msp_bf_compat.c +++ b/src/main/io/displayport_msp_bf_compat.c @@ -578,6 +578,48 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) case (SYM_AH_V_START+4): case (SYM_AH_V_START+5): return BF_SYM_AH_BAR9_4; + + // BF for ESP_RADAR Symbols + case SYM_HUD_CARDINAL: + return BF_SYM_ARROW_SOUTH; + case SYM_HUD_CARDINAL + 1: + return BF_SYM_ARROW_16; + case SYM_HUD_CARDINAL + 2: + return BF_SYM_ARROW_15; + case SYM_HUD_CARDINAL + 3: + return BF_SYM_ARROW_WEST; + case SYM_HUD_CARDINAL + 4: + return BF_SYM_ARROW_12; + case SYM_HUD_CARDINAL + 5: + return BF_SYM_ARROW_11; + case SYM_HUD_CARDINAL + 6: + return BF_SYM_ARROW_NORTH; + case SYM_HUD_CARDINAL + 7: + return BF_SYM_ARROW_7; + case SYM_HUD_CARDINAL + 8: + return BF_SYM_ARROW_6; + case SYM_HUD_CARDINAL + 9: + return BF_SYM_ARROW_EAST; + case SYM_HUD_CARDINAL + 10: + return BF_SYM_ARROW_3; + case SYM_HUD_CARDINAL + 11: + return BF_SYM_ARROW_2; + + case SYM_HUD_ARROWS_R3: + return BF_SYM_AH_RIGHT; + case SYM_HUD_ARROWS_L3: + return BF_SYM_AH_LEFT; + + case SYM_HUD_SIGNAL_0: + return BF_SYM_AH_BAR9_1; + case SYM_HUD_SIGNAL_1: + return BF_SYM_AH_BAR9_3; + case SYM_HUD_SIGNAL_2: + return BF_SYM_AH_BAR9_4; + case SYM_HUD_SIGNAL_3: + return BF_SYM_AH_BAR9_5; + case SYM_HUD_SIGNAL_4: + return BF_SYM_AH_BAR9_7; /* case SYM_VARIO_UP_2A: return BF_SYM_VARIO_UP_2A; From 879b20d5186bf80507bde06fbb9dd0eebe4b8581 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 8 Dec 2023 17:40:39 +0000 Subject: [PATCH 12/14] Update navigation.c --- src/main/navigation/navigation.c | 59 ++++++++++++++++---------------- 1 file changed, 30 insertions(+), 29 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 0a3cfb91b8f..8e210aa533e 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -132,7 +132,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, #endif .waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot .auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h) - .min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT, // Minimum ground speed (m/s) + .min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT, // Minimum ground speed (m/s) .max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT, @@ -1450,7 +1450,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); posControl.landingDelay = 0; - + if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive) posControl.rthState.rthLinearDescentActive = false; @@ -3840,7 +3840,6 @@ void checkManualEmergencyLandingControl(bool forcedActivation) static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) { - static bool canActivateWaypoint = false; static bool canActivateLaunchMode = false; //We can switch modes only when ARMED @@ -3888,10 +3887,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } posControl.rthSanityChecker.rthSanityOK = true; - // Keep canActivateWaypoint flag at FALSE if there is no mission loaded - // Also block WP mission if we are executing RTH - if (!isWaypointMissionValid() || isExecutingRTH) { + /* WP mission activation control: + * canActivateWaypoint & waypointWasActivated are used to prevent WP mission + * auto restarting after interruption by Manual or RTH modes. + * WP mode must be deselected before it can be reactivated again. */ + static bool waypointWasActivated = false; + const bool isWpMissionLoaded = isWaypointMissionValid(); + bool canActivateWaypoint = isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; // Block activation if using WP Mission Planner + + if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) { canActivateWaypoint = false; + if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) { + canActivateWaypoint = true; + waypointWasActivated = false; + } } /* Airplane specific modes */ @@ -3927,22 +3936,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } } - // Failsafe_RTH (can override MANUAL) + /* If we request forced RTH - attempt to activate it no matter what + * This might switch to emergency landing controller if GPS is unavailable */ if (posControl.flags.forcedRTHActivated) { - // If we request forced RTH - attempt to activate it no matter what - // This might switch to emergency landing controller if GPS is unavailable return NAV_FSM_EVENT_SWITCH_TO_RTH; } - /* Pilot-triggered RTH (can override MANUAL), also fall-back for WP if there is no mission loaded - * Prevent MANUAL falling back to RTH if selected during active mission (canActivateWaypoint is set false on MANUAL selection) - * Also prevent WP falling back to RTH if WP mission planner is active */ - const bool blockWPFallback = IS_RC_MODE_ACTIVE(BOXMANUAL) || posControl.flags.wpMissionPlannerActive; - if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !blockWPFallback)) { + /* Pilot-triggered RTH, also fall-back for WP if there is no mission loaded. + * WP prevented from falling back to RTH if WP mission planner is active */ + const bool wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; + if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || wpRthFallbackIsActive) { // Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss - // If don't keep this, loss of any of the canActivateNavigation && canActivateAltHold + // Without this loss of any of the canActivateNavigation && canActivateAltHold // will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back - // logic to kick in (waiting for GPS on airplanes, switch to emergency landing etc) + // logic kicking in (waiting for GPS on airplanes, switch to emergency landing etc) if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { return NAV_FSM_EVENT_SWITCH_TO_RTH; } @@ -3950,24 +3957,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) // MANUAL mode has priority over WP/PH/AH if (IS_RC_MODE_ACTIVE(BOXMANUAL)) { - canActivateWaypoint = false; // Block WP mode if we are in PASSTHROUGH mode return NAV_FSM_EVENT_SWITCH_TO_IDLE; } - // Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded - // Block activation if using WP Mission Planner - // Also check multimission mission change status before activating WP mode + // Pilot-activated waypoint mission. Fall-back to RTH if no mission loaded. + // Also check multimission mission change status before activating WP mode. #ifdef USE_MULTI_MISSION - if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { + if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) { #else - if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { + if (IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) { #endif - if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) + if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { + waypointWasActivated = true; return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT; - } - else { - // Arm the state variable if the WP BOX mode is not enabled - canActivateWaypoint = true; + } } if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) { @@ -3998,8 +4001,6 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD; } } else { - canActivateWaypoint = false; - // Launch mode can be activated if feature FW_LAUNCH is enabled or BOX is turned on prior to arming (avoid switching to LAUNCH in flight) canActivateLaunchMode = isNavLaunchEnabled() && (!sensors(SENSOR_GPS) || (sensors(SENSOR_GPS) && !isGPSHeadingValid())); } From 0535617286bda0313364386a01a23f26d268623a Mon Sep 17 00:00:00 2001 From: Martin Luessi Date: Sat, 16 Dec 2023 08:23:14 -0800 Subject: [PATCH 13/14] USB MSC: Fix SCIS mode sense write protection bit (#9572) --- .../Class/MSC/Src/usbd_msc_scsi.c | 12 ++++++++++++ .../Class/MSC/Src/usbd_msc_scsi.c | 12 ++++++++++++ .../Class/MSC/Src/usbd_msc_scsi.c | 11 +++++++++++ 3 files changed, 35 insertions(+) diff --git a/lib/main/STM32F4/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c b/lib/main/STM32F4/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c index 5d4d2521693..3c63a978f2c 100644 --- a/lib/main/STM32F4/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c +++ b/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; } diff --git a/lib/main/STM32F7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c b/lib/main/STM32F7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c index ed3deef5959..b1d76cc25da 100644 --- a/lib/main/STM32F7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c +++ b/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; } diff --git a/lib/main/STM32H7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c b/lib/main/STM32H7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c index d15d9039a54..ffc615dac5a 100644 --- a/lib/main/STM32H7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c +++ b/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; } From 874407ba61060fa330eb56b30399697dfe5e4ccf Mon Sep 17 00:00:00 2001 From: error414 Date: Mon, 9 Oct 2023 19:18:22 +0200 Subject: [PATCH 14/14] ADSB support for inav over mavlink --- .gitignore | 1 + docs/ADSB.md | 17 +++ docs/Settings.md | 30 ++++ src/main/CMakeLists.txt | 1 + src/main/drivers/osd_symbols.h | 2 + src/main/fc/fc_msp.c | 35 +++++ src/main/fc/fc_tasks.c | 21 +++ src/main/fc/settings.yaml | 25 ++++ src/main/io/adsb.c | 223 ++++++++++++++++++++++++++++ src/main/io/adsb.h | 67 +++++++++ src/main/io/osd.c | 74 +++++++++ src/main/io/osd.h | 7 + src/main/msp/msp_protocol_v2_inav.h | 2 + src/main/scheduler/scheduler.h | 3 + src/main/target/common.h | 8 + src/main/telemetry/mavlink.c | 49 ++++++ 16 files changed, 565 insertions(+) create mode 100644 docs/ADSB.md create mode 100644 src/main/io/adsb.c create mode 100644 src/main/io/adsb.h diff --git a/.gitignore b/.gitignore index b2c3b9ff546..87ca5a87c7b 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,4 @@ make/local.mk launch.json .vscode/tasks.json .vscode/c_cpp_properties.json +/cmake-build-debug/ diff --git a/docs/ADSB.md b/docs/ADSB.md new file mode 100644 index 00000000000..345af30a97e --- /dev/null +++ b/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) + + diff --git a/docs/Settings.md b/docs/Settings.md index adc17297c1e..1872bf84a17 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4002,6 +4002,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) diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 13ac4a92911..1140cdd5080 100755 --- a/src/main/CMakeLists.txt +++ b/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 diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index d645184e5b4..2437b9d3c2c 100644 --- a/src/main/drivers/osd_symbols.h +++ b/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 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 952f13dddb0..8de7ca1c0d0 100644 --- a/src/main/fc/fc_msp.c +++ b/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; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 607a814bed5..246d65c0a7e 100755 --- a/src/main/fc/fc_tasks.c +++ b/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", diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 09823394935..963783ed946 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3444,6 +3444,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 diff --git a/src/main/io/adsb.c b/src/main/io/adsb.c new file mode 100644 index 00000000000..573112c2df7 --- /dev/null +++ b/src/main/io/adsb.c @@ -0,0 +1,223 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + + +#include + + +#include "adsb.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#include "common/maths.h" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-function" +#include "common/mavlink.h" +#pragma GCC diagnostic pop + + +#include "math.h" + + +#ifdef USE_ADSB + +adsbVehicle_t adsbVehiclesList[MAX_ADSB_VEHICLES]; +adsbVehicleStatus_t adsbVehiclesStatus; + +adsbVehicleValues_t vehicleValues; + + +adsbVehicleValues_t* getVehicleForFill(void){ + return &vehicleValues; +} + +// use bsearch function +adsbVehicle_t *findVehicleByIcao(uint32_t avicao) { + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (avicao == adsbVehiclesList[i].vehicleValues.icao) { + return &adsbVehiclesList[i]; + } + } + return NULL; +} + +adsbVehicle_t *findVehicleFarthest(void) { + adsbVehicle_t *adsbLocal = NULL; + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (adsbVehiclesList[i].ttl > 0 && adsbVehiclesList[i].calculatedVehicleValues.valid && (adsbLocal == NULL || adsbLocal->calculatedVehicleValues.dist < adsbVehiclesList[i].calculatedVehicleValues.dist)) { + adsbLocal = &adsbVehiclesList[i]; + } + } + return adsbLocal; +} + +uint8_t getActiveVehiclesCount(void) { + uint8_t total = 0; + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (adsbVehiclesList[i].ttl > 0) { + total++; + } + } + return total; +} + +adsbVehicle_t *findVehicleClosest(void) { + adsbVehicle_t *adsbLocal = NULL; + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (adsbVehiclesList[i].ttl > 0 && adsbVehiclesList[i].calculatedVehicleValues.valid && (adsbLocal == NULL || adsbLocal->calculatedVehicleValues.dist > adsbVehiclesList[i].calculatedVehicleValues.dist)) { + adsbLocal = &adsbVehiclesList[i]; + } + } + return adsbLocal; +} + +adsbVehicle_t *findFreeSpaceInList(void) { + //find expired first + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (adsbVehiclesList[i].ttl == 0) { + return &adsbVehiclesList[i]; + } + } + + return NULL; +} + +adsbVehicle_t *findVehicleNotCalculated(void) { + //find expired first + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (adsbVehiclesList[i].calculatedVehicleValues.valid == false) { + return &adsbVehiclesList[i]; + } + } + + return NULL; +} + +adsbVehicle_t* findVehicle(uint8_t index) +{ + if (index < MAX_ADSB_VEHICLES){ + return &adsbVehiclesList[index]; + } + + return NULL; +} + +adsbVehicleStatus_t* getAdsbStatus(void){ + return &adsbVehiclesStatus; +} + +void gpsDistanceCmBearing(int32_t currentLat1, int32_t currentLon1, int32_t destinationLat2, int32_t destinationLon2, uint32_t *dist, int32_t *bearing) { + float GPS_scaleLonDown = cos_approx((fabsf((float) gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f); + const float dLat = destinationLat2 - currentLat1; // difference of latitude in 1/10 000 000 degrees + const float dLon = (float) (destinationLon2 - currentLon1) * GPS_scaleLonDown; + + *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR; + *bearing = 9000.0f + RADIANS_TO_CENTIDEGREES(atan2_approx(-dLat, dLon)); // Convert the output radians to 100xdeg + *bearing = wrap_36000(*bearing); +}; + +void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) { + + // no valid lat lon or altitude + if((vehicleValuesLocal->flags & (ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS)) != (ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS)){ + return; + } + + adsbVehiclesStatus.vehiclesMessagesTotal++; + adsbVehicle_t *vehicle = NULL; + + vehicle = findVehicleByIcao(vehicleValuesLocal->icao); + if(vehicle != NULL && vehicleValuesLocal->tslc > ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST){ + vehicle->ttl = 0; + return; + } + + // non GPS mode, GPS is not fix, just find free space in list or by icao and save vehicle without calculated values + if (!enviromentOkForCalculatingDistaceBearing()) { + + if(vehicle == NULL){ + vehicle = findFreeSpaceInList(); + } + + if (vehicle != NULL) { + memcpy(&(vehicle->vehicleValues), vehicleValuesLocal, sizeof(vehicle->vehicleValues)); + vehicle->ttl = ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST; + vehicle->calculatedVehicleValues.valid = false; + return; + } + } else { + // GPS mode, GPS is fixed and has enough sats + + + if(vehicle == NULL){ + vehicle = findFreeSpaceInList(); + } + + if(vehicle == NULL){ + vehicle = findVehicleNotCalculated(); + } + + if(vehicle == NULL){ + vehicle = findVehicleFarthest(); + } + + if (vehicle != NULL) { + memcpy(&(vehicle->vehicleValues), vehicleValuesLocal, sizeof(vehicle->vehicleValues)); + recalculateVehicle(vehicle); + vehicle->ttl = ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST; + return; + } + } +}; + +void recalculateVehicle(adsbVehicle_t* vehicle){ + gpsDistanceCmBearing(gpsSol.llh.lat, gpsSol.llh.lon, vehicle->vehicleValues.lat, vehicle->vehicleValues.lon, &(vehicle->calculatedVehicleValues.dist), &(vehicle->calculatedVehicleValues.dir)); + + if (vehicle != NULL && vehicle->calculatedVehicleValues.dist > ADSB_LIMIT_CM) { + vehicle->ttl = 0; + return; + } + + vehicle->calculatedVehicleValues.verticalDistance = vehicle->vehicleValues.alt - (int32_t)getEstimatedActualPosition(Z) - GPS_home.alt; + vehicle->calculatedVehicleValues.valid = true; +} + +void adsbTtlClean(timeUs_t currentTimeUs) { + + static timeUs_t adsbTtlLastCleanServiced = 0; + timeDelta_t adsbTtlSinceLastCleanServiced = cmpTimeUs(currentTimeUs, adsbTtlLastCleanServiced); + + + if (adsbTtlSinceLastCleanServiced > 1000000) // 1s + { + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (adsbVehiclesList[i].ttl > 0) { + adsbVehiclesList[i].ttl--; + } + } + + adsbTtlLastCleanServiced = currentTimeUs; + } +}; + +bool enviromentOkForCalculatingDistaceBearing(void){ + return (STATE(GPS_FIX) && gpsSol.numSat > 4); +} + +#endif + diff --git a/src/main/io/adsb.h b/src/main/io/adsb.h new file mode 100644 index 00000000000..86eefd8aac1 --- /dev/null +++ b/src/main/io/adsb.h @@ -0,0 +1,67 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#include +#include "common/time.h" +#include "fc/runtime_config.h" + +#define ADSB_CALL_SIGN_MAX_LENGTH 9 +#define ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST 10 + +typedef struct { + bool valid; + int32_t dir; // centidegrees direction to plane, pivot is inav FC + uint32_t dist; // CM distance to plane, pivot is inav FC + int32_t verticalDistance; // CM, vertical distance to plane, pivot is inav FC +} adsbVehicleCalculatedValues_t; + +typedef struct { + uint32_t icao; // ICAO address + int32_t lat; // Latitude, expressed as degrees * 1E7 + int32_t lon; // Longitude, expressed as degrees * 1E7 + int32_t alt; // Barometric/Geometric Altitude (ASL), in cm + uint16_t heading; // Course over ground in centidegrees + uint16_t flags; // Flags to indicate various statuses including valid data fields + uint8_t altitudeType; // Type from ADSB_ALTITUDE_TYPE enum + char callsign[ADSB_CALL_SIGN_MAX_LENGTH]; // The callsign, 8 chars + NULL + uint8_t emitterType; // Type from ADSB_EMITTER_TYPE enum + uint8_t tslc; // Time since last communication in seconds +} adsbVehicleValues_t; + +typedef struct { + adsbVehicleValues_t vehicleValues; + adsbVehicleCalculatedValues_t calculatedVehicleValues; + uint8_t ttl; +} adsbVehicle_t; + + + +typedef struct { + uint32_t vehiclesMessagesTotal; +} adsbVehicleStatus_t; + +void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal); +adsbVehicle_t * findVehicleClosest(void); +adsbVehicle_t * findVehicle(uint8_t index); +uint8_t getActiveVehiclesCount(void); +void adsbTtlClean(timeUs_t currentTimeUs); +adsbVehicleStatus_t* getAdsbStatus(void); +adsbVehicleValues_t* getVehicleForFill(void); +bool enviromentOkForCalculatingDistaceBearing(void); +void recalculateVehicle(adsbVehicle_t* vehicle); \ No newline at end of file diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 02d0e02a803..cc34a8399f0 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -62,6 +62,7 @@ #include "drivers/time.h" #include "drivers/vtx_common.h" +#include "io/adsb.h" #include "io/flashfs.h" #include "io/gps.h" #include "io/osd.h" @@ -2067,7 +2068,73 @@ static bool osdDrawSingleElement(uint8_t item) osdFormatCentiNumber(&buff[2], centiHDOP, 0, 1, 0, digits, false); break; } +#ifdef USE_ADSB + case OSD_ADSB_WARNING: + { + static uint8_t adsblen = 1; + uint8_t arrowPositionX = 0; + + for (int i = 0; i <= adsblen; i++) { + buff[i] = SYM_BLANK; + } + + buff[adsblen]='\0'; + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); // clear any previous chars because variable element size + adsblen=1; + adsbVehicle_t *vehicle = findVehicleClosest(); + + if(vehicle != NULL){ + recalculateVehicle(vehicle); + } + + if ( + vehicle != NULL && + (vehicle->calculatedVehicleValues.dist > 0) && + vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_warning) && + (osdConfig()->adsb_ignore_plane_above_me_limit == 0 || METERS_TO_CENTIMETERS(osdConfig()->adsb_ignore_plane_above_me_limit) > vehicle->calculatedVehicleValues.verticalDistance) + ){ + buff[0] = SYM_ADSB; + osdFormatDistanceStr(&buff[1], (int32_t)vehicle->calculatedVehicleValues.dist); + adsblen = strlen(buff); + + buff[adsblen-1] = SYM_BLANK; + + arrowPositionX = adsblen-1; + osdFormatDistanceStr(&buff[adsblen], vehicle->calculatedVehicleValues.verticalDistance); + adsblen = strlen(buff)-1; + + if (vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_alert)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + + buff[adsblen]='\0'; + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + if (arrowPositionX > 0){ + int16_t panHomeDirOffset = 0; + if (osdConfig()->pan_servo_pwm2centideg != 0){ + panHomeDirOffset = osdGetPanServoOffset(); + } + int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading()); + osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX + arrowPositionX, elemPosY), CENTIDEGREES_TO_DEGREES(vehicle->calculatedVehicleValues.dir) - flightDirection + panHomeDirOffset); + } + + return true; + } + case OSD_ADSB_INFO: + { + buff[0] = SYM_ADSB; + if(getAdsbStatus()->vehiclesMessagesTotal > 0){ + tfp_sprintf(buff + 1, "%2d", getActiveVehiclesCount()); + }else{ + buff[1] = '-'; + } + + break; + } + +#endif case OSD_MAP_NORTH: { static uint16_t drawn = 0; @@ -3729,6 +3796,11 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .baro_temp_alarm_min = SETTING_OSD_BARO_TEMP_ALARM_MIN_DEFAULT, .baro_temp_alarm_max = SETTING_OSD_BARO_TEMP_ALARM_MAX_DEFAULT, #endif +#ifdef USE_ADSB + .adsb_distance_warning = SETTING_OSD_ADSB_DISTANCE_WARNING_DEFAULT, + .adsb_distance_alert = SETTING_OSD_ADSB_DISTANCE_ALERT_DEFAULT, + .adsb_ignore_plane_above_me_limit = SETTING_OSD_ADSB_IGNORE_PLANE_ABOVE_ME_LIMIT_DEFAULT, +#endif #ifdef USE_SERIALRX_CRSF .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT, .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT, @@ -3968,6 +4040,8 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9); osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_3] = OSD_POS(2, 10); + osdLayoutsConfig->item_pos[0][OSD_ADSB_WARNING] = OSD_POS(2, 7); + osdLayoutsConfig->item_pos[0][OSD_ADSB_INFO] = OSD_POS(2, 8); #if defined(USE_ESC_SENSOR) osdLayoutsConfig->item_pos[0][OSD_ESC_RPM] = OSD_POS(1, 2); osdLayoutsConfig->item_pos[0][OSD_ESC_TEMPERATURE] = OSD_POS(1, 3); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 5bcf6d34740..8ce197f9bd4 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -280,6 +280,8 @@ typedef enum { OSD_MULTI_FUNCTION, OSD_ODOMETER, OSD_PILOT_LOGO, + OSD_ADSB_WARNING, + OSD_ADSB_INFO, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -455,6 +457,11 @@ typedef struct osdConfig_s { bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo. uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width. uint16_t arm_screen_display_time; // Length of time the arm screen is displayed + #ifdef USE_ADSB + uint16_t adsb_distance_warning; // in metres + uint16_t adsb_distance_alert; // in metres + uint16_t adsb_ignore_plane_above_me_limit; // in metres +#endif } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index eba96690a27..5cd108e560d 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -99,3 +99,5 @@ #define MSP2_INAV_EZ_TUNE_SET 0x2071 #define MSP2_INAV_SELECT_MIXER_PROFILE 0x2080 + +#define MSP2_ADSB_VEHICLE_LIST 0x2090 diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 57707c725b1..0d91876cb72 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -70,6 +70,9 @@ typedef enum { #ifdef USE_BARO TASK_BARO, #endif +#ifdef USE_ADSB + TASK_ADSB, +#endif #ifdef USE_PITOT TASK_PITOT, #endif diff --git a/src/main/target/common.h b/src/main/target/common.h index d2d135cfa2e..d7ffd86e68b 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -182,6 +182,14 @@ #define USE_CMS_FONT_PREVIEW +//ADSB RECEIVER +#ifdef USE_GPS +#define USE_ADSB +#define MAX_ADSB_VEHICLES 5 +#define ADSB_LIMIT_CM 6400000 +#endif + + //Designed to free space of F722 and F411 MCUs #if (MCU_FLASH_SIZE > 512) #define USE_VTX_FFPV diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 187f959b178..b3c3509b216 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -58,6 +58,7 @@ #include "flight/pid.h" #include "flight/servos.h" +#include "io/adsb.h" #include "io/gps.h" #include "io/ledstrip.h" #include "io/serial.h" @@ -1054,6 +1055,50 @@ static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) { return true; } +#ifdef USE_ADSB +static bool handleIncoming_ADSB_VEHICLE(void) { + mavlink_adsb_vehicle_t msg; + mavlink_msg_adsb_vehicle_decode(&mavRecvMsg, &msg); + + adsbVehicleValues_t* vehicle = getVehicleForFill(); + if(vehicle != NULL){ + vehicle->icao = msg.ICAO_address; + vehicle->lat = msg.lat; + vehicle->lon = msg.lon; + vehicle->alt = (int32_t)(msg.altitude / 10); + vehicle->heading = msg.heading; + vehicle->flags = msg.flags; + vehicle->altitudeType = msg.altitude_type; + memcpy(&(vehicle->callsign), msg.callsign, sizeof(vehicle->callsign)); + vehicle->emitterType = msg.emitter_type; + vehicle->tslc = msg.tslc; + + adsbNewVehicle(vehicle); + } + + //debug vehicle + /* if(vehicle != NULL){ + + char name[9] = "DUMMY "; + + vehicle->icao = 666; + vehicle->lat = 492383514; + vehicle->lon = 165148681; + vehicle->alt = 100000; + vehicle->heading = 180; + vehicle->flags = ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS; + vehicle->altitudeType = 0; + memcpy(&(vehicle->callsign), name, sizeof(vehicle->callsign)); + vehicle->emitterType = 6; + vehicle->tslc = 0; + + adsbNewVehicle(vehicle); + }*/ + + return true; +} +#endif + static bool processMAVLinkIncomingTelemetry(void) { while (serialRxBytesWaiting(mavlinkPort) > 0) { @@ -1076,6 +1121,10 @@ static bool processMAVLinkIncomingTelemetry(void) return handleIncoming_MISSION_REQUEST(); case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: return handleIncoming_RC_CHANNELS_OVERRIDE(); +#ifdef USE_ADSB + case MAVLINK_MSG_ID_ADSB_VEHICLE: + return handleIncoming_ADSB_VEHICLE(); +#endif default: return false; }