From 5ed2a9dd6a12dcef50276637afb17a08a1610f57 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Wed, 24 Jun 2020 13:59:25 +0200 Subject: [PATCH 001/109] [LIDAR] Add Benewake TFmini initialization --- src/main/io/rangefinder_benewake.c | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/main/io/rangefinder_benewake.c b/src/main/io/rangefinder_benewake.c index 08e8d0cdc6..b84ab5cbc5 100644 --- a/src/main/io/rangefinder_benewake.c +++ b/src/main/io/rangefinder_benewake.c @@ -55,15 +55,20 @@ typedef struct __attribute__((packed)) { #define BENEWAKE_PACKET_SIZE sizeof(tfminiPacket_t) #define BENEWAKE_MIN_QUALITY 0 +#define BENEWAKE_TIMEOUT_MS 200 // 200ms static serialPort_t * serialPort = NULL; static serialPortConfig_t * portConfig; static uint8_t buffer[BENEWAKE_PACKET_SIZE]; static unsigned bufferPtr; +static timeMs_t lastProtocolActivityMs; static bool hasNewData = false; static int32_t sensorData = RANGEFINDER_NO_NEW_DATA; +// TFmini command to initiate 100Hz sampling +static const uint8_t initCmd100Hz[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 }; + static bool benewakeRangefinderDetect(void) { portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER); @@ -80,16 +85,27 @@ static void benewakeRangefinderInit(void) return; } - serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, baudRates[BAUD_115200], MODE_RX, SERIAL_NOT_INVERTED); + serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED); if (!serialPort) { return; } + lastProtocolActivityMs = 0; bufferPtr = 0; } static void benewakeRangefinderUpdate(void) { + // Initialize the sensor + if (lastProtocolActivityMs == 0 || (millis() - lastProtocolActivityMs) > BENEWAKE_TIMEOUT_MS) { + // Initialize the sensor to do 100Hz sampling to make sure we get the most recent data always + serialWriteBuf(serialPort, initCmd100Hz, sizeof(initCmd100Hz)); + + // Send the init command every BENEWAKE_TIMEOUT_MS + lastProtocolActivityMs = millis(); + } + + // Process incoming bytes tfminiPacket_t *tfminiPacket = (tfminiPacket_t *)buffer; while (serialRxBytesWaiting(serialPort) > 0) { uint8_t c = serialRead(serialPort); @@ -117,6 +133,7 @@ static void benewakeRangefinderUpdate(void) // Valid packet hasNewData = true; sensorData = (tfminiPacket->distL << 0) | (tfminiPacket->distH << 8); + lastProtocolActivityMs = millis(); uint16_t qual = (tfminiPacket->strengthL << 0) | (tfminiPacket->strengthH << 8); From 233c48b52b135c9473737c5b79284b3ccf04e9e1 Mon Sep 17 00:00:00 2001 From: "DESKTOP-N53JVUO\\mix" Date: Tue, 14 Jul 2020 22:35:17 +0300 Subject: [PATCH 002/109] Add Wing Autolaunch settings in the OSD menu as a submenu named "AUTOLAUNCH". The settings related to the Wing cruise was moved to a new submenu named "CRUISE", Both submenus are now under the "FIXED WING" menu. --- src/main/cms/cms_menu_navigation.c | 61 +++++++++++++++++++++++++++--- 1 file changed, 56 insertions(+), 5 deletions(-) diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index af5ac8574b..a60e8e7ed5 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -96,9 +96,9 @@ static const CMS_Menu cmsx_menuRTH = { .entries = cmsx_menuRTHEntries }; -static const OSD_Entry cmsx_menuFixedWingEntries[] = +static const OSD_Entry cmsx_menuFWCruiseEntries[] = { - OSD_LABEL_ENTRY("-- FIXED WING --"), + OSD_LABEL_ENTRY("-- CRUISE --"), OSD_SETTING_ENTRY("CRUISE THROTTLE", SETTING_NAV_FW_CRUISE_THR), OSD_SETTING_ENTRY("MIN THROTTLE", SETTING_NAV_FW_MIN_THR), @@ -113,7 +113,58 @@ static const OSD_Entry cmsx_menuFixedWingEntries[] = OSD_BACK_AND_END_ENTRY, }; -static const CMS_Menu cmsx_menuFixedWing = { +static const CMS_Menu cmsx_menuFWCruise = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUNAVFWCRUISE", + .GUARD_type = OME_MENU, +#endif + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuFWCruiseEntries +}; + +static const OSD_Entry cmsx_menuFWLaunchEntries[] = +{ + OSD_LABEL_ENTRY("-- AUTOLAUNCH --"), + + OSD_SETTING_ENTRY("LAUNCH THR", SETTING_NAV_FW_LAUNCH_THR), + OSD_SETTING_ENTRY("IDLE THROTTLE", SETTING_NAV_FW_LAUNCH_IDLE_THR), + OSD_SETTING_ENTRY("MOTOR SPINUP TIME", SETTING_NAV_FW_LAUNCH_SPINUP_TIME), + OSD_SETTING_ENTRY("TIMEOUT", SETTING_NAV_FW_LAUNCH_TIMEOUT), + OSD_SETTING_ENTRY("MAX ALTITUDE", SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE), + OSD_SETTING_ENTRY("CLIMB ANGLE", SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE), + OSD_SETTING_ENTRY("MAX BANK ANGLE", SETTING_NAV_FW_LAUNCH_MAX_ANGLE), + OSD_SETTING_ENTRY("MOTOR DELAY", SETTING_NAV_FW_LAUNCH_MOTOR_DELAY), + OSD_SETTING_ENTRY("VELOCITY", SETTING_NAV_FW_LAUNCH_VELOCITY), + OSD_SETTING_ENTRY("ACCELERATION", SETTING_NAV_FW_LAUNCH_ACCEL), + OSD_SETTING_ENTRY("DETECT TIME", SETTING_NAV_FW_LAUNCH_DETECT_TIME), + + OSD_BACK_AND_END_ENTRY, + }; + +static const CMS_Menu cmsx_menuFWLaunch = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUNAVFWLAUNCH", + .GUARD_type = OME_MENU, +#endif + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuFWLaunchEntries +}; + +static const OSD_Entry cmsx_menuFWSettingsEntries[] = +{ + OSD_LABEL_ENTRY("-- FIXED WING --"), + + OSD_SUBMENU_ENTRY("AUTOLAUNCH", &cmsx_menuFWLaunch), + OSD_SUBMENU_ENTRY("CRUISE", &cmsx_menuFWCruise), + + OSD_BACK_AND_END_ENTRY, +}; + +static const CMS_Menu cmsx_menuFWSettings = { #ifdef CMS_MENU_DEBUG .GUARD_text = "MENUNAVFW", .GUARD_type = OME_MENU, @@ -121,7 +172,7 @@ static const CMS_Menu cmsx_menuFixedWing = { .onEnter = NULL, .onExit = NULL, .onGlobalExit = NULL, - .entries = cmsx_menuFixedWingEntries + .entries = cmsx_menuFWSettingsEntries }; static const OSD_Entry cmsx_menuNavigationEntries[] = @@ -130,7 +181,7 @@ static const OSD_Entry cmsx_menuNavigationEntries[] = OSD_SUBMENU_ENTRY("BASIC SETTINGS", &cmsx_menuNavSettings), OSD_SUBMENU_ENTRY("RTH", &cmsx_menuRTH), - OSD_SUBMENU_ENTRY("FIXED WING", &cmsx_menuFixedWing), + OSD_SUBMENU_ENTRY("FIXED WING", &cmsx_menuFWSettings), OSD_BACK_AND_END_ENTRY, }; From bab965da77f8e1613a8ab9e8383c584a13e9e9e7 Mon Sep 17 00:00:00 2001 From: "DESKTOP-N53JVUO\\mix" Date: Thu, 16 Jul 2020 09:18:49 +0300 Subject: [PATCH 003/109] Fixed wing launch refactor. Add a smooth transition to the current flight mode at the end of the launch. Use nav_fw_launch_end_time to change the transition time, default = 2000ms --- src/main/fc/settings.yaml | 6 +- src/main/navigation/navigation.c | 1 + src/main/navigation/navigation.h | 3 +- src/main/navigation/navigation_fw_launch.c | 325 +++++++++++++-------- 4 files changed, 209 insertions(+), 126 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 62e6275f93..08a6d5bfc9 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1602,7 +1602,7 @@ groups: field: fw.cruise_speed min: 0 max: 65535 - - name: nav_fw_control_smoothness + - name: nav_fw_control_smoothness field: fw.control_smoothness min: 0 max: 9 @@ -1645,6 +1645,10 @@ groups: field: fw.launch_motor_spinup_time min: 0 max: 1000 + - name: nav_fw_launch_end_time + field: fw.launch_end_time + min: 0 + max: 5000 - name: nav_fw_launch_min_time field: fw.launch_min_time min: 0 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 402062c1c6..cf0a7bc046 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -160,6 +160,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .launch_idle_throttle = 1000, // Motor idle or MOTOR_STOP .launch_motor_timer = 500, // ms .launch_motor_spinup_time = 100, // ms, time to gredually increase throttle from idle to launch + .launch_end_time = 2000, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode .launch_min_time = 0, // ms, min time in launch mode .launch_timeout = 5000, // ms, timeout for launch procedure .launch_max_altitude = 0, // cm, altitude where to consider launch ended diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 57927b0687..667bb39e49 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -211,7 +211,8 @@ typedef struct navConfig_s { uint16_t launch_throttle; // Launch throttle uint16_t launch_motor_timer; // Time to wait before setting launch_throttle (ms) uint16_t launch_motor_spinup_time; // Time to speed-up motors from idle to launch_throttle (ESC desync prevention) - uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early + uint16_t launch_end_time; // Time to make the transition from launch angle to leveled and throttle transition from launch throttle to the stick position + uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early uint16_t launch_timeout; // Launch timeout to disable launch mode and swith to normal flight (ms) uint16_t launch_max_altitude; // cm, altitude where to consider launch ended uint8_t launch_climb_angle; // Target climb angle for launch (deg) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 843d27f650..0df26d791f 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -33,15 +33,6 @@ #include "config/feature.h" #include "drivers/time.h" -#include "drivers/sensor.h" -#include "drivers/accgyro/accgyro.h" - -#include "sensors/sensors.h" -#include "sensors/rangefinder.h" -#include "sensors/barometer.h" -#include "sensors/acceleration.h" -#include "sensors/boardalignment.h" -#include "sensors/compass.h" #include "io/gps.h" #include "io/beeper.h" @@ -57,23 +48,112 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate +#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms + +enum { + FW_LAUNCH_STATE_WAIT_THROTTLE = 0, + FW_LAUNCH_STATE_MOTOR_IDLE = 1, + FW_LAUNCH_STATE_WAIT_DETECTION = 2, + FW_LAUNCH_STATE_DETECTED = 3, + FW_LAUNCH_STATE_MOTOR_DELAY = 4, + FW_LAUNCH_STATE_MOTOR_SPIN_UP = 5, + FW_LAUNCH_STATE_IN_PROGRESS = 6, + FW_LAUNCH_STATE_FINISHING = 7, + FW_LAUNCH_STATE_FINISHED = 8 +}; typedef struct FixedWingLaunchState_s { /* Launch detection */ timeUs_t launchDetectorPreviousUpdate; timeUs_t launchDetectionTimeAccum; - bool launchDetected; - /* Launch progress */ - timeUs_t launchStartedTime; - bool launchFinished; - bool motorControlAllowed; + int state; + timeUs_t stateStartedTimeUs; + timeUs_t launchStartedTimeUs; } FixedWingLaunchState_t; static EXTENDED_FASTRAM FixedWingLaunchState_t launchState; -#define COS_MAX_LAUNCH_ANGLE 0.70710678f // cos(45), just to be safe -#define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate + +static void setLaunchState(int state, timeUs_t currentTimeUs) +{ + launchState.state = state; + launchState.stateStartedTimeUs = currentTimeUs; +} + +static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) +{ + return US2MS(currentTimeUs - launchState.stateStartedTimeUs); +} + +static timeMs_t getElapsedMsAndSetNextState(uint16_t input, timeUs_t currentTimeUs, int nextState) +{ + const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); + + if (input == 0 || input < elapsedTimeMs) { + setLaunchState(nextState, currentTimeUs); + } + return elapsedTimeMs; +} + +static inline bool isFixedWingLaunchMaxAltitudeReached(void) +{ + return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); +} + +static inline bool isLaunchModeMinTimeElapsed(float currentTimeUs) +{ + return US2MS(currentTimeUs - launchState.launchStartedTimeUs) > navConfig()->fw.launch_min_time; +} + +static void updateFixedWingLaunchPitchAngle(uint8_t pitchAngle) +{ + rcCommand[ROLL] = 0; + rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]); + rcCommand[YAW] = 0; +} + +static void lockLaunchPitchAngle(void) +{ + updateFixedWingLaunchPitchAngle(navConfig()->fw.launch_climb_angle); +} + +static void lockLaunchThrottleIdle(void) +{ + rcCommand[THROTTLE] = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); +} + +static void fixedWingLaunchResetPids(void) +{ + // Until motors are started don't use PID I-term and reset TPA filter + if (launchState.state < FW_LAUNCH_STATE_MOTOR_SPIN_UP) { + pidResetErrorAccumulators(); + pidResetTPAFilter(); + } +} + +static void updateFixedWingLaunchAbortTriggers(timeUs_t currentTimeUs) +{ + if (launchState.state < FW_LAUNCH_STATE_MOTOR_DELAY) { + return; + } + if (areSticksDeflectedMoreThanPosHoldDeadband() && isLaunchModeMinTimeElapsed(currentTimeUs)) { + setLaunchState(FW_LAUNCH_STATE_FINISHED, currentTimeUs); + } + else if (launchState.state == FW_LAUNCH_STATE_IN_PROGRESS && isFixedWingLaunchMaxAltitudeReached()) { + setLaunchState(FW_LAUNCH_STATE_FINISHING, currentTimeUs); + } +} + +static void updateFixedWingLaunchThrottleWait(timeUs_t currentTimeUs) +{ + if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) { + setLaunchState(FW_LAUNCH_STATE_MOTOR_IDLE, currentTimeUs); + } + rcCommand[THROTTLE] = getThrottleIdleValue(); +} + static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs) { const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0; @@ -87,7 +167,7 @@ static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs) launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate); launchState.launchDetectorPreviousUpdate = currentTimeUs; if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) { - launchState.launchDetected = true; + setLaunchState(FW_LAUNCH_STATE_DETECTED, currentTimeUs); } } else { @@ -96,143 +176,140 @@ static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs) } } -void resetFixedWingLaunchController(timeUs_t currentTimeUs) +static void updateFixedWingLaunchThrottleIdle(timeMs_t currentTimeUs) { - launchState.launchDetectorPreviousUpdate = currentTimeUs; - launchState.launchDetectionTimeAccum = 0; - launchState.launchStartedTime = 0; - launchState.launchDetected = false; - launchState.launchFinished = false; - launchState.motorControlAllowed = false; -} + const int throttleIdleVale = getThrottleIdleValue(); -bool isFixedWingLaunchDetected(void) -{ - return launchState.launchDetected; -} - -void enableFixedWingLaunchController(timeUs_t currentTimeUs) -{ - launchState.launchStartedTime = currentTimeUs; - launchState.motorControlAllowed = true; -} - -bool isFixedWingLaunchFinishedOrAborted(void) -{ - return launchState.launchFinished; -} - -void abortFixedWingLaunch(void) -{ - launchState.launchFinished = true; -} - -#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 //ms - -static void applyFixedWingLaunchIdleLogic(void) -{ - // Until motors are started don't use PID I-term - pidResetErrorAccumulators(); - - // We're not flying yet, reset TPA filter - pidResetTPAFilter(); + updateFixedWingLaunchDetector(currentTimeUs); // if the launch is detected, the throttle idle will be skipped and jump to FW_LAUNCH_STATE_DETECTED // Throttle control logic - if (navConfig()->fw.launch_idle_throttle <= getThrottleIdleValue()) - { - ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped - rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle + if (navConfig()->fw.launch_idle_throttle <= throttleIdleVale) { + ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped + rcCommand[THROTTLE] = throttleIdleVale; // If MOTOR_STOP is disabled, motors will spin at minthrottle + setLaunchState(FW_LAUNCH_STATE_WAIT_DETECTION, currentTimeUs); + lockLaunchPitchAngle(); } - else - { - static float timeThrottleRaisedMs; - if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) - { - timeThrottleRaisedMs = millis(); - } - else - { - const float timeSinceMotorStartMs = MIN(millis() - timeThrottleRaisedMs, LAUNCH_MOTOR_IDLE_SPINUP_TIME); - rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs, - 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, - getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); - } + else { + timeMs_t elapsedTimeMs = elapsedTimeMs = getElapsedMsAndSetNextState(LAUNCH_MOTOR_IDLE_SPINUP_TIME, currentTimeUs, FW_LAUNCH_STATE_WAIT_DETECTION); + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, throttleIdleVale, navConfig()->fw.launch_idle_throttle); + updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle)); } } -static inline bool isFixedWingLaunchMaxAltitudeReached(void) -{ - return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); -} -static inline bool isLaunchModeMinTimeElapsed(float timeSinceLaunchMs) -{ - return timeSinceLaunchMs > navConfig()->fw.launch_min_time; +static void updateFixedWingLaunchMotorDelay(timeUs_t currentTimeUs) { + getElapsedMsAndSetNextState(navConfig()->fw.launch_motor_timer, currentTimeUs, FW_LAUNCH_STATE_MOTOR_SPIN_UP); + lockLaunchThrottleIdle(); + lockLaunchPitchAngle(); } -static inline bool isLaunchModeMaxTimeElapsed(float timeSinceLaunchMs) +static void updateFixedWingLaunchSpinUp(timeUs_t currentTimeUs) { - return timeSinceLaunchMs >= navConfig()->fw.launch_timeout; + const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time; + const timeMs_t elapsedTimeMs = getElapsedMsAndSetNextState(motorSpinUpMs, currentTimeUs, FW_LAUNCH_STATE_IN_PROGRESS); + const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); + + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, navConfig()->fw.launch_throttle); + lockLaunchPitchAngle(); } -static inline bool isFixedWingLaunchCompleted(float timeSinceLaunchMs) +static void updateFixedWingLaunchProgress(timeUs_t currentTimeUs) { - return (isLaunchModeMaxTimeElapsed(timeSinceLaunchMs)) || ((isLaunchModeMinTimeElapsed(timeSinceLaunchMs)) && (areSticksDeflectedMoreThanPosHoldDeadband())) || isFixedWingLaunchMaxAltitudeReached(); + getElapsedMsAndSetNextState(navConfig()->fw.launch_timeout, currentTimeUs, FW_LAUNCH_STATE_FINISHING); + rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; + lockLaunchPitchAngle(); } +static void updateFixedWingLaunchFinishing(timeUs_t currentTimeUs) +{ + const uint16_t endTimeMs = navConfig()->fw.launch_end_time; + const timeMs_t elapsedTimeMs = getElapsedMsAndSetNextState(endTimeMs, currentTimeUs, FW_LAUNCH_STATE_FINISHED); + + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]); + updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, 0)); +} + +// Public methods --------------------------------------------------------------- + void applyFixedWingLaunchController(timeUs_t currentTimeUs) { // Called at PID rate - if (launchState.launchDetected) { - bool applyLaunchIdleLogic = true; + fixedWingLaunchResetPids(); + updateFixedWingLaunchAbortTriggers(currentTimeUs); - if (launchState.motorControlAllowed) { - // If launch detected we are in launch procedure - control airplane - const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs - launchState.launchStartedTime); + switch (launchState.state) { + case FW_LAUNCH_STATE_WAIT_THROTTLE: + updateFixedWingLaunchThrottleWait(currentTimeUs); // raise throttle stick to jump to FW_LAUNCH_STATE_MOTOR_IDLE and activate detection + break; - launchState.launchFinished = isFixedWingLaunchCompleted(timeElapsedSinceLaunchMs); + case FW_LAUNCH_STATE_MOTOR_IDLE: + updateFixedWingLaunchThrottleIdle(currentTimeUs); + break; - // Motor control enabled - if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_motor_timer) { - // Don't apply idle logic anymore - applyLaunchIdleLogic = false; + case FW_LAUNCH_STATE_WAIT_DETECTION: + updateFixedWingLaunchDetector(currentTimeUs); // when the launch is detected, we jump to FW_LAUNCH_STATE_DETECTED + lockLaunchThrottleIdle(); + lockLaunchPitchAngle(); + break; - // Increase throttle gradually over `launch_motor_spinup_time` milliseconds - if (navConfig()->fw.launch_motor_spinup_time > 0) { - const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time); - const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); - rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs, - 0.0f, navConfig()->fw.launch_motor_spinup_time, - minIdleThrottle, navConfig()->fw.launch_throttle); - } - else { - rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; - } - } - } + case FW_LAUNCH_STATE_DETECTED: + lockLaunchThrottleIdle(); + lockLaunchPitchAngle(); + // nothing else to do, wait for the navigation to switch to NAV_LAUNCH_STATE_MOTOR_DELAY state by calling enableFixedWingLaunchController() + break; - if (applyLaunchIdleLogic) { - // Launch idle logic - low throttle and zero out PIDs - applyFixedWingLaunchIdleLogic(); - } - } - else { - // We are waiting for launch - update launch detector - updateFixedWingLaunchDetector(currentTimeUs); + case FW_LAUNCH_STATE_MOTOR_DELAY: + updateFixedWingLaunchMotorDelay(currentTimeUs); // when finish jump to FW_LAUNCH_STATE_MOTOR_SPIN_UP + break; - // Launch idle logic - low throttle and zero out PIDs - applyFixedWingLaunchIdleLogic(); + case FW_LAUNCH_STATE_MOTOR_SPIN_UP: + updateFixedWingLaunchSpinUp(currentTimeUs); // when finish jump to FW_LAUNCH_STATE_IN_PROGRESS + break; + + case FW_LAUNCH_STATE_IN_PROGRESS: + updateFixedWingLaunchProgress(currentTimeUs); // when finish jump to FW_LAUNCH_STATE_FINISHING + break; + + case FW_LAUNCH_STATE_FINISHING: + updateFixedWingLaunchFinishing(currentTimeUs); // when finish jump to FW_LAUNCH_STATE_FINISHED + break; + + case FW_LAUNCH_STATE_FINISHED: + default: + return; } // Control beeper - if (!launchState.launchFinished) { - beeper(BEEPER_LAUNCH_MODE_ENABLED); - } + beeper(BEEPER_LAUNCH_MODE_ENABLED); +} - // Lock out controls - rcCommand[ROLL] = 0; - rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), pidProfile()->max_angle_inclination[FD_PITCH]); - rcCommand[YAW] = 0; +void resetFixedWingLaunchController(timeUs_t currentTimeUs) +{ + launchState.launchDetectorPreviousUpdate = currentTimeUs; + launchState.launchDetectionTimeAccum = 0; + launchState.launchStartedTimeUs = 0; + setLaunchState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs); +} + +bool isFixedWingLaunchDetected(void) +{ + return launchState.state == FW_LAUNCH_STATE_DETECTED; +} + +void enableFixedWingLaunchController(timeUs_t currentTimeUs) +{ + setLaunchState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs); + launchState.launchStartedTimeUs = currentTimeUs; +} + +bool isFixedWingLaunchFinishedOrAborted(void) +{ + return launchState.state == FW_LAUNCH_STATE_FINISHED; +} + +void abortFixedWingLaunch(void) +{ + launchState.state = FW_LAUNCH_STATE_FINISHED; } #endif From b4448555da1b4d5a7a369b61b706f07d660d945d Mon Sep 17 00:00:00 2001 From: "DESKTOP-N53JVUO\\mix" Date: Thu, 16 Jul 2020 17:03:48 +0300 Subject: [PATCH 004/109] Enable pitch control gradually on launch timeout or altitude reached or the user take control over the sticks, if nav_fw_launch_end_time is set --- src/main/navigation/navigation_fw_launch.c | 26 +++++++++------------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 0df26d791f..4dc0f08939 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -133,15 +133,12 @@ static void fixedWingLaunchResetPids(void) } } -static void updateFixedWingLaunchAbortTriggers(timeUs_t currentTimeUs) +static void updateFixedWingLaunchFinishingTriggers(timeUs_t currentTimeUs) { - if (launchState.state < FW_LAUNCH_STATE_MOTOR_DELAY) { + if (launchState.state < FW_LAUNCH_STATE_MOTOR_DELAY || launchState.state > FW_LAUNCH_STATE_IN_PROGRESS) { return; } - if (areSticksDeflectedMoreThanPosHoldDeadband() && isLaunchModeMinTimeElapsed(currentTimeUs)) { - setLaunchState(FW_LAUNCH_STATE_FINISHED, currentTimeUs); - } - else if (launchState.state == FW_LAUNCH_STATE_IN_PROGRESS && isFixedWingLaunchMaxAltitudeReached()) { + if (isFixedWingLaunchMaxAltitudeReached() || (areSticksDeflectedMoreThanPosHoldDeadband() && isLaunchModeMinTimeElapsed(currentTimeUs))) { setLaunchState(FW_LAUNCH_STATE_FINISHING, currentTimeUs); } } @@ -178,20 +175,18 @@ static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs) static void updateFixedWingLaunchThrottleIdle(timeMs_t currentTimeUs) { - const int throttleIdleVale = getThrottleIdleValue(); - - updateFixedWingLaunchDetector(currentTimeUs); // if the launch is detected, the throttle idle will be skipped and jump to FW_LAUNCH_STATE_DETECTED + const int throttleIdleValue = getThrottleIdleValue(); // Throttle control logic - if (navConfig()->fw.launch_idle_throttle <= throttleIdleVale) { - ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped - rcCommand[THROTTLE] = throttleIdleVale; // If MOTOR_STOP is disabled, motors will spin at minthrottle + if (navConfig()->fw.launch_idle_throttle <= throttleIdleValue) { + ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped + rcCommand[THROTTLE] = throttleIdleValue; // If MOTOR_STOP is disabled, motors will spin at minthrottle setLaunchState(FW_LAUNCH_STATE_WAIT_DETECTION, currentTimeUs); lockLaunchPitchAngle(); } else { timeMs_t elapsedTimeMs = elapsedTimeMs = getElapsedMsAndSetNextState(LAUNCH_MOTOR_IDLE_SPINUP_TIME, currentTimeUs, FW_LAUNCH_STATE_WAIT_DETECTION); - rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, throttleIdleVale, navConfig()->fw.launch_idle_throttle); + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, throttleIdleValue, navConfig()->fw.launch_idle_throttle); updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle)); } } @@ -225,7 +220,7 @@ static void updateFixedWingLaunchFinishing(timeUs_t currentTimeUs) const timeMs_t elapsedTimeMs = getElapsedMsAndSetNextState(endTimeMs, currentTimeUs, FW_LAUNCH_STATE_FINISHED); rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]); - updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, 0)); + updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH])); } // Public methods --------------------------------------------------------------- @@ -234,8 +229,8 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs) { // Called at PID rate + updateFixedWingLaunchFinishingTriggers(currentTimeUs); fixedWingLaunchResetPids(); - updateFixedWingLaunchAbortTriggers(currentTimeUs); switch (launchState.state) { case FW_LAUNCH_STATE_WAIT_THROTTLE: @@ -243,6 +238,7 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs) break; case FW_LAUNCH_STATE_MOTOR_IDLE: + updateFixedWingLaunchDetector(currentTimeUs); // if the launch is detected, the throttle idle will be skipped and jump to FW_LAUNCH_STATE_DETECTED updateFixedWingLaunchThrottleIdle(currentTimeUs); break; From 03e8bce2891169c829efb9107d8c3c1e8250514f Mon Sep 17 00:00:00 2001 From: "DESKTOP-N53JVUO\\mix" Date: Fri, 17 Jul 2020 18:27:54 +0300 Subject: [PATCH 005/109] Partial implementation --- src/main/navigation/navigation_fw_launch.c | 439 +++++++++++++-------- 1 file changed, 283 insertions(+), 156 deletions(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 4dc0f08939..4a1dfa5a2c 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -50,62 +50,137 @@ #define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate #define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms +#define UNUSED(x) ((void)(x)) + +typedef enum { + FW_LAUNCH_EVENT_NONE = 0, + FW_LAUNCH_EVENT_SUCCESS, + FW_LAUNCH_EVENT_GOTO_DETECTION, + FW_LAUNCH_EVENT_CANCEL, + FW_LAUNCH_EVENT_COUNT +} fixedWingLaunchEvent_t; + +typedef enum { + FW_LAUNCH_STATE_INIT = 0, + FW_LAUNCH_STATE_WAIT_THROTTLE, + FW_LAUNCH_STATE_MOTOR_IDLE, + FW_LAUNCH_STATE_WAIT_DETECTION, + FW_LAUNCH_STATE_DETECTED, + FW_LAUNCH_STATE_MOTOR_DELAY, + FW_LAUNCH_STATE_MOTOR_SPINUP, + FW_LAUNCH_STATE_IN_PROGRESS, + FW_LAUNCH_STATE_FINISHING, + FW_LAUNCH_STATE_FINISHED, + FW_LAUNCH_STATE_COUNT +} fixedWingLaunchState_t; + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_INIT(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISHING(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISHED(timeUs_t currentTimeUs); + +typedef struct fixedWingLaunchStateDescriptor_s { + + fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs); + timeUs_t timeReferenceUs; + fixedWingLaunchState_t onEvent[FW_LAUNCH_EVENT_COUNT]; + +} fixedWingLaunchStateDescriptor_t; + +static EXTENDED_FASTRAM fixedWingLaunchState_t currentState; +static EXTENDED_FASTRAM fixedWingLaunchStateDescriptor_t launchSM[FW_LAUNCH_STATE_COUNT] = { + + [FW_LAUNCH_STATE_INIT] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_INIT, + .timeReferenceUs = 0, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_THROTTLE, + [FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_WAIT_DETECTION + } + }, + [FW_LAUNCH_STATE_WAIT_THROTTLE] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE, + .timeReferenceUs = 0, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_IDLE + } + }, + + [FW_LAUNCH_STATE_MOTOR_IDLE] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE, + .timeReferenceUs = 0, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_DETECTION + } + }, + + [FW_LAUNCH_STATE_WAIT_DETECTION] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION, + .timeReferenceUs = 0, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_DETECTED + } + }, + + [FW_LAUNCH_STATE_DETECTED] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_DETECTED, + .timeReferenceUs = 0, + .onEvent = { + // waiting for the navigation to move on the next state FW_LAUNCH_STATE_MOTOR_DELAY + } + }, + + [FW_LAUNCH_STATE_MOTOR_DELAY] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY, + .timeReferenceUs = 0, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_SPINUP, + [FW_LAUNCH_EVENT_CANCEL] = FW_LAUNCH_STATE_FINISHING + } + }, + + [FW_LAUNCH_STATE_MOTOR_SPINUP] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP, + .timeReferenceUs = 0, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IN_PROGRESS, + [FW_LAUNCH_EVENT_CANCEL] = FW_LAUNCH_STATE_FINISHING + } + }, + + [FW_LAUNCH_STATE_IN_PROGRESS] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS, + .timeReferenceUs = 0, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISHING, + } + }, + + [FW_LAUNCH_STATE_FINISHING] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISHING, + .timeReferenceUs = 0, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISHED + } + }, + + [FW_LAUNCH_STATE_FINISHED] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISHED, + .timeReferenceUs = 0, + .onEvent = { + + } + }, -enum { - FW_LAUNCH_STATE_WAIT_THROTTLE = 0, - FW_LAUNCH_STATE_MOTOR_IDLE = 1, - FW_LAUNCH_STATE_WAIT_DETECTION = 2, - FW_LAUNCH_STATE_DETECTED = 3, - FW_LAUNCH_STATE_MOTOR_DELAY = 4, - FW_LAUNCH_STATE_MOTOR_SPIN_UP = 5, - FW_LAUNCH_STATE_IN_PROGRESS = 6, - FW_LAUNCH_STATE_FINISHING = 7, - FW_LAUNCH_STATE_FINISHED = 8 }; -typedef struct FixedWingLaunchState_s { - /* Launch detection */ - timeUs_t launchDetectorPreviousUpdate; - timeUs_t launchDetectionTimeAccum; - int state; - timeUs_t stateStartedTimeUs; - timeUs_t launchStartedTimeUs; -} FixedWingLaunchState_t; - -static EXTENDED_FASTRAM FixedWingLaunchState_t launchState; - - -static void setLaunchState(int state, timeUs_t currentTimeUs) -{ - launchState.state = state; - launchState.stateStartedTimeUs = currentTimeUs; -} - -static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) -{ - return US2MS(currentTimeUs - launchState.stateStartedTimeUs); -} - -static timeMs_t getElapsedMsAndSetNextState(uint16_t input, timeUs_t currentTimeUs, int nextState) -{ - const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); - - if (input == 0 || input < elapsedTimeMs) { - setLaunchState(nextState, currentTimeUs); - } - return elapsedTimeMs; -} - -static inline bool isFixedWingLaunchMaxAltitudeReached(void) -{ - return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); -} - -static inline bool isLaunchModeMinTimeElapsed(float currentTimeUs) -{ - return US2MS(currentTimeUs - launchState.launchStartedTimeUs) > navConfig()->fw.launch_min_time; -} static void updateFixedWingLaunchPitchAngle(uint8_t pitchAngle) { @@ -114,44 +189,96 @@ static void updateFixedWingLaunchPitchAngle(uint8_t pitchAngle) rcCommand[YAW] = 0; } -static void lockLaunchPitchAngle(void) +static bool isFixedWingLaunchIdleEnabled(void) +{ + return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); +} + +static void applyLaunchPitchAngle(void) { updateFixedWingLaunchPitchAngle(navConfig()->fw.launch_climb_angle); } -static void lockLaunchThrottleIdle(void) +static void applyLaunchThrottleIdle(void) { - rcCommand[THROTTLE] = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); + if (isFixedWingLaunchIdleEnabled()) { + rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle; + } + else { + ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped + rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle + } } static void fixedWingLaunchResetPids(void) { // Until motors are started don't use PID I-term and reset TPA filter - if (launchState.state < FW_LAUNCH_STATE_MOTOR_SPIN_UP) { + if (currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) { pidResetErrorAccumulators(); pidResetTPAFilter(); } } -static void updateFixedWingLaunchFinishingTriggers(timeUs_t currentTimeUs) +static inline bool isFixedWingLaunchMaxAltitudeReached(void) { - if (launchState.state < FW_LAUNCH_STATE_MOTOR_DELAY || launchState.state > FW_LAUNCH_STATE_IN_PROGRESS) { - return; + return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); +} + +static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) +{ + return US2MS(currentTimeUs - launchSM[currentState].timeReferenceUs); +} + +static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) { + currentState = nextState; + launchSM[currentState].timeReferenceUs = currentTimeUs; +} + +static inline bool fwLaunchShouldCancel(timeMs_t initialTime, timeUs_t currentTimeUs) +{ + return initialTime + currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband(); +} + +// onEntry state handlers ------------------------------------------------------ + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_INIT(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + if (isFixedWingLaunchIdleEnabled()) { + return FW_LAUNCH_EVENT_SUCCESS; } - if (isFixedWingLaunchMaxAltitudeReached() || (areSticksDeflectedMoreThanPosHoldDeadband() && isLaunchModeMinTimeElapsed(currentTimeUs))) { - setLaunchState(FW_LAUNCH_STATE_FINISHING, currentTimeUs); + else { + return FW_LAUNCH_EVENT_GOTO_DETECTION; } } -static void updateFixedWingLaunchThrottleWait(timeUs_t currentTimeUs) +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); + if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) { - setLaunchState(FW_LAUNCH_STATE_MOTOR_IDLE, currentTimeUs); + return FW_LAUNCH_EVENT_SUCCESS; } - rcCommand[THROTTLE] = getThrottleIdleValue(); + return FW_LAUNCH_EVENT_NONE; } -static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs) +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) +{ + const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); + if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) { + applyLaunchThrottleIdle(); + applyLaunchPitchAngle(); + return FW_LAUNCH_EVENT_SUCCESS; + } + else { + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); + updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle)); + } + return FW_LAUNCH_EVENT_NONE; +} + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) { const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0; const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh); @@ -160,67 +287,105 @@ static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs) const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); + applyLaunchThrottleIdle(); + applyLaunchPitchAngle(); + if (isBungeeLaunched || isSwingLaunched) { - launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate); - launchState.launchDetectorPreviousUpdate = currentTimeUs; - if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) { - setLaunchState(FW_LAUNCH_STATE_DETECTED, currentTimeUs); + if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { + return FW_LAUNCH_EVENT_SUCCESS; } } else { - launchState.launchDetectorPreviousUpdate = currentTimeUs; - launchState.launchDetectionTimeAccum = 0; + setCurrentState(FW_LAUNCH_STATE_WAIT_DETECTION, currentTimeUs); // remain in this state and reset the state counter } + return FW_LAUNCH_EVENT_NONE; } -static void updateFixedWingLaunchThrottleIdle(timeMs_t currentTimeUs) +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) { - const int throttleIdleValue = getThrottleIdleValue(); + UNUSED(currentTimeUs); - // Throttle control logic - if (navConfig()->fw.launch_idle_throttle <= throttleIdleValue) { - ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped - rcCommand[THROTTLE] = throttleIdleValue; // If MOTOR_STOP is disabled, motors will spin at minthrottle - setLaunchState(FW_LAUNCH_STATE_WAIT_DETECTION, currentTimeUs); - lockLaunchPitchAngle(); + applyLaunchThrottleIdle(); + applyLaunchPitchAngle(); + return FW_LAUNCH_EVENT_NONE; +} + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) +{ + applyLaunchThrottleIdle(); + applyLaunchPitchAngle(); + + if (fwLaunchShouldCancel(0, currentTimeUs)) { + return FW_LAUNCH_EVENT_CANCEL; + } + if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) { + return FW_LAUNCH_EVENT_SUCCESS; + } + return FW_LAUNCH_EVENT_NONE; +} + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) +{ + applyLaunchPitchAngle(); + + const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); + + if (fwLaunchShouldCancel(navConfig()->fw.launch_motor_timer, currentTimeUs)) { + return FW_LAUNCH_EVENT_CANCEL; + } + + const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time; + const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); + const uint16_t launchThrottle = navConfig()->fw.launch_throttle; + + + if (elapsedTimeMs > motorSpinUpMs) { + rcCommand[THROTTLE] = launchThrottle; + return FW_LAUNCH_EVENT_SUCCESS; } else { - timeMs_t elapsedTimeMs = elapsedTimeMs = getElapsedMsAndSetNextState(LAUNCH_MOTOR_IDLE_SPINUP_TIME, currentTimeUs, FW_LAUNCH_STATE_WAIT_DETECTION); - rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, throttleIdleValue, navConfig()->fw.launch_idle_throttle); - updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle)); + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle); } + return FW_LAUNCH_EVENT_NONE; } -static void updateFixedWingLaunchMotorDelay(timeUs_t currentTimeUs) { - getElapsedMsAndSetNextState(navConfig()->fw.launch_motor_timer, currentTimeUs, FW_LAUNCH_STATE_MOTOR_SPIN_UP); - lockLaunchThrottleIdle(); - lockLaunchPitchAngle(); -} - -static void updateFixedWingLaunchSpinUp(timeUs_t currentTimeUs) +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) { - const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time; - const timeMs_t elapsedTimeMs = getElapsedMsAndSetNextState(motorSpinUpMs, currentTimeUs, FW_LAUNCH_STATE_IN_PROGRESS); - const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); - - rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, navConfig()->fw.launch_throttle); - lockLaunchPitchAngle(); -} - -static void updateFixedWingLaunchProgress(timeUs_t currentTimeUs) -{ - getElapsedMsAndSetNextState(navConfig()->fw.launch_timeout, currentTimeUs, FW_LAUNCH_STATE_FINISHING); rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; - lockLaunchPitchAngle(); + applyLaunchPitchAngle(); + + if (isFixedWingLaunchMaxAltitudeReached()) { + return FW_LAUNCH_EVENT_SUCCESS; // We're on air, cancel the launch and do the FW_LAUNCH_STATE_FINISHING state + } + if (fwLaunchShouldCancel(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) { + return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISHING state + } + if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) { + return FW_LAUNCH_EVENT_SUCCESS; + } + return FW_LAUNCH_EVENT_NONE; } -static void updateFixedWingLaunchFinishing(timeUs_t currentTimeUs) +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISHING(timeUs_t currentTimeUs) { - const uint16_t endTimeMs = navConfig()->fw.launch_end_time; - const timeMs_t elapsedTimeMs = getElapsedMsAndSetNextState(endTimeMs, currentTimeUs, FW_LAUNCH_STATE_FINISHED); + const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); + const timeMs_t endTimeMs = navConfig()->fw.launch_end_time; - rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]); - updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH])); + if (elapsedTimeMs > endTimeMs) { + return FW_LAUNCH_EVENT_SUCCESS; + } + else { + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]); + updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH])); + } + return FW_LAUNCH_EVENT_NONE; +} + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISHED(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + return FW_LAUNCH_EVENT_NONE; } // Public methods --------------------------------------------------------------- @@ -229,50 +394,16 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs) { // Called at PID rate - updateFixedWingLaunchFinishingTriggers(currentTimeUs); fixedWingLaunchResetPids(); - switch (launchState.state) { - case FW_LAUNCH_STATE_WAIT_THROTTLE: - updateFixedWingLaunchThrottleWait(currentTimeUs); // raise throttle stick to jump to FW_LAUNCH_STATE_MOTOR_IDLE and activate detection + while (launchSM[currentState].onEntry) { + fixedWingLaunchEvent_t newEvent = launchSM[currentState].onEntry(currentTimeUs); + if (newEvent != FW_LAUNCH_EVENT_NONE) { + setCurrentState(launchSM[currentState].onEvent[newEvent], currentTimeUs); + } + else { break; - - case FW_LAUNCH_STATE_MOTOR_IDLE: - updateFixedWingLaunchDetector(currentTimeUs); // if the launch is detected, the throttle idle will be skipped and jump to FW_LAUNCH_STATE_DETECTED - updateFixedWingLaunchThrottleIdle(currentTimeUs); - break; - - case FW_LAUNCH_STATE_WAIT_DETECTION: - updateFixedWingLaunchDetector(currentTimeUs); // when the launch is detected, we jump to FW_LAUNCH_STATE_DETECTED - lockLaunchThrottleIdle(); - lockLaunchPitchAngle(); - break; - - case FW_LAUNCH_STATE_DETECTED: - lockLaunchThrottleIdle(); - lockLaunchPitchAngle(); - // nothing else to do, wait for the navigation to switch to NAV_LAUNCH_STATE_MOTOR_DELAY state by calling enableFixedWingLaunchController() - break; - - case FW_LAUNCH_STATE_MOTOR_DELAY: - updateFixedWingLaunchMotorDelay(currentTimeUs); // when finish jump to FW_LAUNCH_STATE_MOTOR_SPIN_UP - break; - - case FW_LAUNCH_STATE_MOTOR_SPIN_UP: - updateFixedWingLaunchSpinUp(currentTimeUs); // when finish jump to FW_LAUNCH_STATE_IN_PROGRESS - break; - - case FW_LAUNCH_STATE_IN_PROGRESS: - updateFixedWingLaunchProgress(currentTimeUs); // when finish jump to FW_LAUNCH_STATE_FINISHING - break; - - case FW_LAUNCH_STATE_FINISHING: - updateFixedWingLaunchFinishing(currentTimeUs); // when finish jump to FW_LAUNCH_STATE_FINISHED - break; - - case FW_LAUNCH_STATE_FINISHED: - default: - return; + } } // Control beeper @@ -281,31 +412,27 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs) void resetFixedWingLaunchController(timeUs_t currentTimeUs) { - launchState.launchDetectorPreviousUpdate = currentTimeUs; - launchState.launchDetectionTimeAccum = 0; - launchState.launchStartedTimeUs = 0; - setLaunchState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs); + setCurrentState(FW_LAUNCH_STATE_INIT, currentTimeUs); } bool isFixedWingLaunchDetected(void) { - return launchState.state == FW_LAUNCH_STATE_DETECTED; + return currentState == FW_LAUNCH_STATE_DETECTED; } void enableFixedWingLaunchController(timeUs_t currentTimeUs) { - setLaunchState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs); - launchState.launchStartedTimeUs = currentTimeUs; + setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs); } bool isFixedWingLaunchFinishedOrAborted(void) { - return launchState.state == FW_LAUNCH_STATE_FINISHED; + return currentState == FW_LAUNCH_STATE_FINISHED; } void abortFixedWingLaunch(void) { - launchState.state = FW_LAUNCH_STATE_FINISHED; + setCurrentState(FW_LAUNCH_STATE_FINISHED, 0); } #endif From 7583d44cd83a6babff340accab84c62c47b782e5 Mon Sep 17 00:00:00 2001 From: "DESKTOP-N53JVUO\\mix" Date: Sat, 18 Jul 2020 22:53:00 +0300 Subject: [PATCH 006/109] Partial implementation --- src/main/navigation/navigation_fw_launch.c | 242 +++++++++------------ 1 file changed, 100 insertions(+), 142 deletions(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 4a1dfa5a2c..7d1c275486 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -56,12 +56,13 @@ typedef enum { FW_LAUNCH_EVENT_NONE = 0, FW_LAUNCH_EVENT_SUCCESS, FW_LAUNCH_EVENT_GOTO_DETECTION, - FW_LAUNCH_EVENT_CANCEL, + FW_LAUNCH_EVENT_ABORT, FW_LAUNCH_EVENT_COUNT } fixedWingLaunchEvent_t; typedef enum { - FW_LAUNCH_STATE_INIT = 0, + FW_LAUNCH_STATE_IDLE = 0, + FW_LAUNCH_STATE_INIT, FW_LAUNCH_STATE_WAIT_THROTTLE, FW_LAUNCH_STATE_MOTOR_IDLE, FW_LAUNCH_STATE_WAIT_DETECTION, @@ -69,8 +70,7 @@ typedef enum { FW_LAUNCH_STATE_MOTOR_DELAY, FW_LAUNCH_STATE_MOTOR_SPINUP, FW_LAUNCH_STATE_IN_PROGRESS, - FW_LAUNCH_STATE_FINISHING, - FW_LAUNCH_STATE_FINISHED, + FW_LAUNCH_STATE_FINISH, FW_LAUNCH_STATE_COUNT } fixedWingLaunchState_t; @@ -82,179 +82,164 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t cu static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISHING(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISHED(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs); typedef struct fixedWingLaunchStateDescriptor_s { fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs); - timeUs_t timeReferenceUs; fixedWingLaunchState_t onEvent[FW_LAUNCH_EVENT_COUNT]; } fixedWingLaunchStateDescriptor_t; -static EXTENDED_FASTRAM fixedWingLaunchState_t currentState; -static EXTENDED_FASTRAM fixedWingLaunchStateDescriptor_t launchSM[FW_LAUNCH_STATE_COUNT] = { +static EXTENDED_FASTRAM timeUs_t fwStateTimeUs; +static EXTENDED_FASTRAM fixedWingLaunchState_t fwState; + +static const fixedWingLaunchStateDescriptor_t launchSM[FW_LAUNCH_STATE_COUNT] = { + + [FW_LAUNCH_STATE_IDLE] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE, + .onEvent = { + + } + }, [FW_LAUNCH_STATE_INIT] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_INIT, - .timeReferenceUs = 0, + .onEntry = fwLaunchState_FW_LAUNCH_STATE_INIT, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_THROTTLE, - [FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_WAIT_DETECTION + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_THROTTLE, + [FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_WAIT_DETECTION } }, [FW_LAUNCH_STATE_WAIT_THROTTLE] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE, - .timeReferenceUs = 0, + .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_IDLE + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_IDLE } }, [FW_LAUNCH_STATE_MOTOR_IDLE] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE, - .timeReferenceUs = 0, + .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_DETECTION + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_DETECTION } }, [FW_LAUNCH_STATE_WAIT_DETECTION] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION, - .timeReferenceUs = 0, + .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_DETECTED + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_DETECTED } }, [FW_LAUNCH_STATE_DETECTED] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_DETECTED, - .timeReferenceUs = 0, + .onEntry = fwLaunchState_FW_LAUNCH_STATE_DETECTED, .onEvent = { // waiting for the navigation to move on the next state FW_LAUNCH_STATE_MOTOR_DELAY } }, [FW_LAUNCH_STATE_MOTOR_DELAY] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY, - .timeReferenceUs = 0, + .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_SPINUP, - [FW_LAUNCH_EVENT_CANCEL] = FW_LAUNCH_STATE_FINISHING + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_SPINUP, + [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_FINISH } }, [FW_LAUNCH_STATE_MOTOR_SPINUP] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP, - .timeReferenceUs = 0, + .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IN_PROGRESS, - [FW_LAUNCH_EVENT_CANCEL] = FW_LAUNCH_STATE_FINISHING + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IN_PROGRESS, + [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_FINISH } }, [FW_LAUNCH_STATE_IN_PROGRESS] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS, - .timeReferenceUs = 0, + .onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISHING, + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISH, } }, - [FW_LAUNCH_STATE_FINISHING] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISHING, - .timeReferenceUs = 0, + [FW_LAUNCH_STATE_FINISH] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISHED + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE } - }, - - [FW_LAUNCH_STATE_FINISHED] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISHED, - .timeReferenceUs = 0, - .onEvent = { - - } - }, - + } }; +/* Current State Handlers */ +static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) { + return US2MS(currentTimeUs - fwStateTimeUs); +} -static void updateFixedWingLaunchPitchAngle(uint8_t pitchAngle) -{ +static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) { + fwState = nextState; + fwStateTimeUs = currentTimeUs; +} + +/* Wing control Helpers */ + +static void updateFixedWingLaunchPitchAngle(uint8_t pitchAngle) { rcCommand[ROLL] = 0; rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[YAW] = 0; } -static bool isFixedWingLaunchIdleEnabled(void) -{ +static bool isFixedWingLaunchIdleEnabled(void) { return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); } -static void applyLaunchPitchAngle(void) -{ +static void applyLaunchPitchAngle(void) { updateFixedWingLaunchPitchAngle(navConfig()->fw.launch_climb_angle); } -static void applyLaunchThrottleIdle(void) -{ +static void applyLaunchThrottleIdle(void) { if (isFixedWingLaunchIdleEnabled()) { rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle; - } - else { + } else { ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle } } -static void fixedWingLaunchResetPids(void) -{ +static void fixedWingLaunchResetPids(void) { // Until motors are started don't use PID I-term and reset TPA filter - if (currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) { + if (fwState < FW_LAUNCH_STATE_MOTOR_SPINUP) { pidResetErrorAccumulators(); pidResetTPAFilter(); } } -static inline bool isFixedWingLaunchMaxAltitudeReached(void) -{ +static inline bool isFixedWingLaunchMaxAltitudeReached(void) { return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); } -static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) -{ - return US2MS(currentTimeUs - launchSM[currentState].timeReferenceUs); -} - -static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) { - currentState = nextState; - launchSM[currentState].timeReferenceUs = currentTimeUs; -} - -static inline bool fwLaunchShouldCancel(timeMs_t initialTime, timeUs_t currentTimeUs) -{ +static inline bool fwLaunchShouldAbort(timeMs_t initialTime, timeUs_t currentTimeUs) { return initialTime + currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband(); } -// onEntry state handlers ------------------------------------------------------ +/* onEntry state handlers */ -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_INIT(timeUs_t currentTimeUs) -{ +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); + + return FW_LAUNCH_EVENT_NONE; +} + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_INIT(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); if (isFixedWingLaunchIdleEnabled()) { return FW_LAUNCH_EVENT_SUCCESS; - } - else { + } else { return FW_LAUNCH_EVENT_GOTO_DETECTION; } } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) -{ +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) { @@ -263,23 +248,20 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) -{ +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) { const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) { applyLaunchThrottleIdle(); applyLaunchPitchAngle(); return FW_LAUNCH_EVENT_SUCCESS; - } - else { + } else { rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle)); } return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) -{ +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) { const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0; const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh); const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle))); @@ -294,15 +276,13 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { return FW_LAUNCH_EVENT_SUCCESS; } - } - else { - setCurrentState(FW_LAUNCH_STATE_WAIT_DETECTION, currentTimeUs); // remain in this state and reset the state counter + } else { + fwStateTimeUs = currentTimeUs; // reset the state counter } return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) -{ +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); applyLaunchThrottleIdle(); @@ -310,13 +290,12 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t cu return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) -{ +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) { applyLaunchThrottleIdle(); applyLaunchPitchAngle(); - if (fwLaunchShouldCancel(0, currentTimeUs)) { - return FW_LAUNCH_EVENT_CANCEL; + if (fwLaunchShouldAbort(0, currentTimeUs)) { + return FW_LAUNCH_EVENT_ABORT; } if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) { return FW_LAUNCH_EVENT_SUCCESS; @@ -324,41 +303,37 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) -{ +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) { applyLaunchPitchAngle(); const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); - if (fwLaunchShouldCancel(navConfig()->fw.launch_motor_timer, currentTimeUs)) { - return FW_LAUNCH_EVENT_CANCEL; + if (fwLaunchShouldAbort(navConfig()->fw.launch_motor_timer, currentTimeUs)) { + return FW_LAUNCH_EVENT_ABORT; } const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time; const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); const uint16_t launchThrottle = navConfig()->fw.launch_throttle; - if (elapsedTimeMs > motorSpinUpMs) { rcCommand[THROTTLE] = launchThrottle; return FW_LAUNCH_EVENT_SUCCESS; - } - else { + } else { rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle); } return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) -{ +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) { rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; applyLaunchPitchAngle(); if (isFixedWingLaunchMaxAltitudeReached()) { - return FW_LAUNCH_EVENT_SUCCESS; // We're on air, cancel the launch and do the FW_LAUNCH_STATE_FINISHING state + return FW_LAUNCH_EVENT_SUCCESS; // We're on air, cancel the launch and do the FW_LAUNCH_STATE_FINISH state } - if (fwLaunchShouldCancel(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) { - return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISHING state + if (fwLaunchShouldAbort(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) { + return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state } if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) { return FW_LAUNCH_EVENT_SUCCESS; @@ -366,73 +341,56 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISHING(timeUs_t currentTimeUs) -{ +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs) { const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t endTimeMs = navConfig()->fw.launch_end_time; if (elapsedTimeMs > endTimeMs) { return FW_LAUNCH_EVENT_SUCCESS; - } - else { + } else { rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]); updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH])); } return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISHED(timeUs_t currentTimeUs) -{ - UNUSED(currentTimeUs); - - return FW_LAUNCH_EVENT_NONE; -} - // Public methods --------------------------------------------------------------- -void applyFixedWingLaunchController(timeUs_t currentTimeUs) -{ +void applyFixedWingLaunchController(timeUs_t currentTimeUs) { // Called at PID rate fixedWingLaunchResetPids(); - while (launchSM[currentState].onEntry) { - fixedWingLaunchEvent_t newEvent = launchSM[currentState].onEntry(currentTimeUs); - if (newEvent != FW_LAUNCH_EVENT_NONE) { - setCurrentState(launchSM[currentState].onEvent[newEvent], currentTimeUs); - } - else { + while (launchSM[fwState].onEntry) { + fixedWingLaunchEvent_t newEvent = launchSM[fwState].onEntry(currentTimeUs); + if (newEvent == FW_LAUNCH_EVENT_NONE) { break; } + setCurrentState(launchSM[fwState].onEvent[newEvent], currentTimeUs); } // Control beeper beeper(BEEPER_LAUNCH_MODE_ENABLED); } -void resetFixedWingLaunchController(timeUs_t currentTimeUs) -{ +void resetFixedWingLaunchController(timeUs_t currentTimeUs) { setCurrentState(FW_LAUNCH_STATE_INIT, currentTimeUs); } -bool isFixedWingLaunchDetected(void) -{ - return currentState == FW_LAUNCH_STATE_DETECTED; +bool isFixedWingLaunchDetected(void) { + return fwState == FW_LAUNCH_STATE_DETECTED; } -void enableFixedWingLaunchController(timeUs_t currentTimeUs) -{ +void enableFixedWingLaunchController(timeUs_t currentTimeUs) { setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs); } -bool isFixedWingLaunchFinishedOrAborted(void) -{ - return currentState == FW_LAUNCH_STATE_FINISHED; +bool isFixedWingLaunchFinishedOrAborted(void) { + return fwState == FW_LAUNCH_STATE_IDLE; } -void abortFixedWingLaunch(void) -{ - setCurrentState(FW_LAUNCH_STATE_FINISHED, 0); +void abortFixedWingLaunch(void) { + setCurrentState(FW_LAUNCH_STATE_IDLE, 0); } #endif From f4a2589b4eaff37f9c3b435ebb5b02279f4b1e9f Mon Sep 17 00:00:00 2001 From: "DESKTOP-N53JVUO\\mix" Date: Sun, 19 Jul 2020 01:34:11 +0300 Subject: [PATCH 007/109] Added Autolaunch messages --- dev/vscode/launch.json | 2 +- src/main/io/osd.c | 6 +++++- src/main/navigation/navigation.h | 1 + src/main/navigation/navigation_fw_launch.c | 13 +++++++++++++ 4 files changed, 20 insertions(+), 2 deletions(-) diff --git a/dev/vscode/launch.json b/dev/vscode/launch.json index 55af041e7d..d6261de5d8 100644 --- a/dev/vscode/launch.json +++ b/dev/vscode/launch.json @@ -17,7 +17,7 @@ ], "preLaunchCommands": ["monitor arm semihosting enable"], "preLaunchTask": "openocd-debug-prepare", - "svdFile": "./obj/main/${config:TARGET}/svd.svd", + "svdFile": "./obj/main/${config:TARGET}/svd.svd" } ] } diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f7a0f2d943..f465b385d8 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2131,7 +2131,11 @@ static bool osdDrawSingleElement(uint8_t item) messages[messageCount++] = navStateMessage; } } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { - messages[messageCount++] = "AUTOLAUNCH"; + messages[messageCount++] = "AUTOLAUNCH"; + const char *launchStateMessage = fwLaunchStateMessage(); + if (launchStateMessage) { + messages[messageCount++] = launchStateMessage; + } } else { if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 667bb39e49..4c82cf881f 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -483,6 +483,7 @@ bool navigationRTHAllowsLanding(void); bool isNavLaunchEnabled(void); bool isFixedWingLaunchDetected(void); bool isFixedWingLaunchFinishedOrAborted(void); +const char * fwLaunchStateMessage(void); float calculateAverageSpeed(void); diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 7d1c275486..76b9a0414f 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -74,6 +74,7 @@ typedef enum { FW_LAUNCH_STATE_COUNT } fixedWingLaunchState_t; +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_INIT(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs); @@ -393,4 +394,16 @@ void abortFixedWingLaunch(void) { setCurrentState(FW_LAUNCH_STATE_IDLE, 0); } +const char * fwLaunchStateMessage(void) { + if (fwState < FW_LAUNCH_STATE_WAIT_DETECTION) { + return "RAISE THE THROTTLE"; + } else if (fwState == FW_LAUNCH_STATE_WAIT_DETECTION) { + return "READY"; + } else if (fwState > FW_LAUNCH_STATE_DETECTED) { + return "MOVE STICKS TO TAKE CONTROL"; // conforming to OSD_MESSAGE_LENGTH = 28 from osd.c + } else { + return NULL; + } +} + #endif From 605efa80577f9febc7f13e704efd94734118ba23 Mon Sep 17 00:00:00 2001 From: "DESKTOP-N53JVUO\\mix" Date: Sun, 19 Jul 2020 21:31:53 +0300 Subject: [PATCH 008/109] Refinements --- src/main/io/osd.c | 2 +- src/main/navigation/navigation.h | 2 +- src/main/navigation/navigation_fw_launch.c | 187 ++++++++++----------- 3 files changed, 92 insertions(+), 99 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f465b385d8..b80528963f 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2132,7 +2132,7 @@ static bool osdDrawSingleElement(uint8_t item) } } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { messages[messageCount++] = "AUTOLAUNCH"; - const char *launchStateMessage = fwLaunchStateMessage(); + const char *launchStateMessage = fixedWingLaunchStateMessage(); if (launchStateMessage) { messages[messageCount++] = launchStateMessage; } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 4c82cf881f..eb7f304417 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -483,7 +483,7 @@ bool navigationRTHAllowsLanding(void); bool isNavLaunchEnabled(void); bool isFixedWingLaunchDetected(void); bool isFixedWingLaunchFinishedOrAborted(void); -const char * fwLaunchStateMessage(void); +const char * fixedWingLaunchStateMessage(void); float calculateAverageSpeed(void); diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 76b9a0414f..03200dfc55 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -57,12 +57,12 @@ typedef enum { FW_LAUNCH_EVENT_SUCCESS, FW_LAUNCH_EVENT_GOTO_DETECTION, FW_LAUNCH_EVENT_ABORT, + FW_LAUNCH_EVENT_THROTTLE_LOW, FW_LAUNCH_EVENT_COUNT } fixedWingLaunchEvent_t; typedef enum { FW_LAUNCH_STATE_IDLE = 0, - FW_LAUNCH_STATE_INIT, FW_LAUNCH_STATE_WAIT_THROTTLE, FW_LAUNCH_STATE_MOTOR_IDLE, FW_LAUNCH_STATE_WAIT_DETECTION, @@ -75,7 +75,6 @@ typedef enum { } fixedWingLaunchState_t; static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_INIT(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs); @@ -92,10 +91,11 @@ typedef struct fixedWingLaunchStateDescriptor_s { } fixedWingLaunchStateDescriptor_t; -static EXTENDED_FASTRAM timeUs_t fwStateTimeUs; -static EXTENDED_FASTRAM fixedWingLaunchState_t fwState; +static EXTENDED_FASTRAM timeUs_t currentStateTimeUs; +static EXTENDED_FASTRAM fixedWingLaunchState_t currentState; +static EXTENDED_FASTRAM uint8_t pitchAngle; -static const fixedWingLaunchStateDescriptor_t launchSM[FW_LAUNCH_STATE_COUNT] = { +static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = { [FW_LAUNCH_STATE_IDLE] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE, @@ -103,42 +103,33 @@ static const fixedWingLaunchStateDescriptor_t launchSM[FW_LAUNCH_STATE_COUNT] = } }, - - [FW_LAUNCH_STATE_INIT] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_INIT, - .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_THROTTLE, - [FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_WAIT_DETECTION - } - }, [FW_LAUNCH_STATE_WAIT_THROTTLE] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_IDLE + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_IDLE, + [FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_WAIT_DETECTION } }, - [FW_LAUNCH_STATE_MOTOR_IDLE] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_DETECTION + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_DETECTION, + [FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE } }, - [FW_LAUNCH_STATE_WAIT_DETECTION] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_DETECTED + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_DETECTED, + [FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE } }, - [FW_LAUNCH_STATE_DETECTED] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_DETECTED, .onEvent = { // waiting for the navigation to move on the next state FW_LAUNCH_STATE_MOTOR_DELAY } }, - [FW_LAUNCH_STATE_MOTOR_DELAY] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY, .onEvent = { @@ -146,7 +137,6 @@ static const fixedWingLaunchStateDescriptor_t launchSM[FW_LAUNCH_STATE_COUNT] = [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_FINISH } }, - [FW_LAUNCH_STATE_MOTOR_SPINUP] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP, .onEvent = { @@ -154,14 +144,12 @@ static const fixedWingLaunchStateDescriptor_t launchSM[FW_LAUNCH_STATE_COUNT] = [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_FINISH } }, - [FW_LAUNCH_STATE_IN_PROGRESS] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISH, } }, - [FW_LAUNCH_STATE_FINISH] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH, .onEvent = { @@ -173,32 +161,22 @@ static const fixedWingLaunchStateDescriptor_t launchSM[FW_LAUNCH_STATE_COUNT] = /* Current State Handlers */ static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) { - return US2MS(currentTimeUs - fwStateTimeUs); + return US2MS(currentTimeUs - currentStateTimeUs); } static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) { - fwState = nextState; - fwStateTimeUs = currentTimeUs; + currentState = nextState; + currentStateTimeUs = currentTimeUs; } /* Wing control Helpers */ -static void updateFixedWingLaunchPitchAngle(uint8_t pitchAngle) { - rcCommand[ROLL] = 0; - rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]); - rcCommand[YAW] = 0; -} - -static bool isFixedWingLaunchIdleEnabled(void) { +static bool isThrottleIdleEnabled(void) { return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); } -static void applyLaunchPitchAngle(void) { - updateFixedWingLaunchPitchAngle(navConfig()->fw.launch_climb_angle); -} - -static void applyLaunchThrottleIdle(void) { - if (isFixedWingLaunchIdleEnabled()) { +static void applyThrottleIdle(void) { + if (isThrottleIdleEnabled()) { rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle; } else { ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped @@ -206,20 +184,30 @@ static void applyLaunchThrottleIdle(void) { } } -static void fixedWingLaunchResetPids(void) { +static inline bool isThrottleLow(void) { + return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW; +} + +static inline bool isLaunchMaxAltitudeReached(void) { + return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); +} + +static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) { + return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband(); +} + +static void resetPidsIfNeeded(void) { // Until motors are started don't use PID I-term and reset TPA filter - if (fwState < FW_LAUNCH_STATE_MOTOR_SPINUP) { + if (currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) { pidResetErrorAccumulators(); pidResetTPAFilter(); } } -static inline bool isFixedWingLaunchMaxAltitudeReached(void) { - return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); -} - -static inline bool fwLaunchShouldAbort(timeMs_t initialTime, timeUs_t currentTimeUs) { - return initialTime + currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband(); +static void updateRcCommand(void) { + rcCommand[ROLL] = 0; + rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]); + rcCommand[YAW] = 0; } /* onEntry state handlers */ @@ -230,39 +218,43 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t curren return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_INIT(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); - - if (isFixedWingLaunchIdleEnabled()) { - return FW_LAUNCH_EVENT_SUCCESS; - } else { - return FW_LAUNCH_EVENT_GOTO_DETECTION; - } -} - static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) { - return FW_LAUNCH_EVENT_SUCCESS; + pitchAngle = 0; + + if (!isThrottleLow()) { + if (isThrottleIdleEnabled()) { + return FW_LAUNCH_EVENT_SUCCESS; + } else { + return FW_LAUNCH_EVENT_GOTO_DETECTION; + } } + return FW_LAUNCH_EVENT_NONE; } static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) { + if (isThrottleLow()) { + return FW_LAUNCH_EVENT_THROTTLE_LOW; + } const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) { - applyLaunchThrottleIdle(); - applyLaunchPitchAngle(); + applyThrottleIdle(); return FW_LAUNCH_EVENT_SUCCESS; } else { rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); - updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle)); + pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle); } + return FW_LAUNCH_EVENT_NONE; } static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) { + if (isThrottleLow()) { + return FW_LAUNCH_EVENT_THROTTLE_LOW; + } + const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0; const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh); const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle))); @@ -270,75 +262,73 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); - applyLaunchThrottleIdle(); - applyLaunchPitchAngle(); + applyThrottleIdle(); if (isBungeeLaunched || isSwingLaunched) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { return FW_LAUNCH_EVENT_SUCCESS; } } else { - fwStateTimeUs = currentTimeUs; // reset the state counter + currentStateTimeUs = currentTimeUs; // reset the state counter } + return FW_LAUNCH_EVENT_NONE; } static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - applyLaunchThrottleIdle(); - applyLaunchPitchAngle(); + applyThrottleIdle(); + return FW_LAUNCH_EVENT_NONE; } static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) { - applyLaunchThrottleIdle(); - applyLaunchPitchAngle(); + applyThrottleIdle(); - if (fwLaunchShouldAbort(0, currentTimeUs)) { + if (areSticksMoved(0, currentTimeUs)) { return FW_LAUNCH_EVENT_ABORT; } if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) { return FW_LAUNCH_EVENT_SUCCESS; } + return FW_LAUNCH_EVENT_NONE; } static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) { - applyLaunchPitchAngle(); - - const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); - - if (fwLaunchShouldAbort(navConfig()->fw.launch_motor_timer, currentTimeUs)) { + if (areSticksMoved(navConfig()->fw.launch_motor_timer, currentTimeUs)) { return FW_LAUNCH_EVENT_ABORT; } + const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time; - const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); const uint16_t launchThrottle = navConfig()->fw.launch_throttle; if (elapsedTimeMs > motorSpinUpMs) { rcCommand[THROTTLE] = launchThrottle; return FW_LAUNCH_EVENT_SUCCESS; } else { + const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle); } + return FW_LAUNCH_EVENT_NONE; } static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) { rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; - applyLaunchPitchAngle(); - if (isFixedWingLaunchMaxAltitudeReached()) { + if (isLaunchMaxAltitudeReached()) { return FW_LAUNCH_EVENT_SUCCESS; // We're on air, cancel the launch and do the FW_LAUNCH_STATE_FINISH state } - if (fwLaunchShouldAbort(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) { + if (areSticksMoved(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) { return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state } if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) { return FW_LAUNCH_EVENT_SUCCESS; } + return FW_LAUNCH_EVENT_NONE; } @@ -350,8 +340,9 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr return FW_LAUNCH_EVENT_SUCCESS; } else { rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]); - updateFixedWingLaunchPitchAngle(scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH])); + pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]); } + return FW_LAUNCH_EVENT_NONE; } @@ -359,27 +350,28 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr void applyFixedWingLaunchController(timeUs_t currentTimeUs) { // Called at PID rate - - fixedWingLaunchResetPids(); - - while (launchSM[fwState].onEntry) { - fixedWingLaunchEvent_t newEvent = launchSM[fwState].onEntry(currentTimeUs); + + while (launchStateMachine[currentState].onEntry) { + fixedWingLaunchEvent_t newEvent = launchStateMachine[currentState].onEntry(currentTimeUs); if (newEvent == FW_LAUNCH_EVENT_NONE) { break; } - setCurrentState(launchSM[fwState].onEvent[newEvent], currentTimeUs); + setCurrentState(launchStateMachine[currentState].onEvent[newEvent], currentTimeUs); } + resetPidsIfNeeded(); + updateRcCommand(); + // Control beeper beeper(BEEPER_LAUNCH_MODE_ENABLED); } void resetFixedWingLaunchController(timeUs_t currentTimeUs) { - setCurrentState(FW_LAUNCH_STATE_INIT, currentTimeUs); + setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs); } bool isFixedWingLaunchDetected(void) { - return fwState == FW_LAUNCH_STATE_DETECTED; + return currentState == FW_LAUNCH_STATE_DETECTED; } void enableFixedWingLaunchController(timeUs_t currentTimeUs) { @@ -387,22 +379,23 @@ void enableFixedWingLaunchController(timeUs_t currentTimeUs) { } bool isFixedWingLaunchFinishedOrAborted(void) { - return fwState == FW_LAUNCH_STATE_IDLE; + return currentState == FW_LAUNCH_STATE_IDLE; } void abortFixedWingLaunch(void) { setCurrentState(FW_LAUNCH_STATE_IDLE, 0); } -const char * fwLaunchStateMessage(void) { - if (fwState < FW_LAUNCH_STATE_WAIT_DETECTION) { - return "RAISE THE THROTTLE"; - } else if (fwState == FW_LAUNCH_STATE_WAIT_DETECTION) { +const char * fixedWingLaunchStateMessage(void) { + switch (currentState) { + case FW_LAUNCH_STATE_WAIT_DETECTION: return "READY"; - } else if (fwState > FW_LAUNCH_STATE_DETECTED) { - return "MOVE STICKS TO TAKE CONTROL"; // conforming to OSD_MESSAGE_LENGTH = 28 from osd.c - } else { - return NULL; + case FW_LAUNCH_STATE_FINISH: + return "FINISHING"; + case FW_LAUNCH_STATE_WAIT_THROTTLE: + return "RAISE THE THROTTLE"; + default: + return "MOVE THE STICKS TO ABORT"; // conforming to OSD_MESSAGE_LENGTH = 28 from osd.c } } From 169fe5feea02afe44cbada5ebf221ed3a2dda57f Mon Sep 17 00:00:00 2001 From: "DESKTOP-N53JVUO\\mix" Date: Tue, 21 Jul 2020 17:43:36 +0300 Subject: [PATCH 009/109] Move variables into a struct. Refine some states --- src/main/navigation/navigation_fw_launch.c | 69 ++++++++++++---------- 1 file changed, 37 insertions(+), 32 deletions(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 03200dfc55..8bf173b185 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -85,15 +85,17 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs); typedef struct fixedWingLaunchStateDescriptor_s { - - fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs); - fixedWingLaunchState_t onEvent[FW_LAUNCH_EVENT_COUNT]; - + fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs); + fixedWingLaunchState_t onEvent[FW_LAUNCH_EVENT_COUNT]; } fixedWingLaunchStateDescriptor_t; -static EXTENDED_FASTRAM timeUs_t currentStateTimeUs; -static EXTENDED_FASTRAM fixedWingLaunchState_t currentState; -static EXTENDED_FASTRAM uint8_t pitchAngle; +typedef struct fixedWingLaunchData_s { + timeUs_t currentStateTimeUs; + fixedWingLaunchState_t currentState; + uint8_t pitchAngle; // used to smooth the transition of the pitch angle +} fixedWingLaunchData_t; + +static EXTENDED_FASTRAM fixedWingLaunchData_t fwLaunch; static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = { @@ -134,14 +136,14 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_SPINUP, - [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_FINISH + [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE } }, [FW_LAUNCH_STATE_MOTOR_SPINUP] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IN_PROGRESS, - [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_FINISH + [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE } }, [FW_LAUNCH_STATE_IN_PROGRESS] = { @@ -161,12 +163,12 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE /* Current State Handlers */ static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) { - return US2MS(currentTimeUs - currentStateTimeUs); + return US2MS(currentTimeUs - fwLaunch.currentStateTimeUs); } static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) { - currentState = nextState; - currentStateTimeUs = currentTimeUs; + fwLaunch.currentState = nextState; + fwLaunch.currentStateTimeUs = currentTimeUs; } /* Wing control Helpers */ @@ -198,15 +200,16 @@ static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) static void resetPidsIfNeeded(void) { // Until motors are started don't use PID I-term and reset TPA filter - if (currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) { + if (fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) { pidResetErrorAccumulators(); pidResetTPAFilter(); } } static void updateRcCommand(void) { + // lock roll and yaw and apply needed pitch angle rcCommand[ROLL] = 0; - rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]); + rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(fwLaunch.pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[YAW] = 0; } @@ -221,7 +224,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t curren static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - pitchAngle = 0; + fwLaunch.pitchAngle = 0; if (!isThrottleLow()) { if (isThrottleIdleEnabled()) { @@ -236,7 +239,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) { if (isThrottleLow()) { - return FW_LAUNCH_EVENT_THROTTLE_LOW; + return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE } const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) { @@ -244,7 +247,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t return FW_LAUNCH_EVENT_SUCCESS; } else { rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); - pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle); + fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle); } return FW_LAUNCH_EVENT_NONE; @@ -252,7 +255,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) { if (isThrottleLow()) { - return FW_LAUNCH_EVENT_THROTTLE_LOW; + return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE } const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0; @@ -266,10 +269,10 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU if (isBungeeLaunched || isSwingLaunched) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { - return FW_LAUNCH_EVENT_SUCCESS; + return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED } } else { - currentStateTimeUs = currentTimeUs; // reset the state counter + fwLaunch.currentStateTimeUs = currentTimeUs; // reset the state counter } return FW_LAUNCH_EVENT_NONE; @@ -277,7 +280,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - + // waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY applyThrottleIdle(); return FW_LAUNCH_EVENT_NONE; @@ -287,7 +290,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t applyThrottleIdle(); if (areSticksMoved(0, currentTimeUs)) { - return FW_LAUNCH_EVENT_ABORT; + return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE } if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) { return FW_LAUNCH_EVENT_SUCCESS; @@ -298,7 +301,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) { if (areSticksMoved(navConfig()->fw.launch_motor_timer, currentTimeUs)) { - return FW_LAUNCH_EVENT_ABORT; + return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE } const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); @@ -320,13 +323,13 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; if (isLaunchMaxAltitudeReached()) { - return FW_LAUNCH_EVENT_SUCCESS; // We're on air, cancel the launch and do the FW_LAUNCH_STATE_FINISH state + return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state } if (areSticksMoved(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) { return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state } if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) { - return FW_LAUNCH_EVENT_SUCCESS; + return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH } return FW_LAUNCH_EVENT_NONE; @@ -339,8 +342,9 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr if (elapsedTimeMs > endTimeMs) { return FW_LAUNCH_EVENT_SUCCESS; } else { + // make a smooth transition from the launch state to the current state for throttle and the pitch angle rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]); - pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]); + fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]); } return FW_LAUNCH_EVENT_NONE; @@ -351,12 +355,13 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr void applyFixedWingLaunchController(timeUs_t currentTimeUs) { // Called at PID rate - while (launchStateMachine[currentState].onEntry) { - fixedWingLaunchEvent_t newEvent = launchStateMachine[currentState].onEntry(currentTimeUs); + // process the current state, set the next state or exit if FW_LAUNCH_EVENT_NONE + while (launchStateMachine[fwLaunch.currentState].onEntry) { + fixedWingLaunchEvent_t newEvent = launchStateMachine[fwLaunch.currentState].onEntry(currentTimeUs); if (newEvent == FW_LAUNCH_EVENT_NONE) { break; } - setCurrentState(launchStateMachine[currentState].onEvent[newEvent], currentTimeUs); + setCurrentState(launchStateMachine[fwLaunch.currentState].onEvent[newEvent], currentTimeUs); } resetPidsIfNeeded(); @@ -371,7 +376,7 @@ void resetFixedWingLaunchController(timeUs_t currentTimeUs) { } bool isFixedWingLaunchDetected(void) { - return currentState == FW_LAUNCH_STATE_DETECTED; + return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED; } void enableFixedWingLaunchController(timeUs_t currentTimeUs) { @@ -379,7 +384,7 @@ void enableFixedWingLaunchController(timeUs_t currentTimeUs) { } bool isFixedWingLaunchFinishedOrAborted(void) { - return currentState == FW_LAUNCH_STATE_IDLE; + return fwLaunch.currentState == FW_LAUNCH_STATE_IDLE; } void abortFixedWingLaunch(void) { @@ -387,7 +392,7 @@ void abortFixedWingLaunch(void) { } const char * fixedWingLaunchStateMessage(void) { - switch (currentState) { + switch (fwLaunch.currentState) { case FW_LAUNCH_STATE_WAIT_DETECTION: return "READY"; case FW_LAUNCH_STATE_FINISH: From ad1271a2dc539a5564a797b643462fab0296c1fb Mon Sep 17 00:00:00 2001 From: "DESKTOP-N53JVUO\\mix" Date: Mon, 27 Jul 2020 11:19:44 +0300 Subject: [PATCH 010/109] Fixed autolaunch messages --- src/main/navigation/navigation_fw_launch.c | 60 +++++++++++++++------- 1 file changed, 42 insertions(+), 18 deletions(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 8bf173b185..642ca9d227 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -51,6 +51,18 @@ #define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate #define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms #define UNUSED(x) ((void)(x)) +#define FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE "RAISE THE THROTTLE" +#define FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION "READY" +#define FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS "MOVE THE STICKS TO ABORT" +#define FW_LAUNCH_MESSAGE_TEXT_FINISHING "FINISHING" + +typedef enum { + FW_LAUNCH_MESSAGE_TYPE_NONE = 0, + FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE, + FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION, + FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS, + FW_LAUNCH_MESSAGE_TYPE_FINISHING +} fixedWingLaunchMessage_t; typedef enum { FW_LAUNCH_EVENT_NONE = 0, @@ -87,6 +99,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr typedef struct fixedWingLaunchStateDescriptor_s { fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs); fixedWingLaunchState_t onEvent[FW_LAUNCH_EVENT_COUNT]; + fixedWingLaunchMessage_t messageType; } fixedWingLaunchStateDescriptor_t; typedef struct fixedWingLaunchData_s { @@ -103,60 +116,69 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE .onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE, .onEvent = { - } + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_NONE }, [FW_LAUNCH_STATE_WAIT_THROTTLE] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_IDLE, [FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_WAIT_DETECTION - } + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE }, [FW_LAUNCH_STATE_MOTOR_IDLE] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_DETECTION, [FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE - } + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE }, [FW_LAUNCH_STATE_WAIT_DETECTION] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_DETECTED, [FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE - } + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION }, [FW_LAUNCH_STATE_DETECTED] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_DETECTED, .onEvent = { // waiting for the navigation to move on the next state FW_LAUNCH_STATE_MOTOR_DELAY - } + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION }, [FW_LAUNCH_STATE_MOTOR_DELAY] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_SPINUP, [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE - } + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS }, [FW_LAUNCH_STATE_MOTOR_SPINUP] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IN_PROGRESS, [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE - } + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS }, [FW_LAUNCH_STATE_IN_PROGRESS] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISH, - } + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS }, [FW_LAUNCH_STATE_FINISH] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE - } + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_FINISHING } }; @@ -392,15 +414,17 @@ void abortFixedWingLaunch(void) { } const char * fixedWingLaunchStateMessage(void) { - switch (fwLaunch.currentState) { - case FW_LAUNCH_STATE_WAIT_DETECTION: - return "READY"; - case FW_LAUNCH_STATE_FINISH: - return "FINISHING"; - case FW_LAUNCH_STATE_WAIT_THROTTLE: - return "RAISE THE THROTTLE"; - default: - return "MOVE THE STICKS TO ABORT"; // conforming to OSD_MESSAGE_LENGTH = 28 from osd.c + switch (launchStateMachine[fwLaunch.currentState].messageType) { + case FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE: + return FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE; + case FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION: + return FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION; + case FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS: + return FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS; + case FW_LAUNCH_MESSAGE_TYPE_FINISHING: + return FW_LAUNCH_MESSAGE_TEXT_FINISHING; + default: + return NULL; } } From 98074c564b24f626809c739769aa862e96a6599b Mon Sep 17 00:00:00 2001 From: "DESKTOP-N53JVUO\\mix" Date: Mon, 27 Jul 2020 11:21:32 +0300 Subject: [PATCH 011/109] Set pitch when we launch it without idle rpm --- src/main/navigation/navigation_fw_launch.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 642ca9d227..ace3e17d3d 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -246,15 +246,15 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t curren static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - fwLaunch.pitchAngle = 0; - if (!isThrottleLow()) { if (isThrottleIdleEnabled()) { return FW_LAUNCH_EVENT_SUCCESS; } else { + fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle; return FW_LAUNCH_EVENT_GOTO_DETECTION; } } + fwLaunch.pitchAngle = 0; return FW_LAUNCH_EVENT_NONE; } From 8c2d6135b01aff5b5b5947bebe2bc90d089c4765 Mon Sep 17 00:00:00 2001 From: "DESKTOP-N53JVUO\\mix" Date: Mon, 27 Jul 2020 15:06:43 +0300 Subject: [PATCH 012/109] Change default end time to 3s. (After a few launches, I found it more pleasant at this value) --- src/main/navigation/navigation.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index cf0a7bc046..304ef3913e 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -160,7 +160,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .launch_idle_throttle = 1000, // Motor idle or MOTOR_STOP .launch_motor_timer = 500, // ms .launch_motor_spinup_time = 100, // ms, time to gredually increase throttle from idle to launch - .launch_end_time = 2000, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode + .launch_end_time = 3000, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode .launch_min_time = 0, // ms, min time in launch mode .launch_timeout = 5000, // ms, timeout for launch procedure .launch_max_altitude = 0, // cm, altitude where to consider launch ended From 1faad08fb57f39423fa3d14347667abde43d72fc Mon Sep 17 00:00:00 2001 From: "DESKTOP-N53JVUO\\mix" Date: Mon, 27 Jul 2020 15:41:03 +0300 Subject: [PATCH 013/109] Added cli command nav_fw_launch_end_time to the docs --- docs/Cli.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/Cli.md b/docs/Cli.md index aaddd99a60..e6ec27e9b9 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -223,6 +223,7 @@ A shorter form is also supported to enable and disable functions using `serial < | nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] | | nav_fw_launch_thr | 1700 | Launch throttle - throttle to be set during launch sequence (pwm units) | | nav_fw_launch_idle_thr | 1000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) | +| nav_fw_launch_end_time | 3000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | | nav_fw_launch_motor_delay | 500 | Delay between detected launch and launch sequence start and throttling up (ms) | | nav_fw_launch_spinup_time | 100 | Time to bring power from minimum throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller | | nav_fw_launch_timeout | 5000 | Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms) | From 0efa55241987e73f810cf12fec7af4b9fcd96f06 Mon Sep 17 00:00:00 2001 From: "DESKTOP-N53JVUO\\mix" Date: Mon, 27 Jul 2020 16:20:45 +0300 Subject: [PATCH 014/109] Added the new cli nav_fw_launch_end_time to the documentation --- docs/Settings.md | 1 + src/main/fc/settings.yaml | 2 ++ 2 files changed, 3 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index 8e3ca620fd..19cb6c477e 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -224,6 +224,7 @@ | nav_fw_launch_min_time | 0 | Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]. | | nav_fw_launch_motor_delay | 500 | Delay between detected launch and launch sequence start and throttling up (ms) | | nav_fw_launch_spinup_time | 100 | Time to bring power from minimum throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller | +| nav_fw_launch_end_time | 3000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | | nav_fw_launch_thr | 1700 | Launch throttle - throttle to be set during launch sequence (pwm units) | | nav_fw_launch_timeout | 5000 | Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms) | | nav_fw_launch_velocity | 300 | Forward velocity threshold for swing-launch detection [cm/s] | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index fa90afbf4c..5258f42c16 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2323,6 +2323,8 @@ groups: min: 0 max: 1000 - name: nav_fw_launch_end_time + description: "Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms]" + default_value: "3000" field: fw.launch_end_time min: 0 max: 5000 From 88a94450f7141f24e5a2119c24c02dd95c903da5 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Tue, 11 Aug 2020 11:06:43 +0200 Subject: [PATCH 015/109] [IMU] Use bell curve to calculate accelerometer weight in IMU calculations --- src/main/flight/imu.c | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 375a128f89..d75f416f8f 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -77,7 +77,7 @@ FILE_COMPILE_FOR_SPEED // http://gentlenav.googlecode.com/files/fastRotations.pdf #define SPIN_RATE_LIMIT 20 -#define MAX_ACC_SQ_NEARNESS 25 // 25% or G^2, accepted acceleration of (0.87 - 1.12G) +#define MAX_ACC_NEARNESS 0.33 // 33% or G error soft-accepted (0.67-1.33G) #define IMU_CENTRIFUGAL_LPF 1 // Hz FASTRAM fpVector3_t imuMeasuredAccelBF; @@ -472,16 +472,12 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) static float imuCalculateAccelerometerWeight(const float dT) { - // If centrifugal test passed - do the usual "nearness" style check float accMagnitudeSq = 0; - for (int axis = 0; axis < 3; axis++) { accMagnitudeSq += acc.accADCf[axis] * acc.accADCf[axis]; } - // Magnitude^2 in percent of G^2 - const float nearness = ABS(100 - (accMagnitudeSq * 100)); - const float accWeight_Nearness = (nearness > MAX_ACC_SQ_NEARNESS) ? 0.0f : 1.0f; + const float accWeight_Nearness = bellCurve(sqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS); // Experiment: if rotation rate on a FIXED_WING_LEGACY is higher than a threshold - centrifugal force messes up too much and we // should not use measured accel for AHRS comp From b8db639a2a992f60978a0a737c592845cac77799 Mon Sep 17 00:00:00 2001 From: Yuxin Date: Tue, 1 Sep 2020 22:52:58 +0800 Subject: [PATCH 016/109] Returns GCS waypoint in the case of waypoint 255 For MSP query in the case of waypoint 255, in this commit the ground control station waypoint is returned, which is consistent with its waypoint set behavior. This is also the desired behavior to affirm users that the correct waypoint is loaded. If GCS assist mode is not enabled, the current position is returned to preserve compatibility. --- src/main/navigation/navigation.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c16aa048eb..b06253ce42 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2782,11 +2782,16 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData) wpData->alt = GPS_home.alt; } } - // WP #255 - special waypoint - directly get actualPosition + // WP #255 - special waypoint - get desiredPosition that was set by ground control station if in GCS assisted mode, otherwise return current position else if (wpNumber == 255) { gpsLocation_t wpLLH; - geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos); + if ((posControl.gpsOrigin.valid) && (posControl.flags.isGCSAssistedNavigationEnabled) && (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) { + geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &posControl.desiredState.pos); + } + else { + geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos); + } wpData->lat = wpLLH.lat; wpData->lon = wpLLH.lon; From 2a76ce302cd75dc0c208ef172dc8c80855090225 Mon Sep 17 00:00:00 2001 From: Yuxin Date: Wed, 2 Sep 2020 22:27:56 +0800 Subject: [PATCH 017/109] add new special waypoint 254 to facilitate desiredPosition in GCS assisted mode --- src/main/navigation/navigation.c | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b06253ce42..6b5c65b55a 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2782,21 +2782,28 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData) wpData->alt = GPS_home.alt; } } - // WP #255 - special waypoint - get desiredPosition that was set by ground control station if in GCS assisted mode, otherwise return current position + // WP #255 - special waypoint - directly get actualPosition else if (wpNumber == 255) { gpsLocation_t wpLLH; - if ((posControl.gpsOrigin.valid) && (posControl.flags.isGCSAssistedNavigationEnabled) && (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) { - geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &posControl.desiredState.pos); - } - else { - geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos); - } + geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos); wpData->lat = wpLLH.lat; wpData->lon = wpLLH.lon; wpData->alt = wpLLH.alt; } + // WP #254 - special waypoint - get desiredPosition that was set by ground control station if in GCS assisted mode + else if (wpNumber == 254) { + if ((posControl.gpsOrigin.valid) && (posControl.flags.isGCSAssistedNavigationEnabled) && (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) { + gpsLocation_t wpLLH; + + geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &posControl.desiredState.pos); + + wpData->lat = wpLLH.lat; + wpData->lon = wpLLH.lon; + wpData->alt = wpLLH.alt; + } + } // WP #1 - #60 - common waypoints - pre-programmed mission else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) { if (wpNumber <= posControl.waypointCount) { From 1df0d5f9020da2fd32e0fe283426fe7f05542323 Mon Sep 17 00:00:00 2001 From: Yuxin Date: Thu, 3 Sep 2020 01:30:44 +0800 Subject: [PATCH 018/109] Add build-time check for NAV_MAX_WAYPOINTS Assert that NAV_MAX_WAYPOINTS is less than 254 --- src/main/navigation/navigation.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 1d8b853902..1ae4f9a45e 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -67,6 +67,9 @@ bool foundNearbySafeHome(void); // Did we find a safehome nearb #ifndef NAV_MAX_WAYPOINTS #define NAV_MAX_WAYPOINTS 15 + +// waypoint 254, 255 are special waypoints +STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_range); #endif #define NAV_ACCEL_CUTOFF_FREQUENCY_HZ 2 // low-pass filter on XY-acceleration target From 0c76e2cb7ae4ea325cee9ebc70c2bcd668a2740c Mon Sep 17 00:00:00 2001 From: Yuxin Pan <0@outlook.it> Date: Thu, 3 Sep 2020 13:15:27 +0800 Subject: [PATCH 019/109] Remove max waypoint assert from navigation.h --- src/main/navigation/navigation.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 1ae4f9a45e..1d8b853902 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -67,9 +67,6 @@ bool foundNearbySafeHome(void); // Did we find a safehome nearb #ifndef NAV_MAX_WAYPOINTS #define NAV_MAX_WAYPOINTS 15 - -// waypoint 254, 255 are special waypoints -STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_range); #endif #define NAV_ACCEL_CUTOFF_FREQUENCY_HZ 2 // low-pass filter on XY-acceleration target From c64753cdc894d5489c1371fb67f0c93ada42a5c1 Mon Sep 17 00:00:00 2001 From: Yuxin Pan <0@outlook.it> Date: Thu, 3 Sep 2020 13:18:11 +0800 Subject: [PATCH 020/109] Move max waypoint STATIC_ASSERT to navigation.c to ensure it is properly triggered --- src/main/navigation/navigation.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 6b5c65b55a..9dc4f2bb67 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -83,6 +83,10 @@ PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CO #endif #if defined(USE_NAV) + +// waypoint 254, 255 are special waypoints +STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_range); + #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); #endif From cb4791cbce62961be588cbcd1c591048efbb2d11 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 3 Sep 2020 10:33:14 +0200 Subject: [PATCH 021/109] Implement fw_turn_assist_pitch_gain for TURN ASSIST to tune amount of pitch input in coordinated turns. --- src/main/fc/settings.yaml | 6 ++++++ src/main/flight/pid.c | 5 +++-- src/main/flight/pid.h | 1 + 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2d65ec4771..875fd7dec4 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1539,6 +1539,12 @@ groups: field: fixedWingCoordinatedYawGain min: 0 max: 2 + - name: fw_turn_assist_pitch_gain + description: "Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter" + default_value: "1" + field: fixedWingCoordinatedPitchGain + min: 0 + max: 2 - name: fw_iterm_limit_stick_position description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side" default_value: "0.5" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c76b36cfec..4ecfa2276b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -154,7 +154,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 15); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 16); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -256,6 +256,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT, .fixedWingReferenceAirspeed = 1000, .fixedWingCoordinatedYawGain = 1.0f, + .fixedWingCoordinatedPitchGain = 1.0f, .fixedWingItermLimitOnStickPosition = 0.5f, .loiter_direction = NAV_LOITER_RIGHT, @@ -889,7 +890,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) // Add in roll and pitch pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f); - pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f); + pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y * pidProfile()->fixedWingCoordinatedPitchGain, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f); // Replace YAW on quads - add it in on airplanes if (STATE(AIRPLANE)) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c9849800af..24bc9ff323 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -126,6 +126,7 @@ typedef struct pidProfile_s { uint16_t fixedWingItermThrowLimit; float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. + float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) From 3951a7a7934ef7f7e64b446eb2dc272b8979ef9c Mon Sep 17 00:00:00 2001 From: Yuxin Pan <0@outlook.it> Date: Thu, 3 Sep 2020 17:22:50 +0800 Subject: [PATCH 022/109] Waypoint 254 access in any 3D-guided mode Access current target position in any 3D-guided mode for waypoint 254 --- src/main/navigation/navigation.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 9dc4f2bb67..8d6b68e13f 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2796,9 +2796,11 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData) wpData->lon = wpLLH.lon; wpData->alt = wpLLH.alt; } - // WP #254 - special waypoint - get desiredPosition that was set by ground control station if in GCS assisted mode + // WP #254 - special waypoint - get desiredPosition that was set by ground control station if in 3D-guided mode else if (wpNumber == 254) { - if ((posControl.gpsOrigin.valid) && (posControl.flags.isGCSAssistedNavigationEnabled) && (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) { + navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); + + if ((posControl.gpsOrigin.valid) && (navStateFlags & NAV_CTL_ALT) && (navStateFlags & NAV_CTL_POS)) { gpsLocation_t wpLLH; geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &posControl.desiredState.pos); From 78da74841e619d41dc9cc1f3d9694fb2c560e176 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 3 Sep 2020 13:45:17 +0200 Subject: [PATCH 023/109] wrap PG back to 0 --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 4ecfa2276b..27e57abc6a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -154,7 +154,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 16); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { From ff2b2a5447da7d794f94d390ac20e93c8e693585 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Fri, 4 Sep 2020 23:31:04 +0200 Subject: [PATCH 024/109] Fix alignment issues --- src/main/flight/pid.c | 2 +- src/main/flight/pid.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 27e57abc6a..34a657b6f3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -256,7 +256,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT, .fixedWingReferenceAirspeed = 1000, .fixedWingCoordinatedYawGain = 1.0f, - .fixedWingCoordinatedPitchGain = 1.0f, + .fixedWingCoordinatedPitchGain = 1.0f, .fixedWingItermLimitOnStickPosition = 0.5f, .loiter_direction = NAV_LOITER_RIGHT, diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 24bc9ff323..bedd61338e 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -126,7 +126,7 @@ typedef struct pidProfile_s { uint16_t fixedWingItermThrowLimit; float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. - float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. + float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) From 9ef10703decda95fa75d03bb365b5a036127e68e Mon Sep 17 00:00:00 2001 From: Airwide Date: Sat, 5 Sep 2020 23:26:34 +0200 Subject: [PATCH 025/109] Added deadband to fixedWingPitchToThrottleCorrection for testing --- src/main/navigation/navigation_fixedwing.c | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index ae700e3c1f..8cb99e94d5 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -463,7 +463,20 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) int16_t fixedWingPitchToThrottleCorrection(int16_t pitch) { - return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle; + const int16_t pitch_to_throttle_deadband = 30; + + if (pitch > pitch_to_throttle_deadband) { + // Above positive pitch deadband + return DECIDEGREES_TO_DEGREES(pitch - pitch_to_throttle_deadband) * navConfig()->fw.pitch_to_throttle; + } + else if (pitch < -pitch_to_throttle_deadband) { + // Below negative pitch deadband + return DECIDEGREES_TO_DEGREES(pitch + pitch_to_throttle_deadband) * navConfig()->fw.pitch_to_throttle; + } + else { + // Inside deadband + return 0; + } } void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs) From a1c71494143674a26351d38e060c3112008e01e8 Mon Sep 17 00:00:00 2001 From: Airwide Date: Sun, 6 Sep 2020 10:10:06 +0200 Subject: [PATCH 026/109] Renamed variable and added new setting nav_fw_pitch2thr_threshold --- src/main/cms/cms_menu_navigation.c | 1 + src/main/fc/settings.yaml | 6 ++++++ src/main/navigation/navigation.c | 3 ++- src/main/navigation/navigation.h | 1 + src/main/navigation/navigation_fixedwing.c | 18 ++++++++---------- 5 files changed, 18 insertions(+), 11 deletions(-) diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index af5ac8574b..9f70550fdb 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -109,6 +109,7 @@ static const OSD_Entry cmsx_menuFixedWingEntries[] = OSD_SETTING_ENTRY("PITCH TO THR RATIO", SETTING_NAV_FW_PITCH2THR), OSD_SETTING_ENTRY("LOITER RADIUS", SETTING_NAV_FW_LOITER_RADIUS), OSD_SETTING_ENTRY("CONTROL SMOOTHNESS", SETTING_NAV_FW_CONTROL_SMOOTHNESS), + OSD_SETTING_ENTRY("PITCH TO THR THRESHOLD", SETTING_NAV_FW_PITCH2THR_THRESHOLD), OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 65ac689cdd..f8d2295bf5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2260,6 +2260,12 @@ groups: field: fw.pitch_to_throttle min: 0 max: 100 + - name: nav_fw_pitch2thr_threshold + description: "Threshold from zero pitch before pitch_to_throttle is applied [centidegrees]" + default_value: "0" + field: fw.pitch_to_throttle_thresh + min: 0 + max: 100 - name: nav_fw_loiter_radius description: "PosHold radius. 3000 to 7500 is a good value (30-75m) [cm]" default_value: "5000" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c16aa048eb..7d501aacc9 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -87,7 +87,7 @@ PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CO PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 8); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -152,6 +152,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .max_throttle = 1700, .min_throttle = 1200, .pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle) + .pitch_to_throttle_thresh = 0, // Threshold from zero pitch before pitch_to_throttle is applied [centidegrees] .loiter_radius = 5000, // 50m //Fixed wing landing diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 1d8b853902..674dd347e2 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -222,6 +222,7 @@ typedef struct navConfig_s { uint16_t min_throttle; // Minimum allowed throttle in auto mode uint16_t max_throttle; // Maximum allowed throttle in auto mode uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10) + uint8_t pitch_to_throttle_thresh; // Threshold from zero pitch before pitch_to_throttle is applied [centidegrees] uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing int8_t land_dive_angle; uint16_t launch_velocity_thresh; // Velocity threshold for swing launch detection diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 8cb99e94d5..6036121ce2 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -463,18 +463,16 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) int16_t fixedWingPitchToThrottleCorrection(int16_t pitch) { - const int16_t pitch_to_throttle_deadband = 30; - - if (pitch > pitch_to_throttle_deadband) { - // Above positive pitch deadband - return DECIDEGREES_TO_DEGREES(pitch - pitch_to_throttle_deadband) * navConfig()->fw.pitch_to_throttle; + if (pitch > navConfig()->fw.pitch_to_throttle_thresh) { + // Positive pitch above threshold + return DECIDEGREES_TO_DEGREES(pitch - navConfig()->fw.pitch_to_throttle_thresh) * navConfig()->fw.pitch_to_throttle; } - else if (pitch < -pitch_to_throttle_deadband) { - // Below negative pitch deadband - return DECIDEGREES_TO_DEGREES(pitch + pitch_to_throttle_deadband) * navConfig()->fw.pitch_to_throttle; + else if (pitch < -navConfig()->fw.pitch_to_throttle_thresh) { + // Negative pitch below threshold + return DECIDEGREES_TO_DEGREES(pitch + navConfig()->fw.pitch_to_throttle_thresh) * navConfig()->fw.pitch_to_throttle; } - else { - // Inside deadband + else { + // Inside pitch_to_throttle_thresh deadband. Make no throttle correction. return 0; } } From 517fcd0bf6ea9c88b4fe05c6da5eb54e509b0cf3 Mon Sep 17 00:00:00 2001 From: Airwide Date: Sun, 6 Sep 2020 10:11:55 +0200 Subject: [PATCH 027/109] added setting to settings.md --- docs/Settings.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 3f5a76e9cd..ffadd3a62e 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -271,6 +271,7 @@ | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | +| nav_fw_pitch2thr_threshold | 0 | Threshold from zero pitch before nav_fw_pitch2thr is applied [centidegrees] | | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) | @@ -485,4 +486,4 @@ | yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | | yaw_rate | 20 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | -> Note: this table is autogenerated. Do not edit it manually. \ No newline at end of file +> Note: this table is autogenerated. Do not edit it manually. From 3101c11e3c099b422b1478d0f2a2a7d4120c3363 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 6 Sep 2020 13:35:25 +0200 Subject: [PATCH 028/109] Also implement gain for roll --- src/main/fc/settings.yaml | 6 ++++++ src/main/flight/pid.c | 5 +++-- src/main/flight/pid.h | 1 + 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 875fd7dec4..9cbbb27fab 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1545,6 +1545,12 @@ groups: field: fixedWingCoordinatedPitchGain min: 0 max: 2 + - name: fw_turn_assist_roll_gain + description: "Gain required to ??? (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter" + default_value: "1" + field: fixedWingCoordinatedRollGain + min: 0 + max: 2 - name: fw_iterm_limit_stick_position description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side" default_value: "0.5" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 34a657b6f3..63ecce7ef0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -154,7 +154,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -257,6 +257,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingReferenceAirspeed = 1000, .fixedWingCoordinatedYawGain = 1.0f, .fixedWingCoordinatedPitchGain = 1.0f, + .fixedWingCoordinatedRollGain = 1.0f, .fixedWingItermLimitOnStickPosition = 0.5f, .loiter_direction = NAV_LOITER_RIGHT, @@ -889,7 +890,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) imuTransformVectorEarthToBody(&targetRates); // Add in roll and pitch - pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f); + pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x * pidProfile()->fixedWingCoordinatedRollGain, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f); pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y * pidProfile()->fixedWingCoordinatedPitchGain, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f); // Replace YAW on quads - add it in on airplanes diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index bedd61338e..66ea97d318 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -127,6 +127,7 @@ typedef struct pidProfile_s { float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. + float fixedWingCoordinatedRollGain; // This is the gain of the roll rate to ??? during coordinated turns. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) From 3f98e2f6b9de5cf0a04bd57914d64e0f997c1bb7 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 7 Sep 2020 08:53:46 +0200 Subject: [PATCH 029/109] Revert "Also implement gain for roll" This reverts commit 3101c11e3c099b422b1478d0f2a2a7d4120c3363. --- src/main/fc/settings.yaml | 6 ------ src/main/flight/pid.c | 5 ++--- src/main/flight/pid.h | 1 - 3 files changed, 2 insertions(+), 10 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 9cbbb27fab..875fd7dec4 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1545,12 +1545,6 @@ groups: field: fixedWingCoordinatedPitchGain min: 0 max: 2 - - name: fw_turn_assist_roll_gain - description: "Gain required to ??? (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter" - default_value: "1" - field: fixedWingCoordinatedRollGain - min: 0 - max: 2 - name: fw_iterm_limit_stick_position description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side" default_value: "0.5" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 63ecce7ef0..34a657b6f3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -154,7 +154,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -257,7 +257,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingReferenceAirspeed = 1000, .fixedWingCoordinatedYawGain = 1.0f, .fixedWingCoordinatedPitchGain = 1.0f, - .fixedWingCoordinatedRollGain = 1.0f, .fixedWingItermLimitOnStickPosition = 0.5f, .loiter_direction = NAV_LOITER_RIGHT, @@ -890,7 +889,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) imuTransformVectorEarthToBody(&targetRates); // Add in roll and pitch - pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x * pidProfile()->fixedWingCoordinatedRollGain, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f); + pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f); pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y * pidProfile()->fixedWingCoordinatedPitchGain, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f); // Replace YAW on quads - add it in on airplanes diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 66ea97d318..bedd61338e 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -127,7 +127,6 @@ typedef struct pidProfile_s { float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. - float fixedWingCoordinatedRollGain; // This is the gain of the roll rate to ??? during coordinated turns. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) From 3e265617e9bd3c6d6db939a8e495ff72609b4099 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 3 Sep 2020 10:33:14 +0200 Subject: [PATCH 030/109] Implement fw_turn_assist_pitch_gain for TURN ASSIST to tune amount of pitch input in coordinated turns. --- src/main/fc/settings.yaml | 6 ++++++ src/main/flight/pid.c | 5 +++-- src/main/flight/pid.h | 1 + 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 65ac689cdd..132ffb6556 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1539,6 +1539,12 @@ groups: field: fixedWingCoordinatedYawGain min: 0 max: 2 + - name: fw_turn_assist_pitch_gain + description: "Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter" + default_value: "1" + field: fixedWingCoordinatedPitchGain + min: 0 + max: 2 - name: fw_iterm_limit_stick_position description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side" default_value: "0.5" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f58ab3b7de..f71a80fc04 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -154,7 +154,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 15); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 16); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -256,6 +256,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT, .fixedWingReferenceAirspeed = 1000, .fixedWingCoordinatedYawGain = 1.0f, + .fixedWingCoordinatedPitchGain = 1.0f, .fixedWingItermLimitOnStickPosition = 0.5f, .loiter_direction = NAV_LOITER_RIGHT, @@ -889,7 +890,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) // Add in roll and pitch pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f); - pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f); + pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y * pidProfile()->fixedWingCoordinatedPitchGain, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f); // Replace YAW on quads - add it in on airplanes if (STATE(AIRPLANE)) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c325190923..9e2044c8ae 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -126,6 +126,7 @@ typedef struct pidProfile_s { uint16_t fixedWingItermThrowLimit; float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. + float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) From 8acbb34549d87c44861323fa26130750122db974 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 3 Sep 2020 13:45:17 +0200 Subject: [PATCH 031/109] wrap PG back to 0 --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f71a80fc04..b8742c574a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -154,7 +154,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 16); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { From ef7a55eb3858f084d749aa9b266032b0db1f5ddb Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Fri, 4 Sep 2020 23:31:04 +0200 Subject: [PATCH 032/109] Fix alignment issues --- src/main/flight/pid.c | 2 +- src/main/flight/pid.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b8742c574a..00cd8ac9c5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -256,7 +256,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT, .fixedWingReferenceAirspeed = 1000, .fixedWingCoordinatedYawGain = 1.0f, - .fixedWingCoordinatedPitchGain = 1.0f, + .fixedWingCoordinatedPitchGain = 1.0f, .fixedWingItermLimitOnStickPosition = 0.5f, .loiter_direction = NAV_LOITER_RIGHT, diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9e2044c8ae..cc4b20ae59 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -126,7 +126,7 @@ typedef struct pidProfile_s { uint16_t fixedWingItermThrowLimit; float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. - float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. + float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) From 18880e2aefe94491806990aadc65679adc729849 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 6 Sep 2020 13:35:25 +0200 Subject: [PATCH 033/109] Also implement gain for roll --- src/main/fc/settings.yaml | 6 ++++++ src/main/flight/pid.c | 5 +++-- src/main/flight/pid.h | 1 + 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 132ffb6556..906014c7d7 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1545,6 +1545,12 @@ groups: field: fixedWingCoordinatedPitchGain min: 0 max: 2 + - name: fw_turn_assist_roll_gain + description: "Gain required to ??? (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter" + default_value: "1" + field: fixedWingCoordinatedRollGain + min: 0 + max: 2 - name: fw_iterm_limit_stick_position description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side" default_value: "0.5" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 00cd8ac9c5..95143d28a9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -154,7 +154,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -257,6 +257,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingReferenceAirspeed = 1000, .fixedWingCoordinatedYawGain = 1.0f, .fixedWingCoordinatedPitchGain = 1.0f, + .fixedWingCoordinatedRollGain = 1.0f, .fixedWingItermLimitOnStickPosition = 0.5f, .loiter_direction = NAV_LOITER_RIGHT, @@ -889,7 +890,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) imuTransformVectorEarthToBody(&targetRates); // Add in roll and pitch - pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f); + pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x * pidProfile()->fixedWingCoordinatedRollGain, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f); pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y * pidProfile()->fixedWingCoordinatedPitchGain, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f); // Replace YAW on quads - add it in on airplanes diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index cc4b20ae59..9bab955f81 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -127,6 +127,7 @@ typedef struct pidProfile_s { float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. + float fixedWingCoordinatedRollGain; // This is the gain of the roll rate to ??? during coordinated turns. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) From 44068399d09fd3c3a2898cd75e21f4eaddda5c57 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 7 Sep 2020 08:53:46 +0200 Subject: [PATCH 034/109] Revert "Also implement gain for roll" This reverts commit 3101c11e3c099b422b1478d0f2a2a7d4120c3363. --- src/main/fc/settings.yaml | 6 ------ src/main/flight/pid.c | 5 ++--- src/main/flight/pid.h | 1 - 3 files changed, 2 insertions(+), 10 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 906014c7d7..132ffb6556 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1545,12 +1545,6 @@ groups: field: fixedWingCoordinatedPitchGain min: 0 max: 2 - - name: fw_turn_assist_roll_gain - description: "Gain required to ??? (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter" - default_value: "1" - field: fixedWingCoordinatedRollGain - min: 0 - max: 2 - name: fw_iterm_limit_stick_position description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side" default_value: "0.5" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 95143d28a9..00cd8ac9c5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -154,7 +154,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -257,7 +257,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingReferenceAirspeed = 1000, .fixedWingCoordinatedYawGain = 1.0f, .fixedWingCoordinatedPitchGain = 1.0f, - .fixedWingCoordinatedRollGain = 1.0f, .fixedWingItermLimitOnStickPosition = 0.5f, .loiter_direction = NAV_LOITER_RIGHT, @@ -890,7 +889,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) imuTransformVectorEarthToBody(&targetRates); // Add in roll and pitch - pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x * pidProfile()->fixedWingCoordinatedRollGain, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f); + pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f); pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y * pidProfile()->fixedWingCoordinatedPitchGain, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f); // Replace YAW on quads - add it in on airplanes diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9bab955f81..cc4b20ae59 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -127,7 +127,6 @@ typedef struct pidProfile_s { float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. - float fixedWingCoordinatedRollGain; // This is the gain of the roll rate to ??? during coordinated turns. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) From d6d61273102a4a3f04c7e72179b7de1a47e7f3a6 Mon Sep 17 00:00:00 2001 From: Airwide Date: Sun, 27 Sep 2020 23:20:42 +0200 Subject: [PATCH 035/109] Added baseThrottleCorrection calculated from moving average pitch --- src/main/navigation/navigation_fixedwing.c | 35 ++++++++++++---------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 6036121ce2..19a1438c57 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -51,8 +51,8 @@ #include "rx/rx.h" -// Base frequencies for smoothing pitch and roll -#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f +// Base frequencies for smoothing pitch and roll +#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f #define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ 10.0f // If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind @@ -70,9 +70,9 @@ static bool isAutoThrottleManuallyIncreased = false; static int32_t navHeadingError; static int8_t loiterDirYaw = 1; -// Calculates the cutoff frequency for smoothing out roll/pitch commands +// Calculates the cutoff frequency for smoothing out roll/pitch commands // control_smoothness valid range from 0 to 9 -// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz +// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz static float getSmoothnessCutoffFreq(float baseFreq) { uint16_t smoothness = 10 - navConfig()->fw.control_smoothness; @@ -205,7 +205,7 @@ static fpVector3_t virtualDesiredPosition; static pt1Filter_t fwPosControllerCorrectionFilterState; /* - * TODO Currently this function resets both FixedWing and Rover & Boat position controller + * TODO Currently this function resets both FixedWing and Rover & Boat position controller */ void resetFixedWingPositionController(void) { @@ -300,9 +300,9 @@ float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingErr const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0; const float yawAdjustment = navPidApply2( - &posControl.pids.fw_heading, - 0, - applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100), + &posControl.pids.fw_heading, + 0, + applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100), US2S(deltaMicros), -limit, limit, @@ -463,17 +463,20 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) int16_t fixedWingPitchToThrottleCorrection(int16_t pitch) { - if (pitch > navConfig()->fw.pitch_to_throttle_thresh) { - // Positive pitch above threshold - return DECIDEGREES_TO_DEGREES(pitch - navConfig()->fw.pitch_to_throttle_thresh) * navConfig()->fw.pitch_to_throttle; + // Calculate base throttle correction from pitch moving average + const int16_t movingAverageCycles = 128; //Number of main loop cycles for average calculation + static int16_t averagePitch = (averagePitch * movingAverageCycles + pitch - averagePitch) / movingAverageCycles; + const int16_t baseThrottleCorrection = DECIDEGREES_TO_DEGREES(averagePitch) * navConfig()->fw.pitch_to_throttle; + + // Calculate final throttle correction + if (pitch > (averagePitch + navConfig()->fw.pitch_to_throttle_thresh)) { + return baseThrottleCorrection + DECIDEGREES_TO_DEGREES(pitch - averagePitch - navConfig()->fw.pitch_to_throttle_thresh) * navConfig()->fw.pitch_to_throttle; } - else if (pitch < -navConfig()->fw.pitch_to_throttle_thresh) { - // Negative pitch below threshold - return DECIDEGREES_TO_DEGREES(pitch + navConfig()->fw.pitch_to_throttle_thresh) * navConfig()->fw.pitch_to_throttle; + else if (pitch < (averagePitch - navConfig()->fw.pitch_to_throttle_thresh)) { + return baseThrottleCorrection + DECIDEGREES_TO_DEGREES(pitch - averagePitch + navConfig()->fw.pitch_to_throttle_thresh) * navConfig()->fw.pitch_to_throttle; } else { - // Inside pitch_to_throttle_thresh deadband. Make no throttle correction. - return 0; + return baseThrottleCorrection; } } From 103bed8b469401aa0a67b1da2cfbd047775bffce Mon Sep 17 00:00:00 2001 From: Airwide Date: Sun, 27 Sep 2020 23:34:51 +0200 Subject: [PATCH 036/109] Fixed initialization bug --- src/main/navigation/navigation_fixedwing.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 19a1438c57..cc781155ca 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -465,7 +465,9 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch) { // Calculate base throttle correction from pitch moving average const int16_t movingAverageCycles = 128; //Number of main loop cycles for average calculation - static int16_t averagePitch = (averagePitch * movingAverageCycles + pitch - averagePitch) / movingAverageCycles; + static int16_t averagePitch = 0; + + averagePitch = (averagePitch * movingAverageCycles + pitch - averagePitch) / movingAverageCycles; const int16_t baseThrottleCorrection = DECIDEGREES_TO_DEGREES(averagePitch) * navConfig()->fw.pitch_to_throttle; // Calculate final throttle correction From 845de37651a41bb3962119073e52b43c99822029 Mon Sep 17 00:00:00 2001 From: Airwide Date: Mon, 28 Sep 2020 23:02:55 +0200 Subject: [PATCH 037/109] Added nav_fw_pitch2thr_smoothing to cli settings and osd menu --- docs/Settings.md | 3 ++- src/main/cms/cms_menu_navigation.c | 1 + src/main/fc/settings.yaml | 6 +++++ src/main/navigation/navigation.c | 27 +++++++++++----------- src/main/navigation/navigation.h | 1 + src/main/navigation/navigation_fixedwing.c | 2 +- 6 files changed, 25 insertions(+), 15 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index ffadd3a62e..4b5148a7f1 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -271,7 +271,8 @@ | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | -| nav_fw_pitch2thr_threshold | 0 | Threshold from zero pitch before nav_fw_pitch2thr is applied [centidegrees] | +| nav_fw_pitch2thr_smoothing | 1 | Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch | +| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch before nav_fw_pitch2thr is applied [centidegrees] | | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) | diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 9f70550fdb..1d306759e9 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -109,6 +109,7 @@ static const OSD_Entry cmsx_menuFixedWingEntries[] = OSD_SETTING_ENTRY("PITCH TO THR RATIO", SETTING_NAV_FW_PITCH2THR), OSD_SETTING_ENTRY("LOITER RADIUS", SETTING_NAV_FW_LOITER_RADIUS), OSD_SETTING_ENTRY("CONTROL SMOOTHNESS", SETTING_NAV_FW_CONTROL_SMOOTHNESS), + OSD_SETTING_ENTRY("PITCH TO THR SMOOTHING", SETTING_NAV_FW_PITCH2THR_SMOOTHING), OSD_SETTING_ENTRY("PITCH TO THR THRESHOLD", SETTING_NAV_FW_PITCH2THR_THRESHOLD), OSD_BACK_AND_END_ENTRY, diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f8d2295bf5..b9c5763ee7 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2260,6 +2260,12 @@ groups: field: fw.pitch_to_throttle min: 0 max: 100 + - name: nav_fw_pitch2thr_smoothing + description: "Threshold from zero pitch before pitch_to_throttle is applied [centidegrees]" + default_value: "1" + field: fw.pitch_to_throttle_smooth + min: 1 + max: 4096 - name: nav_fw_pitch2thr_threshold description: "Threshold from zero pitch before pitch_to_throttle is applied [centidegrees]" default_value: "0" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 7d501aacc9..131e9784a2 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -152,7 +152,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .max_throttle = 1700, .min_throttle = 1200, .pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle) - .pitch_to_throttle_thresh = 0, // Threshold from zero pitch before pitch_to_throttle is applied [centidegrees] + .pitch_to_throttle_smooth = 1, // Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch + .pitch_to_throttle_thresh = 0, // Threshold from average pitch before pitch_to_throttle is applied [centidegrees] .loiter_radius = 5000, // 50m //Fixed wing landing @@ -1882,13 +1883,13 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) // Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228) // http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf float navPidApply3( - pidController_t *pid, - const float setpoint, - const float measurement, - const float dt, - const float outMin, - const float outMax, - const pidControllerFlags_e pidFlags, + pidController_t *pid, + const float setpoint, + const float measurement, + const float dt, + const float outMin, + const float outMax, + const pidControllerFlags_e pidFlags, const float gainScaler, const float dTermScaler ) { @@ -2414,7 +2415,7 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void) /******************************************************* * Is a safehome being used instead of the arming point? *******************************************************/ -bool isSafeHomeInUse(void) +bool isSafeHomeInUse(void) { return (safehome_used > -1 && safehome_used < MAX_SAFE_HOMES); } @@ -2425,7 +2426,7 @@ bool isSafeHomeInUse(void) **********************************************************/ bool foundNearbySafeHome(void) { - safehome_used = -1; + safehome_used = -1; fpVector3_t safeHome; gpsLocation_t shLLH; shLLH.alt = 0; @@ -2433,7 +2434,7 @@ bool foundNearbySafeHome(void) shLLH.lat = safeHomeConfig(i)->lat; shLLH.lon = safeHomeConfig(i)->lon; geoConvertGeodeticToLocal(&safeHome, &posControl.gpsOrigin, &shLLH, GEO_ALT_RELATIVE); - safehome_distance = calculateDistanceToDestination(&safeHome); + safehome_distance = calculateDistanceToDestination(&safeHome); if (safehome_distance < 20000) { // 200 m safehome_used = i; setHomePosition(&safeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); @@ -2919,7 +2920,7 @@ bool saveNonVolatileWaypointList(void) #if defined(USE_SAFE_HOME) -void resetSafeHomes(void) +void resetSafeHomes(void) { memset(safeHomeConfigMutable(0), 0, sizeof(navSafeHome_t) * MAX_SAFE_HOMES); } @@ -3219,7 +3220,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_IDLE; } - // Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded + // Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded if (IS_RC_MODE_ACTIVE(BOXNAVWP)) { if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 674dd347e2..f49ee82b0d 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -222,6 +222,7 @@ typedef struct navConfig_s { uint16_t min_throttle; // Minimum allowed throttle in auto mode uint16_t max_throttle; // Maximum allowed throttle in auto mode uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10) + uint16_t pitch_to_throttle_smooth; // Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch uint8_t pitch_to_throttle_thresh; // Threshold from zero pitch before pitch_to_throttle is applied [centidegrees] uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing int8_t land_dive_angle; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index cc781155ca..47c005b484 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -464,7 +464,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) int16_t fixedWingPitchToThrottleCorrection(int16_t pitch) { // Calculate base throttle correction from pitch moving average - const int16_t movingAverageCycles = 128; //Number of main loop cycles for average calculation + const int16_t movingAverageCycles = navConfig()->fw.pitch_to_throttle_smooth; static int16_t averagePitch = 0; averagePitch = (averagePitch * movingAverageCycles + pitch - averagePitch) / movingAverageCycles; From cf22b7b9adb9600e4c5f4d63e3a9f68bd1bf5358 Mon Sep 17 00:00:00 2001 From: Airwide Date: Tue, 29 Sep 2020 00:22:08 +0200 Subject: [PATCH 038/109] Corrected cli variable description in settings.yaml --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b9c5763ee7..cd8d18701d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2261,7 +2261,7 @@ groups: min: 0 max: 100 - name: nav_fw_pitch2thr_smoothing - description: "Threshold from zero pitch before pitch_to_throttle is applied [centidegrees]" + description: "Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch" default_value: "1" field: fw.pitch_to_throttle_smooth min: 1 From ec44b038ec9f8ff7acd73cdd4fde10e7963eeeb0 Mon Sep 17 00:00:00 2001 From: Airwide Date: Tue, 29 Sep 2020 20:59:43 +0200 Subject: [PATCH 039/109] Changed throttle field in osd to always show throttle output --- src/main/io/osd.c | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 9128f571b3..2d60c1e391 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -867,20 +867,17 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y) } /** - * Formats throttle position prefixed by its symbol. If autoThr - * is true and the navigation system is controlling THR, it - * uses the THR value applied by the system rather than the - * input value received by the sticks. + * Formats throttle position prefixed by its symbol. + * Shows output to motor, not stick position **/ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr) { buff[0] = SYM_BLANK; buff[1] = SYM_THR; - int16_t thr = rxGetChannelValue(THROTTLE); + int16_t thr = rcCommand[THROTTLE]; if (autoThr && navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; buff[1] = SYM_AUTO_THR1; - thr = rcCommand[THROTTLE]; if (isFixedWingAutoThrottleManuallyIncreased()) TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } From 6e272e07119f4d7d02ac8f73348af5214c529c70 Mon Sep 17 00:00:00 2001 From: Airwide Date: Thu, 1 Oct 2020 21:08:40 +0200 Subject: [PATCH 040/109] Rescaled manual throttle in OSD from 0 to 100% --- src/main/io/osd.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 2d60c1e391..38fd5e9671 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -874,14 +874,15 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t { buff[0] = SYM_BLANK; buff[1] = SYM_THR; - int16_t thr = rcCommand[THROTTLE]; + int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - motorConfig()->minthrottle) * 100 / (motorConfig()->maxthrottle - motorConfig()->minthrottle); if (autoThr && navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; buff[1] = SYM_AUTO_THR1; + thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); if (isFixedWingAutoThrottleManuallyIncreased()) TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } - tfp_sprintf(buff + 2, "%3d", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); + tfp_sprintf(buff + 2, "%3d", thr); } #if defined(USE_ESC_SENSOR) From b76c354dc5dab93b5f61bf5c98048af705486a6b Mon Sep 17 00:00:00 2001 From: Airwide Date: Fri, 2 Oct 2020 06:46:50 +0200 Subject: [PATCH 041/109] Fixed minThrottle value --- src/main/io/osd.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 38fd5e9671..68723b0c30 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -872,9 +872,10 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y) **/ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr) { + const int minThrottle = getThrottleIdleValue(); buff[0] = SYM_BLANK; buff[1] = SYM_THR; - int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - motorConfig()->minthrottle) * 100 / (motorConfig()->maxthrottle - motorConfig()->minthrottle); + int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - minThrottle) * 100 / (motorConfig()->maxthrottle - minThrottle); if (autoThr && navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; buff[1] = SYM_AUTO_THR1; From e24e8f89f8fd1a43c4af871b4e7a4eb932aad3f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sun, 4 Oct 2020 12:13:04 +0100 Subject: [PATCH 042/109] [LINK] Cleanup unused .ld files --- src/main/target/link/stm32_flash_F303_128k.ld | 30 ---------- src/main/target/link/stm32_flash_F303_256k.ld | 30 ---------- src/main/target/link/stm32_flash_F405.ld | 40 ------------- src/main/target/link/stm32_flash_F405_bl.ld | 43 -------------- .../target/link/stm32_flash_F405_for_bl.ld | 42 ------------- src/main/target/link/stm32_flash_F405_opbl.ld | 40 ------------- src/main/target/link/stm32_flash_F411.ld | 40 ------------- src/main/target/link/stm32_flash_F411_opbl.ld | 40 ------------- src/main/target/link/stm32_flash_F427.ld | 59 ------------------- src/main/target/link/stm32_flash_F446.ld | 40 ------------- src/main/target/link/stm32_flash_F746.ld | 47 --------------- src/main/target/link/stm32_flash_F7x2.ld | 48 --------------- src/main/target/link/stm32_flash_F7x2_bl.ld | 50 ---------------- .../target/link/stm32_flash_F7x2_for_bl.ld | 50 ---------------- src/main/target/link/stm32_flash_F7x5xG.ld | 48 --------------- src/main/target/link/stm32_flash_F7x5xG_bl.ld | 50 ---------------- .../target/link/stm32_flash_F7x5xG_for_bl.ld | 50 ---------------- 17 files changed, 747 deletions(-) delete mode 100644 src/main/target/link/stm32_flash_F303_128k.ld delete mode 100644 src/main/target/link/stm32_flash_F303_256k.ld delete mode 100644 src/main/target/link/stm32_flash_F405.ld delete mode 100644 src/main/target/link/stm32_flash_F405_bl.ld delete mode 100644 src/main/target/link/stm32_flash_F405_for_bl.ld delete mode 100644 src/main/target/link/stm32_flash_F405_opbl.ld delete mode 100644 src/main/target/link/stm32_flash_F411.ld delete mode 100644 src/main/target/link/stm32_flash_F411_opbl.ld delete mode 100644 src/main/target/link/stm32_flash_F427.ld delete mode 100644 src/main/target/link/stm32_flash_F446.ld delete mode 100644 src/main/target/link/stm32_flash_F746.ld delete mode 100644 src/main/target/link/stm32_flash_F7x2.ld delete mode 100644 src/main/target/link/stm32_flash_F7x2_bl.ld delete mode 100644 src/main/target/link/stm32_flash_F7x2_for_bl.ld delete mode 100644 src/main/target/link/stm32_flash_F7x5xG.ld delete mode 100644 src/main/target/link/stm32_flash_F7x5xG_bl.ld delete mode 100644 src/main/target/link/stm32_flash_F7x5xG_for_bl.ld diff --git a/src/main/target/link/stm32_flash_F303_128k.ld b/src/main/target/link/stm32_flash_F303_128k.ld deleted file mode 100644 index 4274fb4a45..0000000000 --- a/src/main/target/link/stm32_flash_F303_128k.ld +++ /dev/null @@ -1,30 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash.ld -** -** Abstract : Linker script for STM32F30x Device with -** 128KByte FLASH and 40KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Specify the memory areas. */ -MEMORY -{ - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K - FLASH_CONFIG (r) : ORIGIN = 0x0801F800, LENGTH = 2K - - RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K - CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 8K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("STACKRAM", CCM) -REGION_ALIAS("FASTRAM", CCM) - -INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_F303_256k.ld b/src/main/target/link/stm32_flash_F303_256k.ld deleted file mode 100644 index 1917843495..0000000000 --- a/src/main/target/link/stm32_flash_F303_256k.ld +++ /dev/null @@ -1,30 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash.ld -** -** Abstract : Linker script for STM32F30x Device with -** 256KByte FLASH and 40KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Specify the memory areas. */ -MEMORY -{ - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 250K - FLASH_CONFIG (r) : ORIGIN = 0x0803E800, LENGTH = 6K - - RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K - CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 8K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("STACKRAM", CCM) -REGION_ALIAS("FASTRAM", CCM) - -INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_F405.ld b/src/main/target/link/stm32_flash_F405.ld deleted file mode 100644 index 66721aee5f..0000000000 --- a/src/main/target/link/stm32_flash_F405.ld +++ /dev/null @@ -1,40 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f405.ld -** -** Abstract : Linker script for STM32F405RG Device with -** 1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM) -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x08000000 to 0x08100000 1024K full flash, -0x08000000 to 0x080DFFFF 896K firmware, -0x080E0000 to 0x08100000 128K config, // FLASH_Sector_11 -*/ - -/* Specify the memory areas */ -MEMORY -{ - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 896K - FLASH_CONFIG (r) : ORIGIN = 0x080E0000, LENGTH = 128K - - RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K - BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("STACKRAM", CCM) -REGION_ALIAS("FASTRAM", CCM) - -INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_F405_bl.ld b/src/main/target/link/stm32_flash_F405_bl.ld deleted file mode 100644 index 6ae82e6f8a..0000000000 --- a/src/main/target/link/stm32_flash_F405_bl.ld +++ /dev/null @@ -1,43 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f405.ld -** -** Abstract : Linker script for STM32F405RG Device with -** 1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM) -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x08000000 to 0x08100000 1024K full flash, -0x08000000 to 0x080DFFFF 896K firmware, -0x080E0000 to 0x08100000 128K config, // FLASH_Sector_11 -*/ - -/* Specify the memory areas */ -MEMORY -{ - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K - FIRMWARE (rx) : ORIGIN = 0x08008000, LENGTH = 864K - FLASH_CONFIG (r) : ORIGIN = 0x080E0000, LENGTH = 128K - - RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K - BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("STACKRAM", CCM) -REGION_ALIAS("FASTRAM", CCM) - -__firmware_start = ORIGIN(FIRMWARE); - -INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_F405_for_bl.ld b/src/main/target/link/stm32_flash_F405_for_bl.ld deleted file mode 100644 index c2a60a1429..0000000000 --- a/src/main/target/link/stm32_flash_F405_for_bl.ld +++ /dev/null @@ -1,42 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f405.ld -** -** Abstract : Linker script for STM32F405RG Device with -** 1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM) -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x08000000 to 0x08100000 1024K full flash, -0x08000000 to 0x080DFFFF 896K firmware, -0x080E0000 to 0x08100000 128K config, // FLASH_Sector_11 -*/ - -/* Specify the memory areas */ -MEMORY -{ - FLASH (rx) : ORIGIN = 0x08008000, LENGTH = 864K - FLASH_CONFIG (r) : ORIGIN = 0x080E0000, LENGTH = 128K - - RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K - BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("STACKRAM", CCM) -REGION_ALIAS("FASTRAM", CCM) - -__firmware_start = ORIGIN(FLASH); - -INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_F405_opbl.ld b/src/main/target/link/stm32_flash_F405_opbl.ld deleted file mode 100644 index d12578f7db..0000000000 --- a/src/main/target/link/stm32_flash_F405_opbl.ld +++ /dev/null @@ -1,40 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f405.ld -** -** Abstract : Linker script for STM32F405RG Device with -** 1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM) -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x08000000 to 0x08100000 1024K full flash, -0x08000000 to 0x08004000 16K OPBL, -0x08004000 to 0x080DFFFF 880K firmware, -0x080E0000 to 0x08100000 128K config, // FLASH_Sector_11 -*/ - -/* Specify the memory areas */ -MEMORY -{ - FLASH (rx) : ORIGIN = 0x08004000, LENGTH = 880K - FLASH_CONFIG (r): ORIGIN = 0x080E0000, LENGTH = 128K - - RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("STACKRAM", CCM) -REGION_ALIAS("FASTRAM", CCM) - -INCLUDE "stm32_flash.ld" \ No newline at end of file diff --git a/src/main/target/link/stm32_flash_F411.ld b/src/main/target/link/stm32_flash_F411.ld deleted file mode 100644 index 0326840b34..0000000000 --- a/src/main/target/link/stm32_flash_F411.ld +++ /dev/null @@ -1,40 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f411.ld -** -** Abstract : Linker script for STM32F11 Device with -** 512KByte FLASH, 128KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x08000000 to 0x0807FFFF 512K full flash, -0x08000000 to 0x08003FFF 16K isr vector, startup code, -0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1 -0x08008000 to 0x0807FFFF 480K firmware, -*/ - -/* Specify the memory areas */ -MEMORY -{ - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K - FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K - FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 480K - - RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("STACKRAM", RAM) -REGION_ALIAS("FASTRAM", RAM) - -INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_F411_opbl.ld b/src/main/target/link/stm32_flash_F411_opbl.ld deleted file mode 100644 index af264a87e1..0000000000 --- a/src/main/target/link/stm32_flash_F411_opbl.ld +++ /dev/null @@ -1,40 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f411.ld -** -** Abstract : Linker script for STM32F411 Device with -** 512KByte FLASH, 128KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x08000000 to 0x08080000 512K full flash, -0x08000000 to 0x08004000 16K OPBL, -0x08004000 to 0x0805FFFF 368K firmware, -0x08060000 to 0x08080000 128K config, -*/ - -/* Specify the memory areas */ -MEMORY -{ - FLASH (rx) : ORIGIN = 0x08004000, LENGTH = 368K - FLASH_CONFIG (r) : ORIGIN = 0x08060000, LENGTH = 128K - - RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 64K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("STACKRAM", CCM) -REGION_ALIAS("FASTRAM", CCM) - -INCLUDE "stm32_flash.ld" \ No newline at end of file diff --git a/src/main/target/link/stm32_flash_F427.ld b/src/main/target/link/stm32_flash_F427.ld deleted file mode 100644 index 8c080f38e7..0000000000 --- a/src/main/target/link/stm32_flash_F427.ld +++ /dev/null @@ -1,59 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash.ld -** -** Abstract : Linker script for STM32F427 Device with -** 2048 KByte FLASH, 192KByte RAM -** -** Set heap size, stack size and stack location according -** to application requirements. -** -** Set memory bank area and size if external memory is used. -** -** Target : STMicroelectronics STM32 -** -** Environment : Atollic TrueSTUDIO(R) -** -** Distribution: The file is distributed as is, without any warranty -** of any kind. -** -** (c)Copyright Atollic AB. -** You may use this file as-is or modify it according to the needs of your -** project. Distribution of this file (unmodified or modified) is not -** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the -** rights to distribute the assembled, compiled & linked contents of this -** file as part of an application binary file, provided that it is built -** using the Atollic TrueSTUDIO(R) toolchain. -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x08000000 to 0x08100000 1024K full flash, -0x08000000 to 0x080DFFFF 896K firmware, -0x080E0000 to 0x08100000 128K config, // FLASH_Sector_11 -*/ - -/* Specify the memory areas */ -MEMORY -{ - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 896K - FLASH_CONFIG (r) : ORIGIN = 0x080E0000, LENGTH = 128K - - RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("STACKRAM", CCM) -REGION_ALIAS("FASTRAM", CCM) - -INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_F446.ld b/src/main/target/link/stm32_flash_F446.ld deleted file mode 100644 index 1128fa3506..0000000000 --- a/src/main/target/link/stm32_flash_F446.ld +++ /dev/null @@ -1,40 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f411.ld -** -** Abstract : Linker script for STM32F11 Device with -** 512KByte FLASH, 128KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x08000000 to 0x0807FFFF 512K full flash, -0x08000000 to 0x08003FFF 16K isr vector, startup code, -0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1 -0x08008000 to 0x0807FFFF 480K firmware, -*/ - -/* Specify the memory areas */ -MEMORY -{ - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K - FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K - FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 480K - - RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("STACKRAM", RAM) -REGION_ALIAS("FASTRAM", RAM) - -INCLUDE "stm32_flash_split.ld" \ No newline at end of file diff --git a/src/main/target/link/stm32_flash_F746.ld b/src/main/target/link/stm32_flash_F746.ld deleted file mode 100644 index 443c72837a..0000000000 --- a/src/main/target/link/stm32_flash_F746.ld +++ /dev/null @@ -1,47 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f746.ld -** -** Abstract : Linker script for STM32F746VGTx Device with -** 1024KByte FLASH, 320KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x08000000 to 0x080FFFFF 1024K full flash, -0x08000000 to 0x08007FFF 32K isr vector, startup code, -0x08008000 to 0x0800FFFF 32K config, // FLASH_Sector_1 -0x08010000 to 0x080FFFFF 960K firmware, -*/ - -/* Specify the memory areas */ -MEMORY -{ - ITCM_RAM (rx) : ORIGIN = 0x00000000, LENGTH = 16K - ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 32K - /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */ - ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00208000, LENGTH = 32K - ITCM_FLASH1 (rx) : ORIGIN = 0x00210000, LENGTH = 960K - - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K - FLASH_CONFIG (r) : ORIGIN = 0x08008000, LENGTH = 32K - FLASH1 (rx) : ORIGIN = 0x08010000, LENGTH = 960K - - TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 256K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} -/* note CCM could be used for stack */ -REGION_ALIAS("STACKRAM", TCM) -REGION_ALIAS("FASTRAM", TCM) - -INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/target/link/stm32_flash_F7x2.ld b/src/main/target/link/stm32_flash_F7x2.ld deleted file mode 100644 index 296708678d..0000000000 --- a/src/main/target/link/stm32_flash_F7x2.ld +++ /dev/null @@ -1,48 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f722.ld -** -** Abstract : Linker script for STM32F722RETx Device with -** 512KByte FLASH, 256KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x08000000 to 0x0807FFFF 512K full flash, -0x08000000 to 0x08003FFF 16K isr vector, startup code, -0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1 -0x08008000 to 0x0807FFFF 480K firmware, -*/ - -/* Specify the memory areas */ -MEMORY -{ - ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 16K - - ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 16K - /* config occupies the entire flash sector 1 for the ease of erasure, 16K on F72x */ - ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00204000, LENGTH = 16K - ITCM_FLASH1 (rx) : ORIGIN = 0x00208000, LENGTH = 480K - - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K - FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K - FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 480K - - TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 192K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("STACKRAM", TCM) -REGION_ALIAS("FASTRAM", TCM) - -INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/target/link/stm32_flash_F7x2_bl.ld b/src/main/target/link/stm32_flash_F7x2_bl.ld deleted file mode 100644 index 5bfd3ba491..0000000000 --- a/src/main/target/link/stm32_flash_F7x2_bl.ld +++ /dev/null @@ -1,50 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f722.ld -** -** Abstract : Linker script for STM32F722RETx Device with -** 512KByte FLASH, 256KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x08000000 to 0x0807FFFF 512K full flash, -0x08000000 to 0x08003FFF 16K isr vector, startup code, -0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1 -0x08008000 to 0x0807FFFF 480K firmware, -*/ - -/* Specify the memory areas */ -MEMORY -{ - ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 16K - - ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 16K - /* config occupies the entire flash sector 1 for the ease of erasure, 16K on F72x */ - ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00204000, LENGTH = 16K - ITCM_FLASH1 (rx) : ORIGIN = 0x00208000, LENGTH = 480K - - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K - FIRMWARE (rx) : ORIGIN = 0x08008000, LENGTH = 16K - FLASH_CONFIG (r) : ORIGIN = 0x0800c000, LENGTH = 16K - - TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 192K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("STACKRAM", TCM) -REGION_ALIAS("FASTRAM", TCM) - -__firmware_start = ORIGIN(FIRMWARE); - -INCLUDE "stm32_flash_f7.ld" diff --git a/src/main/target/link/stm32_flash_F7x2_for_bl.ld b/src/main/target/link/stm32_flash_F7x2_for_bl.ld deleted file mode 100644 index 83c9a43fc5..0000000000 --- a/src/main/target/link/stm32_flash_F7x2_for_bl.ld +++ /dev/null @@ -1,50 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f722.ld -** -** Abstract : Linker script for STM32F722RETx Device with -** 512KByte FLASH, 256KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x08000000 to 0x0807FFFF 512K full flash, -0x08000000 to 0x08003FFF 16K isr vector, startup code, -0x08004000 to 0x08007FFF 16K config, // FLASH_Sector_1 -0x08008000 to 0x0807FFFF 480K firmware, -*/ - -/* Specify the memory areas */ -MEMORY -{ - ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 16K - - /*ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 16K*/ - /* config occupies the entire flash sector 1 for the ease of erasure, 16K on F72x */ - /*ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00204000, LENGTH = 16K*/ - /*ITCM_FLASH1 (rx) : ORIGIN = 0x00208000, LENGTH = 480K*/ - - FLASH (rx) : ORIGIN = 0x08008000, LENGTH = 16K - FLASH_CONFIG (r) : ORIGIN = 0x0800c000, LENGTH = 16K - FLASH1 (rx) : ORIGIN = 0x08010000, LENGTH = 448K - - TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 192K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("STACKRAM", TCM) -REGION_ALIAS("FASTRAM", TCM) - -__firmware_start = ORIGIN(FLASH); - -INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/target/link/stm32_flash_F7x5xG.ld b/src/main/target/link/stm32_flash_F7x5xG.ld deleted file mode 100644 index a83374e4e4..0000000000 --- a/src/main/target/link/stm32_flash_F7x5xG.ld +++ /dev/null @@ -1,48 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f745.ld -** -** Abstract : Linker script for STM32F745VGTx Device with -** 1024KByte FLASH, 320KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x00000000 to 0x00003FFF 16K ITCM RAM, -0x08000000 to 0x080FFFFF 1024K full flash, -0x08000000 to 0x08007FFF 32K isr vector, startup code, -0x08008000 to 0x0800FFFF 32K config, // FLASH_Sector_1 -0x08010000 to 0x080FFFFF 960K firmware, -*/ - -/* Specify the memory areas */ -MEMORY -{ - ITCM_RAM (rx) : ORIGIN = 0x00000000, LENGTH = 16K - ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 32K - /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */ - ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00208000, LENGTH = 32K - ITCM_FLASH1 (rx) : ORIGIN = 0x00210000, LENGTH = 960K - - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K - FLASH_CONFIG (r) : ORIGIN = 0x08008000, LENGTH = 32K - FLASH1 (rx) : ORIGIN = 0x08010000, LENGTH = 960K - - TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 256K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} -/* note CCM could be used for stack */ -REGION_ALIAS("STACKRAM", TCM) -REGION_ALIAS("FASTRAM", TCM) - -INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/target/link/stm32_flash_F7x5xG_bl.ld b/src/main/target/link/stm32_flash_F7x5xG_bl.ld deleted file mode 100644 index d357aad98d..0000000000 --- a/src/main/target/link/stm32_flash_F7x5xG_bl.ld +++ /dev/null @@ -1,50 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f745.ld -** -** Abstract : Linker script for STM32F745VGTx Device with -** 1024KByte FLASH, 320KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x00000000 to 0x00003FFF 16K ITCM RAM, -0x08000000 to 0x080FFFFF 1024K full flash, -0x08000000 to 0x08007FFF 32K isr vector, startup code, -0x08008000 to 0x0800FFFF 32K config, // FLASH_Sector_1 -0x08010000 to 0x080FFFFF 960K firmware, -*/ - -/* Specify the memory areas */ -MEMORY -{ - ITCM_RAM (rx) : ORIGIN = 0x00000000, LENGTH = 16K - ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 32K - /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */ - ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00208000, LENGTH = 32K - ITCM_FLASH1 (rx) : ORIGIN = 0x00210000, LENGTH = 928K - - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K - FIRMWARE (rx) : ORIGIN = 0x08008000, LENGTH = 32K - FLASH_CONFIG (r) : ORIGIN = 0x08010000, LENGTH = 32K - - TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 256K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} -/* note CCM could be used for stack */ -REGION_ALIAS("STACKRAM", TCM) -REGION_ALIAS("FASTRAM", TCM) - -__firmware_start = ORIGIN(FIRMWARE); - -INCLUDE "stm32_flash_f7.ld" diff --git a/src/main/target/link/stm32_flash_F7x5xG_for_bl.ld b/src/main/target/link/stm32_flash_F7x5xG_for_bl.ld deleted file mode 100644 index c7667d1dc2..0000000000 --- a/src/main/target/link/stm32_flash_F7x5xG_for_bl.ld +++ /dev/null @@ -1,50 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f745.ld -** -** Abstract : Linker script for STM32F745VGTx Device with -** 1024KByte FLASH, 320KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x00000000 to 0x00003FFF 16K ITCM RAM, -0x08000000 to 0x080FFFFF 1024K full flash, -0x08000000 to 0x08007FFF 32K isr vector, startup code, -0x08008000 to 0x0800FFFF 32K config, // FLASH_Sector_1 -0x08010000 to 0x080FFFFF 960K firmware, -*/ - -/* Specify the memory areas */ -MEMORY -{ - ITCM_RAM (rx) : ORIGIN = 0x00000000, LENGTH = 16K - ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 32K - /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */ - ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00208000, LENGTH = 32K - ITCM_FLASH1 (rx) : ORIGIN = 0x00210000, LENGTH = 960K - - FLASH (rx) : ORIGIN = 0x08008000, LENGTH = 32K - FLASH_CONFIG (r) : ORIGIN = 0x08010000, LENGTH = 32K - FLASH1 (rx) : ORIGIN = 0x08018000, LENGTH = 928K - - TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 256K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} -/* note CCM could be used for stack */ -REGION_ALIAS("STACKRAM", TCM) -REGION_ALIAS("FASTRAM", TCM) - -__firmware_start = ORIGIN(FLASH); - -INCLUDE "stm32_flash_f7_split.ld" From 8b2e81fb3135abff7e0bfac039b8277a20984d05 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Tue, 6 Oct 2020 09:38:20 -0400 Subject: [PATCH 043/109] Scaled LQ based on CRSF manual numbers. --- src/main/io/osd.c | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 78ed9d6f78..5c9937fb24 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1651,15 +1651,21 @@ static bool osdDrawSingleElement(uint8_t item) } break; - case OSD_CRSF_LQ: + case OSD_CRSF_LQ: { buff[0] = SYM_BLANK; - tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%"); - if (!failsafeIsReceivingRxData()){ - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } else if (rxLinkStatistics.uplinkLQ < osdConfig()->rssi_alarm) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + int16_t scaledLQ = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 170, 300); + if (rxLinkStatistics.rfMode == 2) { + tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, scaledLQ, "%"); + } else { + tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%"); + } + if (!failsafeIsReceivingRxData()){ + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (rxLinkStatistics.uplinkLQ < osdConfig()->rssi_alarm) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; } - break; #if defined(USE_SERIALRX_CRSF) case OSD_CRSF_SNR_DB: { From 6ac103ffc2085bc1f1c9f465e24bf227930f1d00 Mon Sep 17 00:00:00 2001 From: OptimusTi <45466510+OptimusTi@users.noreply.github.com> Date: Tue, 6 Oct 2020 12:07:37 -0400 Subject: [PATCH 044/109] Update osd.c --- src/main/io/osd.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 5c9937fb24..89b446c8b5 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1653,7 +1653,8 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_CRSF_LQ: { buff[0] = SYM_BLANK; - int16_t scaledLQ = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 170, 300); + int16_t statsLQ = rxLinkStatistics.uplinkLQ; + int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); if (rxLinkStatistics.rfMode == 2) { tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, scaledLQ, "%"); } else { From c8f91cbd963a92ee844ece990480d4a7b8c54997 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Tue, 6 Oct 2020 19:44:06 -0400 Subject: [PATCH 045/109] 300% format test --- src/main/io/osd.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 5c9937fb24..86a182bb56 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1655,9 +1655,9 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_BLANK; int16_t scaledLQ = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 170, 300); if (rxLinkStatistics.rfMode == 2) { - tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, scaledLQ, "%"); + tfp_sprintf(buff, "%3d%s", scaledLQ, "%"); } else { - tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%"); + tfp_sprintf(buff, "%3d%s", rxLinkStatistics.uplinkLQ, "%"); } if (!failsafeIsReceivingRxData()){ TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); From 4caa061d1b422e19d8b6c95f87726bf2a1afa04b Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Wed, 7 Oct 2020 18:23:23 -0400 Subject: [PATCH 046/109] SNR alarm default Default changed to match prewarning recommended in Crossfire manual v4.0 --- src/main/fc/settings.yaml | 2 +- src/main/io/osd.c | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a10ceda175..94d2e12063 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2712,7 +2712,7 @@ groups: max: 1250 - name: osd_snr_alarm description: "Value below which Crossfire SNR Alarm pops-up. (dB)" - default_value: "5" + default_value: "4" field: snr_alarm min: -12 max: 8 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index a198851080..43e27d4d40 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2578,7 +2578,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .baro_temp_alarm_max = 600, #endif #ifdef USE_SERIALRX_CRSF - .snr_alarm = 5, + .snr_alarm = 4, #endif #ifdef USE_TEMPERATURE_SENSOR .temp_label_align = OSD_ALIGN_LEFT, @@ -2667,10 +2667,10 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6); #ifdef USE_SERIALRX_CRSF - osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12); - osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(22, 11); - osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(23, 9); - osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10); + osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(24, 12); + osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(26, 11); + osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(25, 9); + osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(25, 10); #endif osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8); From 319cd3a0d39c320913e6377d040bc3ca6d665487 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 10 Oct 2020 13:04:30 +0200 Subject: [PATCH 047/109] Airplane outputs for MambaF722 --- src/main/target/MAMBAF722/target.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/target/MAMBAF722/target.c b/src/main/target/MAMBAF722/target.c index 6ed573e6b6..f46df2fa0c 100644 --- a/src/main/target/MAMBAF722/target.c +++ b/src/main/target/MAMBAF722/target.c @@ -27,10 +27,10 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0 ), // S1_OUT – D(2, 4, 7) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR, 0, 0 ), // S2_OUT – D(2, 7, 7) - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR, 0, 1 ), // S3_OUT – D(2, 1, 6) - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR, 0, 1 ), // S4_OUT – D(2, 2, 6) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1_OUT – D(2, 4, 7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2_OUT – D(2, 7, 7) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S3_OUT – D(2, 1, 6) + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4_OUT – D(2, 2, 6) DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3) }; From fd8b2022a5674affc135fc7ea2118759afbd7f8a Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Sat, 10 Oct 2020 20:56:08 -0400 Subject: [PATCH 048/109] Adds CRSF LQ Formats Adds LQ format used by TBS hardware and format used by other FC software. --- src/main/cms/cms_menu_osd.c | 3 ++- src/main/fc/settings.yaml | 9 +++++++++ src/main/io/osd.c | 17 +++++++++++++---- src/main/io/osd.h | 7 +++++++ 4 files changed, 31 insertions(+), 5 deletions(-) diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 69c45d6217..ff972556d6 100755 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -143,8 +143,9 @@ static const OSD_Entry menuCrsfRxEntries[]= { OSD_LABEL_ENTRY("-- CRSF RX --"), - OSD_SETTING_ENTRY("CRSF SNR LEVEL", SETTING_OSD_SNR_ALARM), + OSD_SETTING_ENTRY("LQ FORMAT", SETTING_OSD_CRSF_LQ_FORMAT), OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_RSSI_ALARM), + OSD_SETTING_ENTRY("SNR ALARM LEVEL", SETTING_OSD_SNR_ALARM), OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_CRSF_RSSI_DBM), OSD_ELEMENT_ENTRY("RX LQ", OSD_CRSF_LQ), OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_CRSF_SNR_DB), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 94d2e12063..0b4805dd82 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -144,6 +144,9 @@ tables: - name: tristate enum: tristate_e values: ["AUTO", "ON", "OFF"] + - name: osd_crsf_lq_format + enum: osd_crsf_lq_format_e + values: ["TBS", "TYPE2"] groups: - name: PG_GYRO_CONFIG @@ -2739,6 +2742,12 @@ groups: field: crosshairs_style table: osd_crosshairs_style type: uint8_t + - name: osd_crsf_lq_format + description: "To select LQ format" + default_value: "TBS" + field: crsf_lq_format + table: osd_crsf_lq_format + type: uint8_t - name: osd_horizon_offset description: "To vertically adjust the whole OSD and AHI and scrolling bars" default_value: "0" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 43e27d4d40..382d70e741 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1651,14 +1651,23 @@ static bool osdDrawSingleElement(uint8_t item) } break; +#if defined(USE_SERIALRX_CRSF) case OSD_CRSF_LQ: { buff[0] = SYM_BLANK; int16_t statsLQ = rxLinkStatistics.uplinkLQ; int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); if (rxLinkStatistics.rfMode == 2) { - tfp_sprintf(buff, "%3d%s", scaledLQ, "%"); + if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TBS) { + tfp_sprintf(buff, "%5d%s", scaledLQ, "%"); + } else { + tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%"); + } } else { - tfp_sprintf(buff, "%3d%s", rxLinkStatistics.uplinkLQ, "%"); + if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TBS) { + tfp_sprintf(buff, "%5d%s", rxLinkStatistics.uplinkLQ, "%"); + } else { + tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%"); + } } if (!failsafeIsReceivingRxData()){ TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -1668,7 +1677,6 @@ static bool osdDrawSingleElement(uint8_t item) break; } -#if defined(USE_SERIALRX_CRSF) case OSD_CRSF_SNR_DB: { const char* showsnr = "-12"; const char* hidesnr = " "; @@ -2579,6 +2587,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, #endif #ifdef USE_SERIALRX_CRSF .snr_alarm = 4, + .crsf_lq_format = OSD_CRSF_LQ_TBS, #endif #ifdef USE_TEMPERATURE_SENSOR .temp_label_align = OSD_ALIGN_LEFT, @@ -2668,7 +2677,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) #ifdef USE_SERIALRX_CRSF osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(24, 12); - osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(26, 11); + osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(24, 11); osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(25, 9); osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(25, 10); #endif diff --git a/src/main/io/osd.h b/src/main/io/osd.h index f05a268ef5..8e1c3ff973 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -202,6 +202,11 @@ typedef enum { OSD_AHI_STYLE_LINE, } osd_ahi_style_e; +typedef enum { + OSD_CRSF_LQ_TBS, + OSD_CRSF_LQ_OTX, +} osd_crsf_lq_format_e; + typedef struct osdLayoutsConfig_s { // Layouts uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT]; @@ -279,6 +284,8 @@ typedef struct osdConfig_s { uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. + uint8_t crsf_lq_format; + } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); From 3c5a5bd3aa6e4d8ce076e0affe58f2595558f9f8 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Mon, 12 Oct 2020 17:48:44 -0400 Subject: [PATCH 049/109] Update settings.yaml Changed values names to generic names. --- src/main/fc/settings.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0b4805dd82..4ddcd2f620 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -146,7 +146,7 @@ tables: values: ["AUTO", "ON", "OFF"] - name: osd_crsf_lq_format enum: osd_crsf_lq_format_e - values: ["TBS", "TYPE2"] + values: ["TYPE1", "TYPE2"] groups: - name: PG_GYRO_CONFIG @@ -2744,7 +2744,7 @@ groups: type: uint8_t - name: osd_crsf_lq_format description: "To select LQ format" - default_value: "TBS" + default_value: "TYPE1" field: crsf_lq_format table: osd_crsf_lq_format type: uint8_t From 924f05e67833e3439d0a7a7a66d96f998d63ece5 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 14 Oct 2020 09:13:48 +0100 Subject: [PATCH 050/109] Update settings.yaml Remove whitespace --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 96a350a17b..fd611a8547 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2111,7 +2111,7 @@ groups: description: "Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details" default_value: "AT_LEAST" field: general.flags.rth_alt_control_mode - table: nav_rth_alt_mode + table: nav_rth_alt_mode - name: nav_rth_abort_threshold description: "RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm]" default_value: "50000" From 655946a1118054db3fb9fd7430b85022d9a1ff2f Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Wed, 14 Oct 2020 13:20:28 -0400 Subject: [PATCH 051/109] Formatting and variable name fixes --- src/main/io/osd.c | 10 +++++----- src/main/io/osd.h | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 382d70e741..37ead36df3 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1657,16 +1657,16 @@ static bool osdDrawSingleElement(uint8_t item) int16_t statsLQ = rxLinkStatistics.uplinkLQ; int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); if (rxLinkStatistics.rfMode == 2) { - if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TBS) { + if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) { tfp_sprintf(buff, "%5d%s", scaledLQ, "%"); } else { tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%"); } } else { - if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TBS) { - tfp_sprintf(buff, "%5d%s", rxLinkStatistics.uplinkLQ, "%"); + if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) { + tfp_sprintf(buff, "%5d%s", rxLinkStatistics.uplinkLQ, "%"); } else { - tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%"); + tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%"); } } if (!failsafeIsReceivingRxData()){ @@ -2587,7 +2587,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, #endif #ifdef USE_SERIALRX_CRSF .snr_alarm = 4, - .crsf_lq_format = OSD_CRSF_LQ_TBS, + .crsf_lq_format = OSD_CRSF_LQ_TYPE1, #endif #ifdef USE_TEMPERATURE_SENSOR .temp_label_align = OSD_ALIGN_LEFT, diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 8e1c3ff973..abc4132715 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -203,8 +203,8 @@ typedef enum { } osd_ahi_style_e; typedef enum { - OSD_CRSF_LQ_TBS, - OSD_CRSF_LQ_OTX, + OSD_CRSF_LQ_TYPE1, + OSD_CRSF_LQ_TYPE2, } osd_crsf_lq_format_e; typedef struct osdLayoutsConfig_s { From be338c0ffca5767d5fdc66dc081d3c163772be60 Mon Sep 17 00:00:00 2001 From: "DESKTOP-N53JVUO\\mix" Date: Sat, 17 Oct 2020 13:59:25 +0300 Subject: [PATCH 052/109] Resolve merge conflicts --- src/main/io/osd.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4a4f951f1c..78bd1eebb9 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3307,6 +3307,10 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); + const char *launchStateMessage = fixedWingLaunchStateMessage(); + if (launchStateMessage) { + messages[messageCount++] = launchStateMessage; + } } else { if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO From d17bfc67e3eb197b8a4aa3c1df9e13977e86255a Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 19 Oct 2020 22:30:23 +0200 Subject: [PATCH 053/109] Add logic condition for 3D home distance based on Pythagorean theorem. --- docs/Programming Framework.md | 1 + src/main/programming/logic_condition.c | 4 ++++ src/main/programming/logic_condition.h | 1 + 3 files changed, 6 insertions(+) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 6c4e96f48d..80a7320c27 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -116,6 +116,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 28 | STABILIZED_YAW | Yaw PID controller output `[-500:500]` | | 29 | ACTIVE_WAYPOINT_INDEX | Indexed from `1`. To verify WP is in progress, use `IS_WP` | | 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph | +| 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem | ##### ACTIVE_WAYPOINT_ACTION diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 8b40506750..8c9d7c00de 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -478,6 +478,10 @@ static int logicConditionGetFlightOperandValue(int operand) { return NAV_Status.activeWpAction; break; + case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m + return constrain(sqrtf(sq(GPS_distanceToHome) + sq(getEstimatedActualPosition(Z)/100)), 0, INT16_MAX); + break; + default: return 0; break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index e46b8c6c43..0327fdb385 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -112,6 +112,7 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW, // 28 LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_INDEX, // 29 LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_ACTION, // 30 + LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 31 } logicFlightOperands_e; typedef enum { From b2621796e6d4b7c63634a96248a3105182595c9d Mon Sep 17 00:00:00 2001 From: 4712 <4712@outlook.de> Date: Tue, 20 Oct 2020 14:49:04 +0200 Subject: [PATCH 054/109] Update serial-4way-if to v20005 Reservation of an MCU_ID area to simplificate adding of upcoming BLHeli_32 MCU. --- src/main/io/serial_4way.c | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 7cf24890b7..a45f41c8bc 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -72,7 +72,7 @@ // *** change to adapt Revision #define SERIAL_4WAY_VER_MAIN 20 #define SERIAL_4WAY_VER_SUB_1 (uint8_t) 0 -#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 03 +#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 05 #define SERIAL_4WAY_PROTOCOL_VER 107 // *** end @@ -326,14 +326,13 @@ uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) { #define ATMEL_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x9307) || (pDeviceInfo->words[0] == 0x930A) || \ (pDeviceInfo->words[0] == 0x930F) || (pDeviceInfo->words[0] == 0x940B)) -#define SILABS_DEVICE_MATCH ((pDeviceInfo->words[0] == 0xF310)||(pDeviceInfo->words[0] ==0xF330) || \ +#define SILABS_DEVICE_MATCH ((pDeviceInfo->words[0] == 0xF310)||(pDeviceInfo->words[0] == 0xF330) || \ (pDeviceInfo->words[0] == 0xF410) || (pDeviceInfo->words[0] == 0xF390) || \ (pDeviceInfo->words[0] == 0xF850) || (pDeviceInfo->words[0] == 0xE8B1) || \ (pDeviceInfo->words[0] == 0xE8B2)) -#define ARM_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x1F06) || \ - (pDeviceInfo->words[0] == 0x3306) || (pDeviceInfo->words[0] == 0x3406) || (pDeviceInfo->words[0] == 0x3506) || \ - (pDeviceInfo->words[0] == 0x2B06) || (pDeviceInfo->words[0] == 0x4706)) +// BLHeli_32 MCU ID hi > 0x00 and < 0x90 / lo always = 0x06 +#define ARM_DEVICE_MATCH ((pDeviceInfo->bytes[1] > 0x00) && (pDeviceInfo->bytes[1] < 0x90) && (pDeviceInfo->bytes[0] == 0x06)) static uint8_t CurrentInterfaceMode; From c8accacea356cc90bb4b09e8fa6b700f43466ac6 Mon Sep 17 00:00:00 2001 From: mfoo Date: Tue, 20 Oct 2020 17:08:12 +0200 Subject: [PATCH 055/109] DJI CRAFT_NAME can be used for warnings, trip distance, efficiency, 3dspeed and throttle --- src/main/io/osd_dji_hd.c | 494 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 490 insertions(+), 4 deletions(-) diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 379937d63d..5eaf1a0dc5 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -45,6 +45,7 @@ #include "fc/fc_msp.h" #include "fc/fc_msp_box.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "flight/imu.h" #include "flight/pid.h" @@ -68,9 +69,15 @@ #include "msp/msp_protocol.h" #include "msp/msp_serial.h" +#include "common/string_light.h" #include "navigation/navigation.h" - +#include "navigation/navigation_private.h" +#include "common/constants.h" #include "scheduler/scheduler.h" +#include "common/printf.h" +#include +#include "rx/rx.h" +#include "fc/rc_controls.h" #if defined(USE_DJI_HD_OSD) @@ -80,6 +87,21 @@ #define DJI_OSD_WARNING_COUNT 16 #define DJI_OSD_TIMER_COUNT 2 #define DJI_OSD_FLAGS_OSD_FEATURE (1 << 0) +#define EFFICIENCY_UPDATE_INTERVAL (5 * 1000) + +#define RC_RX_LINK_LOST_MSG "!RC RX LINK LOST!" +// Adjust OSD_MESSAGE's default position when +// changing OSD_MESSAGE_LENGTH +#define OSD_MESSAGE_LENGTH 28 +#define OSD_ALTERNATING_CHOICES(ms, num_choices) ((millis() / ms) % num_choices) +#define _CONST_STR_SIZE(s) ((sizeof(s)/sizeof(s[0]))-1) // -1 to avoid counting final '\0' +// Wrap all string constants intenteded for display as messages with +// this macro to ensure compile time length validation. +#define OSD_MESSAGE_STR(x) ({ \ + STATIC_ASSERT(_CONST_STR_SIZE(x) <= OSD_MESSAGE_LENGTH, message_string_ ## __COUNTER__ ## _too_long); \ + x; \ +}) + /* * DJI HD goggles use MSPv1 compatible with Betaflight 4.1.0 @@ -376,6 +398,326 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst) } #endif +static const char * osdArmingDisabledReasonMessage(void) +{ + switch (isArmingDisabledReason()) { + case ARMING_DISABLED_FAILSAFE_SYSTEM: + // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c + if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) { + if (failsafeIsReceivingRxData()) { + // If we're not using sticks, it means the ARM switch + // hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING + // yet + return OSD_MESSAGE_STR("DISARM!"); + } + // Not receiving RX data + return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG); + } + return OSD_MESSAGE_STR("FAILSAFE"); + case ARMING_DISABLED_NOT_LEVEL: + return OSD_MESSAGE_STR("!LEVEL"); + case ARMING_DISABLED_SENSORS_CALIBRATING: + return OSD_MESSAGE_STR("CALIBRATING"); + case ARMING_DISABLED_SYSTEM_OVERLOADED: + return OSD_MESSAGE_STR("OVERLOAD"); + case ARMING_DISABLED_NAVIGATION_UNSAFE: +#if defined(USE_NAV) + // Check the exact reason + switch (navigationIsBlockingArming(NULL)) { + case NAV_ARMING_BLOCKER_NONE: + break; + case NAV_ARMING_BLOCKER_MISSING_GPS_FIX: + return OSD_MESSAGE_STR("NO GPS FIX"); + case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE: + return OSD_MESSAGE_STR("DISABLE NAV"); + case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR: + return OSD_MESSAGE_STR("1ST WYP TOO FAR"); + case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR: + return OSD_MESSAGE_STR("WYP MISCONFIGURED"); + } +#endif + break; + case ARMING_DISABLED_COMPASS_NOT_CALIBRATED: + return OSD_MESSAGE_STR("COMPS CALIB"); + case ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED: + return OSD_MESSAGE_STR("ACC CALIB"); + case ARMING_DISABLED_ARM_SWITCH: + return OSD_MESSAGE_STR("DISARM!"); + case ARMING_DISABLED_HARDWARE_FAILURE: + return OSD_MESSAGE_STR("ERR HW!"); + // { + // if (!HW_SENSOR_IS_HEALTHY(getHwGyroStatus())) { + // return OSD_MESSAGE_STR("GYRO FAILURE"); + // } + // if (!HW_SENSOR_IS_HEALTHY(getHwAccelerometerStatus())) { + // return OSD_MESSAGE_STR("ACCELEROMETER FAILURE"); + // } + // if (!HW_SENSOR_IS_HEALTHY(getHwCompassStatus())) { + // return OSD_MESSAGE_STR("COMPASS FAILURE"); + // } + // if (!HW_SENSOR_IS_HEALTHY(getHwBarometerStatus())) { + // return OSD_MESSAGE_STR("BAROMETER FAILURE"); + // } + // if (!HW_SENSOR_IS_HEALTHY(getHwGPSStatus())) { + // return OSD_MESSAGE_STR("GPS FAILURE"); + // } + // if (!HW_SENSOR_IS_HEALTHY(getHwRangefinderStatus())) { + // return OSD_MESSAGE_STR("RANGE FINDER FAILURE"); + // } + // if (!HW_SENSOR_IS_HEALTHY(getHwPitotmeterStatus())) { + // return OSD_MESSAGE_STR("PITOT METER FAILURE"); + // } + // } + // return OSD_MESSAGE_STR("HARDWARE FAILURE"); + case ARMING_DISABLED_BOXFAILSAFE: + return OSD_MESSAGE_STR("FAILSAFE ENABLED"); + case ARMING_DISABLED_BOXKILLSWITCH: + return OSD_MESSAGE_STR("KILLSWITCH ENABLED"); + case ARMING_DISABLED_RC_LINK: + return OSD_MESSAGE_STR("NO RC LINK"); + case ARMING_DISABLED_THROTTLE: + return OSD_MESSAGE_STR("THROTTLE!"); + case ARMING_DISABLED_ROLLPITCH_NOT_CENTERED: + return OSD_MESSAGE_STR("ROLLPITCH!"); + case ARMING_DISABLED_SERVO_AUTOTRIM: + return OSD_MESSAGE_STR("AUTOTRIM!"); + case ARMING_DISABLED_OOM: + return OSD_MESSAGE_STR("MEM LOW"); + case ARMING_DISABLED_INVALID_SETTING: + return OSD_MESSAGE_STR("ERR SETTING"); + case ARMING_DISABLED_CLI: + return OSD_MESSAGE_STR("CLI"); + case ARMING_DISABLED_PWM_OUTPUT_ERROR: + return OSD_MESSAGE_STR("PWM ERR"); + // Cases without message + case ARMING_DISABLED_CMS_MENU: + FALLTHROUGH; + case ARMING_DISABLED_OSD_MENU: + FALLTHROUGH; + case ARMING_DISABLED_ALL_FLAGS: + FALLTHROUGH; + case ARMED: + FALLTHROUGH; + case WAS_EVER_ARMED: + break; + } + return NULL; +} + +static const char * osdFailsafePhaseMessage(void) +{ + // See failsafe.h for each phase explanation + switch (failsafePhase()) { +#ifdef USE_NAV + case FAILSAFE_RETURN_TO_HOME: + // XXX: Keep this in sync with OSD_FLYMODE. + return OSD_MESSAGE_STR("(RTH)"); +#endif + case FAILSAFE_LANDING: + // This should be considered an emergengy landing + return OSD_MESSAGE_STR("(EMRGY LANDING)"); + case FAILSAFE_RX_LOSS_MONITORING: + // Only reachable from FAILSAFE_LANDED, which performs + // a disarm. Since aircraft has been disarmed, we no + // longer show failsafe details. + FALLTHROUGH; + case FAILSAFE_LANDED: + // Very brief, disarms and transitions into + // FAILSAFE_RX_LOSS_MONITORING. Note that it prevents + // further rearming via ARMING_DISABLED_FAILSAFE_SYSTEM, + // so we'll show the user how to re-arm in when + // that flag is the reason to prevent arming. + FALLTHROUGH; + case FAILSAFE_RX_LOSS_IDLE: + // This only happens when user has chosen NONE as FS + // procedure. The recovery messages should be enough. + FALLTHROUGH; + case FAILSAFE_IDLE: + // Failsafe not active + FALLTHROUGH; + case FAILSAFE_RX_LOSS_DETECTED: + // Very brief, changes to FAILSAFE_RX_LOSS_RECOVERED + // or the FS procedure immediately. + FALLTHROUGH; + case FAILSAFE_RX_LOSS_RECOVERED: + // Exiting failsafe + break; + } + return NULL; +} + +static const char * osdFailsafeInfoMessage(void) +{ + if (failsafeIsReceivingRxData()) { + // User must move sticks to exit FS mode + return OSD_MESSAGE_STR("!MOVE STICKS TO EXIT FS!"); + } + return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG); +} + +static const char * navigationStateMessage(void) +{ + switch (NAV_Status.state) { + case MW_NAV_STATE_NONE: + break; + case MW_NAV_STATE_RTH_START: + return OSD_MESSAGE_STR("STARTING RTH"); + case MW_NAV_STATE_RTH_ENROUTE: + // TODO: Break this up between climb and head home + return OSD_MESSAGE_STR("EN ROUTE TO HOME"); + case MW_NAV_STATE_HOLD_INFINIT: + // Used by HOLD flight modes. No information to add. + break; + case MW_NAV_STATE_HOLD_TIMED: + // TODO: Maybe we can display a count down + return OSD_MESSAGE_STR("HOLDING WAYPOINT"); + break; + case MW_NAV_STATE_WP_ENROUTE: + // TODO: Show WP number + return OSD_MESSAGE_STR("TO WP"); + case MW_NAV_STATE_PROCESS_NEXT: + return OSD_MESSAGE_STR("PREPARING FOR NEXT WAYPOINT"); + case MW_NAV_STATE_DO_JUMP: + // Not used + break; + case MW_NAV_STATE_LAND_START: + // Not used + break; + case MW_NAV_STATE_EMERGENCY_LANDING: + return OSD_MESSAGE_STR("EMRGY LANDING"); + case MW_NAV_STATE_LAND_IN_PROGRESS: + return OSD_MESSAGE_STR("LANDING"); + case MW_NAV_STATE_HOVER_ABOVE_HOME: + if (STATE(FIXED_WING_LEGACY)) { + return OSD_MESSAGE_STR("LOITERING AROUND HOME"); + } + return OSD_MESSAGE_STR("HOVERING"); + case MW_NAV_STATE_LANDED: + return OSD_MESSAGE_STR("LANDED"); + case MW_NAV_STATE_LAND_SETTLE: + return OSD_MESSAGE_STR("PREPARING TO LAND"); + case MW_NAV_STATE_LAND_START_DESCENT: + // Not used + break; + } + return NULL; +} + + +// end cat +// new features here + +/** + * Converts velocity based on the current unit system (kmh or mph). + * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/second) + */ +static int32_t osdConvertVelocityToUnit(int32_t vel) +{ + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + return (vel * 224) / 10000; // Convert to mph + case OSD_UNIT_METRIC: + return (vel * 36) / 1000; // Convert to kmh + } + // Unreachable + return -1; +} +static int16_t osdDJIGet3DSpeed(void) +{ + int16_t vert_speed = getEstimatedActualVelocity(Z); + int16_t hor_speed = gpsSol.groundSpeed; + return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed)); +} +/** + * Converts velocity into a string based on the current unit system. + * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds) + */ +void osdDJIFormatVelocityStr(char* buff, int32_t vel ) +{ + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "MPH"); + break; + case OSD_UNIT_METRIC: + tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "KMH"); + break; + } +} +static void osdDJIFormatThrottlePosition(char *buff, bool autoThr ) +{ + + int16_t thr = rxGetChannelValue(THROTTLE); + if (autoThr && navigationIsControllingThrottle()) { + thr = rcCommand[THROTTLE]; + } + tfp_sprintf(buff, "%3d%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR"); +} + +/** + * Converts distance into a string based on the current unit system. + * @param dist Distance in centimeters + */ +static void osdDJIFormatDistanceStr(char *buff, int32_t dist) +{ + int32_t centifeet; + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_IMPERIAL: + centifeet = CENTIMETERS_TO_CENTIFEET(dist); + if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { + // Show feet when dist < 0.5mi + tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT"); + } else { + // Show miles when dist >= 0.5mi + tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)), + (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi"); + } + break; + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_METRIC: + if (abs(dist) < METERS_PER_KILOMETER * 100) { + // Show meters when dist < 1km + tfp_sprintf(buff, "%d%s", (int)(dist / 100), "M"); + } else { + // Show kilometers when dist >= 1km + tfp_sprintf(buff, "%d.%02d%s", (int)(dist / (100*METERS_PER_KILOMETER)), + (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, "KM"); + } + break; + } +} + +static void osdDJIEfficiencyMahPerKM (char *buff) +{ + // amperage is in centi amps, speed is in cms/s. We want + // mah/km. Values over 999 are considered useless and + // displayed as "---"" + static pt1Filter_t eFilterState; + static timeUs_t efficiencyUpdated = 0; + int32_t value = 0; + timeUs_t currentTimeUs = micros(); + timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); + if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { + if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, + 1, efficiencyTimeDelta * 1e-6f); + + efficiencyUpdated = currentTimeUs; + } else { + value = eFilterState.state; + } + } + if (value > 0 && value <= 999) { + tfp_sprintf(buff, "%3d%s", (int)value, "mAhKM"); + } else { + tfp_sprintf(buff, "%s", "---mAhKM"); + } +} + static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) { UNUSED(mspPostProcessFn); @@ -411,10 +753,154 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms { const char * name = systemConfig()->name; int len = strlen(name); - if (len > 12) len = 12; - sbufWriteData(dst, name, len); + + if(name[0] != ':'){ + if (len > 12) len = 12; + sbufWriteData(dst, name, len); + break; + }else{ + // :W T S E D + // | | | | Distance Trip + // | | | Efficiency mA/KM + // | | S 3dSpeed + // | Throttle + // Warnings + const char *message = " "; + const char *enabledElements = name + 1; + + char djibuf[24]; + // clear name from chars : and leading W + if(enabledElements[0] == 'W') + enabledElements += 1; + + int elemLen = strlen(enabledElements); + + if(elemLen > 0){ + switch ( enabledElements[OSD_ALTERNATING_CHOICES(3000, elemLen )] ){ + case 'T': + osdDJIFormatThrottlePosition(djibuf,true); + break; + case 'S': + osdDJIFormatVelocityStr(djibuf, osdDJIGet3DSpeed() ); + break; + case 'E': + osdDJIEfficiencyMahPerKM(djibuf); + break; + case 'D': + osdDJIFormatDistanceStr( djibuf, getTotalTravelDistance()); + break; + case 'W': + tfp_sprintf(djibuf, "%s", "MAKE_W_FIRST"); + break; + default: + tfp_sprintf(djibuf, "%s", "UNKOWN_ELEM"); + break; + } + + if(djibuf[0] != '\0') + message = djibuf; + } + + if (name[1] == 'W' ){ + char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; + if (ARMING_FLAG(ARMED)) { + // Aircraft is armed. We might have up to 5 + // messages to show. + const char *messages[5]; + unsigned messageCount = 0; + if (FLIGHT_MODE(FAILSAFE_MODE)) { + // In FS mode while being armed too + const char *failsafePhaseMessage = osdFailsafePhaseMessage(); + const char *failsafeInfoMessage = osdFailsafeInfoMessage(); + const char *navStateFSMessage = navigationStateMessage(); + if (failsafePhaseMessage) { + messages[messageCount++] = failsafePhaseMessage; + } + if (failsafeInfoMessage) { + messages[messageCount++] = failsafeInfoMessage; + } + if (navStateFSMessage) { + messages[messageCount++] = navStateFSMessage; + } + if (messageCount > 0) { + message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; + if (message == failsafeInfoMessage) { + // failsafeInfoMessage is not useful for recovering + // a lost model, but might help avoiding a crash. + // Blink to grab user attention. + //doesnt work TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + // We're shoing either failsafePhaseMessage or + // navStateFSMessage. Don't BLINK here since + // having this text available might be crucial + // during a lost aircraft recovery and blinking + // will cause it to be missing from some frames. + } + } else { + if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { + const char *navStateMessage = navigationStateMessage(); + if (navStateMessage) { + messages[messageCount++] = navStateMessage; + } + } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { + messages[messageCount++] = "AUTOLAUNCH"; + } else { + if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { + // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO + // when it doesn't require ANGLE mode (required only in FW + // right now). If if requires ANGLE, its display is handled + // by OSD_FLYMODE. + messages[messageCount++] = "(ALT HOLD)"; + } + if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { + messages[messageCount++] = "(AUTOTRIM)"; + } + if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { + messages[messageCount++] = "(AUTOTUNE)"; + } + if (FLIGHT_MODE(HEADFREE_MODE)) { + messages[messageCount++] = "(HEADFREE)"; + } + } + // Pick one of the available messages. Each message lasts + // a second. + if (messageCount > 0) { + message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; + } + } + } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { + unsigned invalidIndex; + // Check if we're unable to arm for some reason + if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) { + if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) { + const setting_t *setting = settingGet(invalidIndex); + settingGetName(setting, messageBuf); + for (int ii = 0; messageBuf[ii]; ii++) { + messageBuf[ii] = sl_toupper(messageBuf[ii]); + } + message = messageBuf; + } else { + message = "ERR SETTING"; + // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); + } + } else { + if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) { + message = "CANT ARM"; + // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); + } else { + // Show the reason for not arming + message = osdArmingDisabledReasonMessage(); + } + } + } + } + + if(message[0] != '\0') + sbufWriteData(dst, message, strlen(message)); + + break; } - break; + } case DJI_MSP_STATUS: case DJI_MSP_STATUS_EX: From 6da5c679071a29674baa62407444b454097743ab Mon Sep 17 00:00:00 2001 From: mfoo Date: Tue, 20 Oct 2020 21:15:32 +0200 Subject: [PATCH 056/109] small cosmetic correction --- src/main/io/osd_dji_hd.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 5eaf1a0dc5..36d5f127e0 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -767,7 +767,6 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms // Warnings const char *message = " "; const char *enabledElements = name + 1; - char djibuf[24]; // clear name from chars : and leading W if(enabledElements[0] == 'W') From e57dbdf3909e10e843cbcc7a930c12988944fa14 Mon Sep 17 00:00:00 2001 From: mfoo Date: Wed, 21 Oct 2020 08:36:31 +0200 Subject: [PATCH 057/109] local build works, desparate attempt to pass the ci build checks --- src/main/io/osd_dji_hd.c | 80 ++++++++++++++++++++-------------------- 1 file changed, 41 insertions(+), 39 deletions(-) diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 36d5f127e0..d62fad1a10 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -613,14 +613,14 @@ static const char * navigationStateMessage(void) */ static int32_t osdConvertVelocityToUnit(int32_t vel) { - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - return (vel * 224) / 10000; // Convert to mph - case OSD_UNIT_METRIC: +//debug-CI--local-build-works switch ((osd_unit_e)osdConfig()->units) { +//debug-CI--local-build-works case OSD_UNIT_UK: +//debug-CI--local-build-works FALLTHROUGH; +//debug-CI--local-build-works case OSD_UNIT_IMPERIAL: +//debug-CI--local-build-works return (vel * 224) / 10000; // Convert to mph +//debug-CI--local-build-works case OSD_UNIT_METRIC: return (vel * 36) / 1000; // Convert to kmh - } +//debug-CI--local-build-works } // Unreachable return -1; } @@ -636,16 +636,16 @@ static int16_t osdDJIGet3DSpeed(void) */ void osdDJIFormatVelocityStr(char* buff, int32_t vel ) { - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "MPH"); - break; - case OSD_UNIT_METRIC: +//debug-CI--local-build-works switch ((osd_unit_e)osdConfig()->units) { +//debug-CI--local-build-works case OSD_UNIT_UK: +//debug-CI--local-build-works FALLTHROUGH; +//debug-CI--local-build-works case OSD_UNIT_IMPERIAL: +//debug-CI--local-build-works tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "MPH"); +//debug-CI--local-build-works break; +//debug-CI--local-build-works case OSD_UNIT_METRIC: tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "KMH"); - break; - } +//debug-CI--local-build-works break; +//debug-CI--local-build-works } } static void osdDJIFormatThrottlePosition(char *buff, bool autoThr ) { @@ -663,22 +663,22 @@ static void osdDJIFormatThrottlePosition(char *buff, bool autoThr ) */ static void osdDJIFormatDistanceStr(char *buff, int32_t dist) { - int32_t centifeet; - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_IMPERIAL: - centifeet = CENTIMETERS_TO_CENTIFEET(dist); - if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { - // Show feet when dist < 0.5mi - tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT"); - } else { - // Show miles when dist >= 0.5mi - tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)), - (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi"); - } - break; - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_METRIC: +//debug-CI--local-build-works int32_t centifeet; +//debug-CI--local-build-works switch ((osd_unit_e)osdConfig()->units) { +//debug-CI--local-build-works case OSD_UNIT_IMPERIAL: +//debug-CI--local-build-works centifeet = CENTIMETERS_TO_CENTIFEET(dist); +//debug-CI--local-build-works if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { +//debug-CI--local-build-works // Show feet when dist < 0.5mi +//debug-CI--local-build-works tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT"); +//debug-CI--local-build-works } else { +//debug-CI--local-build-works // Show miles when dist >= 0.5mi +//debug-CI--local-build-works tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)), +//debug-CI--local-build-works (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi"); +//debug-CI--local-build-works } +//debug-CI--local-build-works break; +//debug-CI--local-build-works case OSD_UNIT_UK: +//debug-CI--local-build-works FALLTHROUGH; +//debug-CI--local-build-works case OSD_UNIT_METRIC: if (abs(dist) < METERS_PER_KILOMETER * 100) { // Show meters when dist < 1km tfp_sprintf(buff, "%d%s", (int)(dist / 100), "M"); @@ -687,8 +687,8 @@ static void osdDJIFormatDistanceStr(char *buff, int32_t dist) tfp_sprintf(buff, "%d.%02d%s", (int)(dist / (100*METERS_PER_KILOMETER)), (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, "KM"); } - break; - } +//debug-CI--local-build-works break; +//debug-CI--local-build-works } } static void osdDJIEfficiencyMahPerKM (char *buff) @@ -769,8 +769,9 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms const char *enabledElements = name + 1; char djibuf[24]; // clear name from chars : and leading W - if(enabledElements[0] == 'W') + if(enabledElements[0] == 'W'){ enabledElements += 1; + } int elemLen = strlen(enabledElements); @@ -796,8 +797,9 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms break; } - if(djibuf[0] != '\0') + if(djibuf[0] != '\0'){ message = djibuf; + } } if (name[1] == 'W' ){ @@ -894,12 +896,12 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms } } - if(message[0] != '\0') + if(message[0] != '\0'){ sbufWriteData(dst, message, strlen(message)); - - break; + } } } + break; case DJI_MSP_STATUS: case DJI_MSP_STATUS_EX: From 3191ece5df4b4c402737e0d37f6bdd9c7ba25914 Mon Sep 17 00:00:00 2001 From: mfoo Date: Wed, 21 Oct 2020 10:07:40 +0200 Subject: [PATCH 058/109] compiled, flashed and tested unit IMPERIAL successful --- src/main/io/osd_dji_hd.c | 68 ++++++++++++++++++++-------------------- 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index d62fad1a10..532e9477b4 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -613,14 +613,14 @@ static const char * navigationStateMessage(void) */ static int32_t osdConvertVelocityToUnit(int32_t vel) { -//debug-CI--local-build-works switch ((osd_unit_e)osdConfig()->units) { -//debug-CI--local-build-works case OSD_UNIT_UK: -//debug-CI--local-build-works FALLTHROUGH; -//debug-CI--local-build-works case OSD_UNIT_IMPERIAL: -//debug-CI--local-build-works return (vel * 224) / 10000; // Convert to mph -//debug-CI--local-build-works case OSD_UNIT_METRIC: + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + return (vel * 224) / 10000; // Convert to mph + case OSD_UNIT_METRIC: return (vel * 36) / 1000; // Convert to kmh -//debug-CI--local-build-works } + } // Unreachable return -1; } @@ -636,16 +636,16 @@ static int16_t osdDJIGet3DSpeed(void) */ void osdDJIFormatVelocityStr(char* buff, int32_t vel ) { -//debug-CI--local-build-works switch ((osd_unit_e)osdConfig()->units) { -//debug-CI--local-build-works case OSD_UNIT_UK: -//debug-CI--local-build-works FALLTHROUGH; -//debug-CI--local-build-works case OSD_UNIT_IMPERIAL: -//debug-CI--local-build-works tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "MPH"); -//debug-CI--local-build-works break; -//debug-CI--local-build-works case OSD_UNIT_METRIC: + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "MPH"); + break; + case OSD_UNIT_METRIC: tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "KMH"); -//debug-CI--local-build-works break; -//debug-CI--local-build-works } + break; + } } static void osdDJIFormatThrottlePosition(char *buff, bool autoThr ) { @@ -663,22 +663,22 @@ static void osdDJIFormatThrottlePosition(char *buff, bool autoThr ) */ static void osdDJIFormatDistanceStr(char *buff, int32_t dist) { -//debug-CI--local-build-works int32_t centifeet; -//debug-CI--local-build-works switch ((osd_unit_e)osdConfig()->units) { -//debug-CI--local-build-works case OSD_UNIT_IMPERIAL: -//debug-CI--local-build-works centifeet = CENTIMETERS_TO_CENTIFEET(dist); -//debug-CI--local-build-works if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { -//debug-CI--local-build-works // Show feet when dist < 0.5mi -//debug-CI--local-build-works tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT"); -//debug-CI--local-build-works } else { -//debug-CI--local-build-works // Show miles when dist >= 0.5mi -//debug-CI--local-build-works tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)), -//debug-CI--local-build-works (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi"); -//debug-CI--local-build-works } -//debug-CI--local-build-works break; -//debug-CI--local-build-works case OSD_UNIT_UK: -//debug-CI--local-build-works FALLTHROUGH; -//debug-CI--local-build-works case OSD_UNIT_METRIC: + int32_t centifeet; + switch (osdConfig()->units) { + case OSD_UNIT_IMPERIAL: + centifeet = CENTIMETERS_TO_CENTIFEET(dist); + if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { + // Show feet when dist < 0.5mi + tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT"); + } else { + // Show miles when dist >= 0.5mi + tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)), + (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi"); + } + break; + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_METRIC: if (abs(dist) < METERS_PER_KILOMETER * 100) { // Show meters when dist < 1km tfp_sprintf(buff, "%d%s", (int)(dist / 100), "M"); @@ -687,8 +687,8 @@ static void osdDJIFormatDistanceStr(char *buff, int32_t dist) tfp_sprintf(buff, "%d.%02d%s", (int)(dist / (100*METERS_PER_KILOMETER)), (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, "KM"); } -//debug-CI--local-build-works break; -//debug-CI--local-build-works } + break; + } } static void osdDJIEfficiencyMahPerKM (char *buff) From 5e4987179f12f9463f8d84049fbab814fb03a25e Mon Sep 17 00:00:00 2001 From: mfoo Date: Wed, 21 Oct 2020 11:10:10 +0200 Subject: [PATCH 059/109] adding ifdef USE_OSD around new functions --- src/main/io/osd_dji_hd.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 532e9477b4..7c2d03e9d7 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -398,6 +398,7 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst) } #endif +#if defined(USE_OSD) //testme static const char * osdArmingDisabledReasonMessage(void) { switch (isArmingDisabledReason()) { @@ -604,9 +605,6 @@ static const char * navigationStateMessage(void) } -// end cat -// new features here - /** * Converts velocity based on the current unit system (kmh or mph). * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/second) @@ -717,6 +715,7 @@ static void osdDJIEfficiencyMahPerKM (char *buff) tfp_sprintf(buff, "%s", "---mAhKM"); } } +#endif static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) { @@ -758,7 +757,8 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms if (len > 12) len = 12; sbufWriteData(dst, name, len); break; - }else{ + }else{ +#if defined(USE_OSD) // :W T S E D // | | | | Distance Trip // | | | Efficiency mA/KM @@ -899,6 +899,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms if(message[0] != '\0'){ sbufWriteData(dst, message, strlen(message)); } +#endif } } break; From a88bab5f91fb8c6c0b07171373e09cc74be7c4a1 Mon Sep 17 00:00:00 2001 From: MiguelFAlvarez Date: Fri, 23 Oct 2020 01:13:18 -0500 Subject: [PATCH 060/109] Added Spektrum SRXL2 support. (#5791) This consists of various files/changes brought over from betaflight with modifications to operate in the current state of inav. It also includes files/changes that were not a part of the betaflight SRXL2 merge, as the previous bidi srxl implementation was not yet implemented either, and SRXL2 has some dependencies on the Spektrum telemetry structuring from those files. --- src/main/CMakeLists.txt | 6 + src/main/fc/cli.c | 33 ++ src/main/fc/fc_init.c | 6 + src/main/fc/settings.yaml | 11 +- src/main/io/displayport_srxl.c | 152 ++++++ src/main/io/displayport_srxl.h | 25 + src/main/rx/rx.c | 17 +- src/main/rx/rx.h | 3 + src/main/rx/spektrum.c | 129 ++++-- src/main/rx/spektrum.h | 22 +- src/main/rx/srxl2.c | 580 +++++++++++++++++++++++ src/main/rx/srxl2.h | 15 + src/main/rx/srxl2_types.h | 138 ++++++ src/main/target/common.h | 8 + src/main/telemetry/srxl.c | 812 +++++++++++++++++++++++++++++++++ src/main/telemetry/srxl.h | 35 ++ src/main/telemetry/telemetry.c | 13 + 17 files changed, 1958 insertions(+), 47 deletions(-) create mode 100644 src/main/io/displayport_srxl.c create mode 100644 src/main/io/displayport_srxl.h create mode 100644 src/main/rx/srxl2.c create mode 100644 src/main/rx/srxl2.h create mode 100644 src/main/rx/srxl2_types.h create mode 100644 src/main/telemetry/srxl.c create mode 100644 src/main/telemetry/srxl.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 674b766d0b..146164679e 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -401,6 +401,8 @@ main_sources(COMMON_SRC rx/sbus_channels.h rx/spektrum.c rx/spektrum.h + rx/srxl2.c + rx/srxl2.h rx/sumd.c rx/sumd.h rx/sumh.c @@ -486,6 +488,8 @@ main_sources(COMMON_SRC io/displayport_msp.h io/displayport_oled.c io/displayport_oled.h + io/displayport_srxl.c + io/displayport_srxl.h io/displayport_hott.c io/displayport_hott.h io/flashfs.c @@ -547,6 +551,8 @@ main_sources(COMMON_SRC telemetry/crsf.c telemetry/crsf.h + telemetry/srxl.c + telemetry/srxl.h telemetry/frsky.c telemetry/frsky.h telemetry/frsky_d.c diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index cdeac26521..53c7fdb58c 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -103,6 +103,7 @@ extern uint8_t __config_end; #include "rx/rx.h" #include "rx/spektrum.h" #include "rx/eleres.h" +#include "rx/srxl2.h" #include "scheduler/scheduler.h" @@ -2578,6 +2579,35 @@ static void cliEleresBind(char *cmdline) } #endif // USE_RX_ELERES +#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2) +void cliRxBind(char *cmdline){ + UNUSED(cmdline); + if (rxConfig()->receiverType == RX_TYPE_SERIAL) { + switch (rxConfig()->serialrx_provider) { + default: + cliPrint("Not supported."); + break; +#if defined(USE_SERIALRX_SRXL2) + case SERIALRX_SRXL2: + srxl2Bind(); + cliPrint("Binding SRXL2 receiver..."); + break; +#endif + } + } +#if defined(USE_RX_SPI) + else if (rxConfig()->receiverType == RX_TYPE_SPI) { + switch (rxConfig()->rx_spi_protocol) { + default: + cliPrint("Not supported."); + break; + } + + } +#endif +} +#endif + static void cliExit(char *cmdline) { UNUSED(cmdline); @@ -3465,6 +3495,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n" "\t<+|->[name]", cliBeeper), #endif +#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2) + CLI_COMMAND_DEF("bind_rx", "initiate binding for RX SPI or SRXL2", NULL, cliRxBind), +#endif #if defined(USE_BOOTLOG) CLI_COMMAND_DEF("bootlog", "show boot events", NULL, cliBootlog), #endif diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 36b17ec055..c3da09cdf1 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -107,6 +107,7 @@ #include "io/displayport_frsky_osd.h" #include "io/displayport_msp.h" #include "io/displayport_max7456.h" +#include "io/displayport_srxl.h" #include "io/flashfs.h" #include "io/gps.h" #include "io/ledstrip.h" @@ -573,6 +574,11 @@ void init(void) uavInterconnectBusInit(); #endif +#if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL) + // Register the srxl Textgen telemetry sensor as a displayport device + cmsDisplayPortRegister(displayPortSrxlInit()); +#endif + #ifdef USE_GPS if (feature(FEATURE_GPS)) { gpsInit(); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index fd611a8547..2d549692ad 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -25,7 +25,7 @@ tables: values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"] enum: rxReceiverType_e - name: serial_rx - values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2"] + values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2"] - name: rx_spi_protocol values: ["V202_250K", "V202_1M", "SYMA_X", "SYMA_X5C", "CX10", "CX10A", "H8_3D", "INAV", "ELERES"] enum: rx_spi_protocol_e @@ -144,6 +144,8 @@ tables: - name: tristate enum: tristate_e values: ["AUTO", "ON", "OFF"] + - name: off_on + values: ["OFF", "ON"] groups: - name: PG_GYRO_CONFIG @@ -567,6 +569,13 @@ groups: condition: USE_SPEKTRUM_BIND min: SPEKTRUM_SAT_BIND_DISABLED max: SPEKTRUM_SAT_BIND_MAX + - name: srxl2_unit_id + condition: USE_SERIALRX_SRXL2 + min: 0 + max: 15 + - name: srxl2_baud_fast + condition: USE_SERIALRX_SRXL2 + table: off_on - name: rx_min_usec description: "Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc." default_value: "885" diff --git a/src/main/io/displayport_srxl.c b/src/main/io/displayport_srxl.c new file mode 100644 index 0000000000..8c73be1ae5 --- /dev/null +++ b/src/main/io/displayport_srxl.c @@ -0,0 +1,152 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" +#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) && defined(USE_TELEMETRY_SRXL) + +#include "common/utils.h" + +#include "drivers/display.h" +#include "cms/cms.h" + +#include "telemetry/srxl.h" + +displayPort_t srxlDisplayPort; + +static int srxlDrawScreen(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int srxlScreenSize(const displayPort_t *displayPort) +{ + return displayPort->rows * displayPort->cols; +} + +static int srxlWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr) +{ + return (spektrumTmTextGenPutChar(col, row, c)); + UNUSED(displayPort); + UNUSED(attr); +} + + +static int srxlWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *s, textAttributes_t attr) +{ + while (*s) { + srxlWriteChar(displayPort, col++, row, *(s++), attr); + } + return 0; +} + +static int srxlClearScreen(displayPort_t *displayPort) +{ + textAttributes_t attr = 0; + for (int row = 0; row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS; row++) { + for (int col= 0; col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS; col++) { + srxlWriteChar(displayPort, col, row, ' ', attr); + } + } + srxlWriteString(displayPort, 1, 0, "INAV", attr); + + if (displayPort->grabCount == 0) { + srxlWriteString(displayPort, 0, 2, CMS_STARTUP_HELP_TEXT1, attr); + srxlWriteString(displayPort, 2, 3, CMS_STARTUP_HELP_TEXT2, attr); + srxlWriteString(displayPort, 2, 4, CMS_STARTUP_HELP_TEXT3, attr); + } + return 0; +} + +static bool srxlIsTransferInProgress(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return false; +} + +/* +static bool srxlIsSynced(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return true; +} +*/ + +static int srxlHeartbeat(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static void srxlResync(displayPort_t *displayPort) +{ + UNUSED(displayPort); +} + +static uint32_t srxlTxBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return UINT32_MAX; +} + +static int srxlGrab(displayPort_t *displayPort) +{ + return displayPort->grabCount = 1; +} + +static int srxlRelease(displayPort_t *displayPort) +{ + int cnt = displayPort->grabCount = 0; + srxlClearScreen(displayPort); + return cnt; +} + +static const displayPortVTable_t srxlVTable = { + .grab = srxlGrab, + .release = srxlRelease, + .clearScreen = srxlClearScreen, + .drawScreen = srxlDrawScreen, + .screenSize = srxlScreenSize, + .writeString = srxlWriteString, + .writeChar = srxlWriteChar, + .isTransferInProgress = srxlIsTransferInProgress, + .heartbeat = srxlHeartbeat, + .resync = srxlResync, + //.isSynced = srxlIsSynced, + .txBytesFree = srxlTxBytesFree, + //.layerSupported = NULL, + //.layerSelect = NULL, + //.layerCopy = NULL, +}; + +displayPort_t *displayPortSrxlInit(void) +{ + srxlDisplayPort.device = NULL; + displayInit(&srxlDisplayPort, &srxlVTable); + srxlDisplayPort.rows = SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS; + srxlDisplayPort.cols = SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS; + return &srxlDisplayPort; +} + +#endif diff --git a/src/main/io/displayport_srxl.h b/src/main/io/displayport_srxl.h new file mode 100644 index 0000000000..62301a9436 --- /dev/null +++ b/src/main/io/displayport_srxl.h @@ -0,0 +1,25 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +displayPort_t *displayPortSrxlInit(void); + +extern displayPort_t srxlDisplayPort; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 5c2110f791..fa24d6ceb9 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -61,6 +61,7 @@ #include "rx/rx_spi.h" #include "rx/sbus.h" #include "rx/spektrum.h" +#include "rx/srxl2.h" #include "rx/sumd.h" #include "rx/sumh.h" #include "rx/uib_rx.h" @@ -91,6 +92,7 @@ static bool mspOverrideDataProcessingRequired = false; static bool rxSignalReceived = false; static bool rxFlightChannelsValid = false; static bool rxIsInFailsafeMode = true; +static uint8_t rxChannelCount; static timeUs_t rxNextUpdateAtUs = 0; static timeUs_t needRxSignalBefore = 0; @@ -141,6 +143,8 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig, .mspOverrideChannels = 15, #endif .rssi_source = RSSI_SOURCE_AUTO, + .srxl2_unit_id = 1, + .srxl2_baud_fast = 1, ); void resetAllRxChannelRangeConfigurations(void) @@ -187,6 +191,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig { bool enabled = false; switch (rxConfig->serialrx_provider) { +#ifdef USE_SERIALRX_SRXL2 + case SERIALRX_SRXL2: + enabled = srxl2RxInit(rxConfig, rxRuntimeConfig); + break; +#endif #ifdef USE_SERIALRX_SPEKTRUM case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: @@ -347,6 +356,8 @@ void rxInit(void) mspOverrideInit(); } #endif + + rxChannelCount = MIN(MAX_SUPPORTED_RC_CHANNEL_COUNT, rxRuntimeConfig.channelCount); } void rxUpdateRSSISource(void) @@ -516,7 +527,7 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) rxFlightChannelsValid = true; // Read and process channel data - for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { + for (int channel = 0; channel < rxChannelCount; channel++) { const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); // sample the channel @@ -549,11 +560,11 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) // If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good input values are retained if (rxFlightChannelsValid && rxSignalReceived) { if (rxRuntimeConfig.requireFiltering) { - for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { + for (int channel = 0; channel < rxChannelCount; channel++) { rcChannels[channel].data = applyChannelFiltering(channel, rcStaging[channel]); } } else { - for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { + for (int channel = 0; channel < rxChannelCount; channel++) { rcChannels[channel].data = rcStaging[channel]; } } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index af27191bb7..f16a0c0f7f 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -82,6 +82,7 @@ typedef enum { SERIALRX_FPORT = 10, SERIALRX_SBUS_FAST = 11, SERIALRX_FPORT2 = 12, + SERIALRX_SRXL2 = 13, } rxSerialReceiverType_e; #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 16 @@ -128,6 +129,8 @@ typedef struct rxConfig_s { uint8_t rcFilterFrequency; // RC filter cutoff frequency (smoothness vs response sharpness) uint16_t mspOverrideChannels; // Channels to override with MSP RC when BOXMSPRCOVERRIDE is active uint8_t rssi_source; + uint8_t srxl2_unit_id; + uint8_t srxl2_baud_fast; } rxConfig_t; PG_DECLARE(rxConfig_t, rxConfig); diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 42b46c7b3b..8e1263939f 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -15,10 +15,13 @@ * along with Cleanflight. If not, see . */ +#include #include #include #include +#include "common/maths.h" + #include "platform.h" #ifdef USE_SERIALRX_SPEKTRUM @@ -44,19 +47,13 @@ #include "rx/spektrum.h" // driver for spektrum satellite receiver / sbus - -#define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12 -#define SPEKTRUM_2048_CHANNEL_COUNT 12 -#define SPEKTRUM_1024_CHANNEL_COUNT 7 - -#define SPEK_FRAME_SIZE 16 -#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000 - -#define SPEKTRUM_BAUDRATE 115200 +#define SPEKTRUM_TELEMETRY_FRAME_DELAY_US 1000 // Gap between received Rc frame and transmited TM frame #define SPEKTRUM_MAX_FADE_PER_SEC 40 #define SPEKTRUM_FADE_REPORTS_PER_SEC 2 +bool srxlEnabled = false; + static uint8_t spek_chan_shift; static uint8_t spek_chan_mask; static bool rcFrameComplete = false; @@ -81,6 +78,10 @@ static IO_t BindPin = DEFIO_IO(NONE); static IO_t BindPlug = DEFIO_IO(NONE); #endif +#if defined(USE_TELEMETRY_SRXL) +static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX]; +static uint8_t telemetryBufLen = 0; +#endif // Receive ISR callback static void spektrumDataReceive(uint16_t c, void *rxCallbackData) @@ -116,46 +117,67 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxRuntimeConfig); - if (!rcFrameComplete) { - return RX_FRAME_PENDING; - } +#if defined(USE_TELEMETRY_SRXL) + static timeUs_t telemetryFrameRequestedUs = 0; - rcFrameComplete = false; + timeUs_t currentTimeUs = micros(); +#endif - // Fetch the fade count - const uint16_t fade = (spekFrame[0] << 8) + spekFrame[1]; - const uint32_t current_secs = millis() / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC); + uint8_t result = RX_FRAME_PENDING; - if (spek_fade_last_sec == 0) { - // This is the first frame status received. - spek_fade_last_sec_count = fade; - spek_fade_last_sec = current_secs; - } else if (spek_fade_last_sec != current_secs) { - // If the difference is > 1, then we missed several seconds worth of frames and - // should just throw out the fade calc (as it's likely a full signal loss). - if ((current_secs - spek_fade_last_sec) == 1) { - if (rssi_channel != 0) { - if (spekHiRes) - spekChannelData[rssi_channel] = 2048 - ((fade - spek_fade_last_sec_count) * 2048 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC)); - else - spekChannelData[rssi_channel] = 1024 - ((fade - spek_fade_last_sec_count) * 1024 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC)); + if (rcFrameComplete) { + rcFrameComplete = false; + + // Fetch the fade count + const uint16_t fade = (spekFrame[0] << 8) + spekFrame[1]; + const uint32_t current_secs = millis() / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC); + + if (spek_fade_last_sec == 0) { + // This is the first frame status received. + spek_fade_last_sec_count = fade; + spek_fade_last_sec = current_secs; + } else if (spek_fade_last_sec != current_secs) { + // If the difference is > 1, then we missed several seconds worth of frames and + // should just throw out the fade calc (as it's likely a full signal loss). + if ((current_secs - spek_fade_last_sec) == 1) { + if (rssi_channel != 0) { + if (spekHiRes) + spekChannelData[rssi_channel] = 2048 - ((fade - spek_fade_last_sec_count) * 2048 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC)); + else + spekChannelData[rssi_channel] = 1024 - ((fade - spek_fade_last_sec_count) * 1024 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC)); + } + } + spek_fade_last_sec_count = fade; + spek_fade_last_sec = current_secs; + } + + + for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) { + const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift); + if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) { + if (rssi_channel == 0 || spekChannel != rssi_channel) { + spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b]; + } } } - spek_fade_last_sec_count = fade; - spek_fade_last_sec = current_secs; - } - - for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) { - const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift); - if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) { - if (rssi_channel == 0 || spekChannel != rssi_channel) { - spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b]; - } +#if defined(USE_TELEMETRY_SRXL) + if (srxlEnabled && (spekFrame[2] & 0x80) == 0) { + telemetryFrameRequestedUs = currentTimeUs; } - } +#endif - return RX_FRAME_COMPLETE; + result = RX_FRAME_COMPLETE; + } +#if defined(USE_TELEMETRY_SRXL) + if (telemetryBufLen && telemetryFrameRequestedUs && cmpTimeUs(currentTimeUs, telemetryFrameRequestedUs) >= SPEKTRUM_TELEMETRY_FRAME_DELAY_US) { + telemetryFrameRequestedUs = 0; + + result = (result & ~RX_FRAME_PENDING) | RX_FRAME_PROCESSING_REQUIRED; + } +#endif + + return result; } static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) @@ -251,6 +273,25 @@ void spektrumBind(rxConfig_t *rxConfig) } #endif // SPEKTRUM_BIND +#if defined(USE_TELEMETRY_SRXL) + +bool srxlTelemetryBufferEmpty(void) +{ + if (telemetryBufLen == 0) { + return true; + } else { + return false; + } +} + +void srxlRxWriteTelemetryData(const void *data, int len) +{ + len = MIN(len, (int)sizeof(telemetryBuf)); + memcpy(telemetryBuf, data, len); + telemetryBufLen = len; +} +#endif + bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { rxRuntimeConfigPtr = rxRuntimeConfig; @@ -310,4 +351,10 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig return serialPort != NULL; } + +bool srxlRxIsActive(void) +{ + return serialPort != NULL; +} + #endif // USE_SERIALRX_SPEKTRUM diff --git a/src/main/rx/spektrum.h b/src/main/rx/spektrum.h index 44aab64192..c63396b4a9 100644 --- a/src/main/rx/spektrum.h +++ b/src/main/rx/spektrum.h @@ -17,8 +17,26 @@ #pragma once -#define SPEKTRUM_SAT_BIND_DISABLED 0 -#define SPEKTRUM_SAT_BIND_MAX 10 +#define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12 +#define SPEKTRUM_2048_CHANNEL_COUNT 12 +#define SPEKTRUM_1024_CHANNEL_COUNT 7 + +#define SPEKTRUM_SAT_BIND_DISABLED 0 +#define SPEKTRUM_SAT_BIND_MAX 10 + +#define SPEK_FRAME_SIZE 16 +#define SRXL_FRAME_OVERHEAD 5 +#define SRXL_FRAME_SIZE_MAX (SPEK_FRAME_SIZE + SRXL_FRAME_OVERHEAD) + +#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000 + +#define SPEKTRUM_BAUDRATE 115200 + +extern bool srxlEnabled; void spektrumBind(rxConfig_t *rxConfig); bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); + +bool srxlTelemetryBufferEmpty(void); +void srxlRxWriteTelemetryData(const void *data, int len); +bool srxlRxIsActive(void); \ No newline at end of file diff --git a/src/main/rx/srxl2.c b/src/main/rx/srxl2.c new file mode 100644 index 0000000000..221995ed68 --- /dev/null +++ b/src/main/rx/srxl2.c @@ -0,0 +1,580 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" + +#ifdef USE_SERIALRX_SRXL2 + +#include "common/crc.h" +#include "common/maths.h" +#include "common/streambuf.h" + +#include "drivers/nvic.h" +#include "drivers/time.h" +#include "drivers/serial.h" +#include "drivers/serial_uart.h" + +#include "io/serial.h" + +#include "rx/srxl2.h" +#include "rx/srxl2_types.h" + +#ifndef SRXL2_DEBUG +#define SRXL2_DEBUG 0 +#endif + +#if SRXL2_DEBUG +//void cliPrintf(const char *format, ...); +//#define DEBUG_PRINTF(format, ...) cliPrintf(format, __VA_ARGS__) +#define DEBUG_PRINTF(...) //Temporary until a better debug printf can be included +#else +#define DEBUG_PRINTF(...) +#endif + + + +#define SRXL2_MAX_CHANNELS 32 +#define SRXL2_FRAME_PERIOD_US 11000 // 5500 for DSMR +#define SRXL2_CHANNEL_SHIFT 5 +#define SRXL2_CHANNEL_CENTER 0x8000 + +#define SRXL2_PORT_BAUDRATE_DEFAULT 115200 +#define SRXL2_PORT_BAUDRATE_HIGH 400000 +#define SRXL2_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_BIDIR) +#define SRXL2_PORT_MODE MODE_RXTX + +#define SRXL2_REPLY_QUIESCENCE (2 * 10 * 1000000 / SRXL2_PORT_BAUDRATE_DEFAULT) // 2 * (lastIdleTimestamp - lastReceiveTimestamp). Time taken to send 2 bytes + +#define SRXL2_ID 0xA6 +#define SRXL2_MAX_PACKET_LENGTH 80 +#define SRXL2_DEVICE_ID_BROADCAST 0xFF + +#define SRXL2_FRAME_TIMEOUT_US 50000 + +#define SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US 50000 +#define SRXL2_SEND_HANDSHAKE_TIMEOUT_US 50000 +#define SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US 200000 + +#define SPEKTRUM_PULSE_OFFSET 988 // Offset value to convert digital data into RC pulse + +typedef union { + uint8_t raw[SRXL2_MAX_PACKET_LENGTH]; + Srxl2Header header; +} Srxl2Frame; + +struct rxBuf { + volatile unsigned len; + Srxl2Frame packet; +}; + +static uint8_t unitId = 0; +static uint8_t baudRate = 0; + +static Srxl2State state = Disabled; +static uint32_t timeoutTimestamp = 0; +static uint32_t fullTimeoutTimestamp = 0; +static uint32_t lastValidPacketTimestamp = 0; +static volatile uint32_t lastReceiveTimestamp = 0; +static volatile uint32_t lastIdleTimestamp = 0; + +struct rxBuf readBuffer[2]; +struct rxBuf* readBufferPtr = &readBuffer[0]; +struct rxBuf* processBufferPtr = &readBuffer[1]; +static volatile unsigned readBufferIdx = 0; +static volatile bool transmittingTelemetry = false; +static uint8_t writeBuffer[SRXL2_MAX_PACKET_LENGTH]; +static unsigned writeBufferIdx = 0; + +static serialPort_t *serialPort; + +static uint8_t busMasterDeviceId = 0xFF; +static bool telemetryRequested = false; + +static uint8_t telemetryFrame[22]; + +uint8_t globalResult = 0; + +/* handshake protocol + 1. listen for 50ms for serial activity and go to State::Running if found, autobaud may be necessary + 2. if srxl2_unitId = 0: + send a Handshake with destinationDeviceId = 0 every 50ms for at least 200ms + else: + listen for Handshake for at least 200ms + 3. respond to Handshake as currently implemented in process if rePst received + 4. respond to broadcast Handshake +*/ + +// if 50ms with not activity, go to default baudrate and to step 1 + +bool srxl2ProcessHandshake(const Srxl2Header* header) +{ + const Srxl2HandshakeSubHeader* handshake = (Srxl2HandshakeSubHeader*)(header + 1); + if (handshake->destinationDeviceId == Broadcast) { + DEBUG_PRINTF("broadcast handshake from %x\r\n", handshake->sourceDeviceId); + busMasterDeviceId = handshake->sourceDeviceId; + + if (handshake->baudSupported == 1) { + serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_HIGH); + DEBUG_PRINTF("switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_HIGH); + } + + state = Running; + + return true; + } + + + if (handshake->destinationDeviceId != ((FlightController << 4) | unitId)) { + return true; + } + + DEBUG_PRINTF("FC handshake from %x\r\n", handshake->sourceDeviceId); + + Srxl2HandshakeFrame response = { + .header = *header, + .payload = { + handshake->destinationDeviceId, + handshake->sourceDeviceId, + /* priority */ 10, + /* baudSupported*/ baudRate, + /* info */ 0, + // U_ID_2 + } + }; + + srxl2RxWriteData(&response, sizeof(response)); + + return true; +} + +void srxl2ProcessChannelData(const Srxl2ChannelDataHeader* channelData, rxRuntimeConfig_t *rxRuntimeConfig) { + globalResult = RX_FRAME_COMPLETE; + + if (channelData->rssi >= 0) { + const int rssiPercent = channelData->rssi; + lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(rssiPercent, 0, 100), 0, 100, 0, RSSI_MAX_VALUE)); + //setRssi(scaleRange(rssiPercent, 0, 100, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_PROTOCOL); + } + + //If receiver is in a connected state, and a packet is missed, the channel mask will be 0. + if (!channelData->channelMask.u32) { + globalResult |= RX_FRAME_FAILSAFE; + return; + } + + const uint16_t *frameChannels = (const uint16_t *) (channelData + 1); + uint32_t channelMask = channelData->channelMask.u32; + while (channelMask) { + unsigned idx = __builtin_ctz (channelMask); + uint32_t mask = 1 << idx; + rxRuntimeConfig->channelData[idx] = *frameChannels++; + channelMask &= ~mask; + } + + DEBUG_PRINTF("channel data: %d %d %x\r\n", channelData_header->rssi, channelData_header->frameLosses, channelData_header->channelMask.u32); +} + +bool srxl2ProcessControlData(const Srxl2Header* header, rxRuntimeConfig_t *rxRuntimeConfig) +{ + const Srxl2ControlDataSubHeader* controlData = (Srxl2ControlDataSubHeader*)(header + 1); + const uint8_t ownId = (FlightController << 4) | unitId; + if (controlData->replyId == ownId) { + telemetryRequested = true; + DEBUG_PRINTF("command: %x replyId: %x ownId: %x\r\n", controlData->command, controlData->replyId, ownId); + } + + switch (controlData->command) { + case ChannelData: + srxl2ProcessChannelData((const Srxl2ChannelDataHeader *) (controlData + 1), rxRuntimeConfig); + break; + + case FailsafeChannelData: { + globalResult |= RX_FRAME_FAILSAFE; + //setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL); + lqTrackerSet(rxRuntimeConfig->lqTracker, 0); + // DEBUG_PRINTF("fs channel data\r\n"); + } break; + + case VTXData: { +#if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON) + Srxl2VtxData *vtxData = (Srxl2VtxData*)(controlData + 1); + DEBUG_PRINTF("vtx data\r\n"); + DEBUG_PRINTF("vtx band: %x\r\n", vtxData->band); + DEBUG_PRINTF("vtx channel: %x\r\n", vtxData->channel); + DEBUG_PRINTF("vtx pit: %x\r\n", vtxData->pit); + DEBUG_PRINTF("vtx power: %x\r\n", vtxData->power); + DEBUG_PRINTF("vtx powerDec: %x\r\n", vtxData->powerDec); + DEBUG_PRINTF("vtx region: %x\r\n", vtxData->region); + // Pack data as it was used before srxl2 to use existing functions. + // Get the VTX control bytes in a frame + uint32_t vtxControl = (0xE0 << 24) | (0xE0 << 8) | + ((vtxData->band & 0x07) << 21) | + ((vtxData->channel & 0x0F) << 16) | + ((vtxData->pit & 0x01) << 4) | + ((vtxData->region & 0x01) << 3) | + ((vtxData->power & 0x07)); + spektrumHandleVtxControl(vtxControl); +#endif + } break; + } + + return true; +} + +bool srxl2ProcessPacket(const Srxl2Header* header, rxRuntimeConfig_t *rxRuntimeConfig) +{ + switch (header->packetType) { + case Handshake: + return srxl2ProcessHandshake(header); + case ControlData: + return srxl2ProcessControlData(header, rxRuntimeConfig); + default: + DEBUG_PRINTF("Other packet type, ID: %x \r\n", header->packetType); + break; + } + + return false; +} + +// @note assumes packet is fully there +void srxl2Process(rxRuntimeConfig_t *rxRuntimeConfig) +{ + if (processBufferPtr->packet.header.id != SRXL2_ID || processBufferPtr->len != processBufferPtr->packet.header.length) { + DEBUG_PRINTF("invalid header id: %x, or length: %x received vs %x expected \r\n", processBufferPtr->packet.header.id, processBufferPtr->len, processBufferPtr->packet.header.length); + globalResult = RX_FRAME_DROPPED; + return; + } + + const uint16_t calculatedCrc = crc16_ccitt_update(0, processBufferPtr->packet.raw, processBufferPtr->packet.header.length); + + //Invalid if crc non-zero + if (calculatedCrc) { + globalResult = RX_FRAME_DROPPED; + DEBUG_PRINTF("crc mismatch %x\r\n", calculatedCrc); + return; + } + + //Packet is valid only after ID and CRC check out + lastValidPacketTimestamp = micros(); + + if (srxl2ProcessPacket(&processBufferPtr->packet.header, rxRuntimeConfig)) { + return; + } + + DEBUG_PRINTF("could not parse packet: %x\r\n", processBufferPtr->packet.header.packetType); + globalResult = RX_FRAME_DROPPED; +} + + +static void srxl2DataReceive(uint16_t character, void *data) +{ + UNUSED(data); + + lastReceiveTimestamp = microsISR(); + + //If the buffer len is not reset for whatever reason, disable reception + if (readBufferPtr->len > 0 || readBufferIdx >= SRXL2_MAX_PACKET_LENGTH) { + readBufferIdx = 0; + globalResult = RX_FRAME_DROPPED; + } + else { + readBufferPtr->packet.raw[readBufferIdx] = character; + readBufferIdx++; + } +} + +static void srxl2Idle(void) +{ + if(transmittingTelemetry) { // Transmitting telemetry triggers idle interrupt as well. We dont want to change buffers then + transmittingTelemetry = false; + } + else if(readBufferIdx == 0) { // Packet was invalid + readBufferPtr->len = 0; + } + else { + lastIdleTimestamp = microsISR(); + //Swap read and process buffer pointers + if(processBufferPtr == &readBuffer[0]) { + processBufferPtr = &readBuffer[1]; + readBufferPtr = &readBuffer[0]; + } else { + processBufferPtr = &readBuffer[0]; + readBufferPtr = &readBuffer[1]; + } + processBufferPtr->len = readBufferIdx; + } + + readBufferIdx = 0; +} + +static uint8_t srxl2FrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) +{ + UNUSED(rxRuntimeConfig); + + if(serialIsIdle(serialPort)) + { + srxl2Idle(); + } + + globalResult = RX_FRAME_PENDING; + + // len should only be set after an idle interrupt (packet reception complete) + if (processBufferPtr != NULL && processBufferPtr->len) { + srxl2Process(rxRuntimeConfig); + processBufferPtr->len = 0; + } + + uint8_t result = globalResult; + + const uint32_t now = micros(); + + switch (state) { + case Disabled: break; + + case ListenForActivity: { + // activity detected + if (lastValidPacketTimestamp != 0) { + // as ListenForActivity is done at default baud-rate, we don't need to change anything + // @todo if there were non-handshake packets - go to running, + // if there were - go to either Send Handshake or Listen For Handshake + state = Running; + } else if (cmpTimeUs(lastIdleTimestamp, lastReceiveTimestamp) > 0) { + if (baudRate != 0) { + uint32_t currentBaud = serialGetBaudRate(serialPort); + + if(currentBaud == SRXL2_PORT_BAUDRATE_DEFAULT) + serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_HIGH); + else + serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT); + } + } else if (cmpTimeUs(now, timeoutTimestamp) >= 0) { + // @todo if there was activity - detect baudrate and ListenForHandshake + + if (unitId == 0) { + state = SendHandshake; + timeoutTimestamp = now + SRXL2_SEND_HANDSHAKE_TIMEOUT_US; + fullTimeoutTimestamp = now + SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US; + } else { + state = ListenForHandshake; + timeoutTimestamp = now + SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US; + } + } + } break; + + case SendHandshake: { + if (cmpTimeUs(now, timeoutTimestamp) >= 0) { + // @todo set another timeout for 50ms tries + // fill write buffer with handshake frame + result |= RX_FRAME_PROCESSING_REQUIRED; + } + + if (cmpTimeUs(now, fullTimeoutTimestamp) >= 0) { + serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT); + DEBUG_PRINTF("case SendHandshake: switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_DEFAULT); + timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US; + result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE; + + state = ListenForActivity; + lastReceiveTimestamp = 0; + } + } break; + + case ListenForHandshake: { + if (cmpTimeUs(now, timeoutTimestamp) >= 0) { + serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT); + DEBUG_PRINTF("case ListenForHandshake: switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_DEFAULT); + timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US; + result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE; + + state = ListenForActivity; + lastReceiveTimestamp = 0; + } + } break; + + case Running: { + // frame timed out, reset state + if (cmpTimeUs(now, lastValidPacketTimestamp) >= SRXL2_FRAME_TIMEOUT_US) { + serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT); + DEBUG_PRINTF("case Running: switching to %d baud: %d %d\r\n", SRXL2_PORT_BAUDRATE_DEFAULT, now, lastValidPacketTimestamp); + timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US; + result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE; + + state = ListenForActivity; + lastReceiveTimestamp = 0; + lastValidPacketTimestamp = 0; + } + } break; + }; + + if (writeBufferIdx) { + result |= RX_FRAME_PROCESSING_REQUIRED; + } + + return result; +} + +static bool srxl2ProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) +{ + UNUSED(rxRuntimeConfig); + + if (writeBufferIdx == 0) { + return true; + } + + const uint32_t now = micros(); + + if (cmpTimeUs(lastIdleTimestamp, lastReceiveTimestamp) > 0) { + // time sufficient for at least 2 characters has passed + if (cmpTimeUs(now, lastReceiveTimestamp) > SRXL2_REPLY_QUIESCENCE) { + transmittingTelemetry = true; + serialWriteBuf(serialPort, writeBuffer, writeBufferIdx); + writeBufferIdx = 0; + } else { + DEBUG_PRINTF("not enough time to send 2 characters passed yet, %d us since last receive, %d required\r\n", now - lastReceiveTimestamp, SRXL2_REPLY_QUIESCENCE); + } + } else { + DEBUG_PRINTF("still receiving a frame, %d %d\r\n", lastIdleTimestamp, lastReceiveTimestamp); + } + + return true; +} + +static uint16_t srxl2ReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channelIdx) +{ + if (channelIdx >= rxRuntimeConfig->channelCount) { + return 0; + } + + return SPEKTRUM_PULSE_OFFSET + ((rxRuntimeConfig->channelData[channelIdx] >> SRXL2_CHANNEL_SHIFT) >> 1); +} + +void srxl2RxWriteData(const void *data, int len) +{ + const uint16_t crc = crc16_ccitt_update(0, (uint8_t*)data, len - 2); + ((uint8_t*)data)[len-2] = ((uint8_t *) &crc)[1] & 0xFF; + ((uint8_t*)data)[len-1] = ((uint8_t *) &crc)[0] & 0xFF; + + len = MIN(len, (int)sizeof(writeBuffer)); + memcpy(writeBuffer, data, len); + writeBufferIdx = len; +} + +bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +{ + static uint16_t channelData[SRXL2_MAX_CHANNELS]; + for (size_t i = 0; i < SRXL2_MAX_CHANNELS; ++i) { + channelData[i] = SRXL2_CHANNEL_CENTER; + } + + unitId = rxConfig->srxl2_unit_id; + baudRate = rxConfig->srxl2_baud_fast; + + rxRuntimeConfig->channelData = channelData; + rxRuntimeConfig->channelCount = SRXL2_MAX_CHANNELS; + rxRuntimeConfig->rxRefreshRate = SRXL2_FRAME_PERIOD_US; + + rxRuntimeConfig->rcReadRawFn = srxl2ReadRawRC; + rxRuntimeConfig->rcFrameStatusFn = srxl2FrameStatus; + rxRuntimeConfig->rcProcessFrameFn = srxl2ProcessFrame; + + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + if (!portConfig) { + return false; + } + + portOptions_t options = SRXL2_PORT_OPTIONS; + if (rxConfig->serialrx_inverted) { + options |= SERIAL_INVERTED; + } + if (rxConfig->halfDuplex) { + options |= SERIAL_BIDIR; + } + + serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, srxl2DataReceive, + NULL, SRXL2_PORT_BAUDRATE_DEFAULT, SRXL2_PORT_MODE, options); + + if (!serialPort) { + return false; + } + + state = ListenForActivity; + timeoutTimestamp = micros() + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US; + + //Looks like this needs to be set in cli config + //if (rssiSource == RSSI_SOURCE_NONE) { + // rssiSource = RSSI_SOURCE_RX_PROTOCOL; + //} + + return (bool)serialPort; +} + +bool srxl2RxIsActive(void) +{ + return serialPort; +} + +bool srxl2TelemetryRequested(void) +{ + return telemetryRequested; +} + +void srxl2InitializeFrame(sbuf_t *dst) +{ + dst->ptr = telemetryFrame; + dst->end = ARRAYEND(telemetryFrame); + + sbufWriteU8(dst, SRXL2_ID); + sbufWriteU8(dst, TelemetrySensorData); + sbufWriteU8(dst, ARRAYLEN(telemetryFrame)); + sbufWriteU8(dst, busMasterDeviceId); +} + +void srxl2FinalizeFrame(sbuf_t *dst) +{ + sbufSwitchToReader(dst, telemetryFrame); + // Include 2 additional bytes of length since we're letting the srxl2RxWriteData function add the CRC in + srxl2RxWriteData(sbufPtr(dst), sbufBytesRemaining(dst) + 2); + telemetryRequested = false; +} + +void srxl2Bind(void) +{ + const size_t length = sizeof(Srxl2BindInfoFrame); + + Srxl2BindInfoFrame bind = { + .header = { + .id = SRXL2_ID, + .packetType = BindInfo, + .length = length + }, + .payload = { + .request = EnterBindMode, + .deviceId = busMasterDeviceId, + .bindType = DMSX_11ms, + .options = SRXL_BIND_OPT_TELEM_TX_ENABLE | SRXL_BIND_OPT_BIND_TX_ENABLE, + } + }; + + srxl2RxWriteData(&bind, length); +} + +#endif diff --git a/src/main/rx/srxl2.h b/src/main/rx/srxl2.h new file mode 100644 index 0000000000..5c3bb14276 --- /dev/null +++ b/src/main/rx/srxl2.h @@ -0,0 +1,15 @@ +#pragma once +#include +#include + +#include "rx/rx.h" + +struct sbuf_s; + +bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +bool srxl2RxIsActive(void); +void srxl2RxWriteData(const void *data, int len); +bool srxl2TelemetryRequested(void); +void srxl2InitializeFrame(struct sbuf_s *dst); +void srxl2FinalizeFrame(struct sbuf_s *dst); +void srxl2Bind(void); diff --git a/src/main/rx/srxl2_types.h b/src/main/rx/srxl2_types.h new file mode 100644 index 0000000000..d5edce381b --- /dev/null +++ b/src/main/rx/srxl2_types.h @@ -0,0 +1,138 @@ +#pragma once + +#define PACKED __attribute__((packed)) + +typedef enum { + Disabled, + ListenForActivity, + SendHandshake, + ListenForHandshake, + Running +} Srxl2State; + +typedef enum { + Handshake = 0x21, + BindInfo = 0x41, + ParameterConfiguration = 0x50, + SignalQuality = 0x55, + TelemetrySensorData = 0x80, + ControlData = 0xCD, +} Srxl2PacketType; + +typedef struct { + uint8_t id; + uint8_t packetType; + uint8_t length; +} PACKED Srxl2Header; + +typedef struct { + uint8_t sourceDeviceId; + uint8_t destinationDeviceId; + uint8_t priority; + uint8_t baudSupported; + uint8_t info; + uint32_t uniqueId; +} PACKED Srxl2HandshakeSubHeader; + +typedef struct { + uint8_t command; + uint8_t replyId; +} PACKED Srxl2ControlDataSubHeader; + +typedef enum { + ChannelData = 0x00, + FailsafeChannelData = 0x01, + VTXData = 0x02, +} Srxl2ControlDataCommand; + +typedef struct { + int8_t rssi; + uint16_t frameLosses; + union { + //struct { + // uint8_t channels_0_7; + // uint8_t channels_8_15; + // uint8_t channels_16_23; + // uint8_t channels_24_31; + //} u8; + uint8_t u8[4]; + uint32_t u32; + } channelMask; +} PACKED Srxl2ChannelDataHeader; + +typedef enum { + NoDevice = 0, + RemoteReceiver = 1, + Receiver = 2, + FlightController = 3, + ESC = 4, + Reserved = 5, + SRXLServo = 6, + SRXLServo_2 = 7, + VTX = 8, +} Srxl2DeviceType; + +typedef enum { + FlightControllerDefault = 0x30, + FlightControllerMax = 0x3F, + Broadcast = 0xFF, +} Srxl2DeviceId; + +typedef struct { + Srxl2Header header; + Srxl2HandshakeSubHeader payload; + uint8_t crcHigh; + uint8_t crcLow; +} PACKED Srxl2HandshakeFrame; + +typedef enum { + EnterBindMode = 0xEB, + RequestBindStatus = 0xB5, + BoundDataReport = 0xDB, + SetBindInfo = 0x5B, +} Srxl2BindRequest; + +typedef enum { + NotBound = 0x0, + DSM2_1024_22ms = 0x01, + DSM2_1024_MC24 = 0x02, + DMS2_2048_11ms = 0x12, + DMSX_22ms = 0xA2, + DMSX_11ms = 0xB2, + Surface_DSM2_16_5ms = 0x63, + DSMR_11ms_22ms = 0xE2, + DSMR_5_5ms = 0xE4, +} Srxl2BindType; + +// Bit masks for Options byte +#define SRXL_BIND_OPT_NONE (0x00) +#define SRXL_BIND_OPT_TELEM_TX_ENABLE (0x01) // Set if this device should be enabled as the current telemetry device to tx over RF +#define SRXL_BIND_OPT_BIND_TX_ENABLE (0x02) // Set if this device should reply to a bind request with a Discover packet over RF + +typedef struct { + uint8_t request; + uint8_t deviceId; + uint8_t bindType; + uint8_t options; + uint64_t guid; + uint32_t uid; +} PACKED Srxl2BindInfoPayload; + +typedef struct { + Srxl2Header header; + Srxl2BindInfoPayload payload; + uint8_t crcHigh; + uint8_t crcLow; +} PACKED Srxl2BindInfoFrame; + +// VTX Data +typedef struct { + uint8_t band; // VTX Band (0 = Fatshark, 1 = Raceband, 2 = E, 3 = B, 4 = A) + uint8_t channel; // VTX Channel (0-7) + uint8_t pit; // Pit/Race mode (0 = Race, 1 = Pit). Race = (normal operating) mode. Pit = (reduced power) mode. + uint8_t power; // VTX Power (0 = Off, 1 = 1mw to 14mW, 2 = 15mW to 25mW, 3 = 26mW to 99mW, 4 = 100mW to 299mW, 5 = 300mW to 600mW, 6 = 601mW+, 7 = manual control) + uint16_t powerDec; // VTX Power as a decimal 1mw/unit + uint8_t region; // Region (0 = USA, 1 = EU) +} PACKED Srxl2VtxData; + +#undef PACKED diff --git a/src/main/target/common.h b/src/main/target/common.h index b76bdf4d3b..1c70a04218 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -124,6 +124,14 @@ #define USE_I2C_IO_EXPANDER +#define USE_SERIALRX_SRXL2 // Spektrum SRXL2 protocol +#define USE_TELEMETRY_SRXL +#define USE_SPEKTRUM_CMS_TELEMETRY +//#define USE_SPEKTRUM_VTX_CONTROL //Some functions from betaflight still not implemented +#define USE_SPEKTRUM_VTX_TELEMETRY + +#define USE_VTX_COMMON + #else // FLASH_SIZE < 256 #define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR #endif diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c new file mode 100644 index 0000000000..8e01e47ef8 --- /dev/null +++ b/src/main/telemetry/srxl.c @@ -0,0 +1,812 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_TELEMETRY_SRXL) + +#include "build/version.h" + +#include "cms/cms.h" +#include "io/displayport_srxl.h" + +#include "common/crc.h" +#include "common/streambuf.h" +#include "common/utils.h" + +#include "config/feature.h" + +#include "io/gps.h" +#include "io/serial.h" + +//#include "config/config.h" +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" +#include "flight/mixer.h" + +#include "io/gps.h" + +//#include "pg/motor.h" + +#include "rx/rx.h" +#include "rx/spektrum.h" +#include "rx/srxl2.h" + +#include "sensors/battery.h" +//#include "sensors/adcinternal.h" +#include "sensors/esc_sensor.h" + +#include "telemetry/telemetry.h" +#include "telemetry/srxl.h" + +#include "drivers/vtx_common.h" +//#include "drivers/dshot.h" + +#include "io/vtx_tramp.h" +#include "io/vtx_smartaudio.h" + +#define SRXL_ADDRESS_FIRST 0xA5 +#define SRXL_ADDRESS_SECOND 0x80 +#define SRXL_PACKET_LENGTH 0x15 + +#define SRXL_FRAMETYPE_TELE_QOS 0x7F +#define SRXL_FRAMETYPE_TELE_RPM 0x7E +#define SRXL_FRAMETYPE_POWERBOX 0x0A +#define SRXL_FRAMETYPE_TELE_FP_MAH 0x34 +#define TELE_DEVICE_VTX 0x0D // Video Transmitter Status +#define SRXL_FRAMETYPE_SID 0x00 +#define SRXL_FRAMETYPE_GPS_LOC 0x16 // GPS Location Data (Eagle Tree) +#define SRXL_FRAMETYPE_GPS_STAT 0x17 + +static bool srxlTelemetryEnabled; +static bool srxl2 = false; +static uint8_t srxlFrame[SRXL_FRAME_SIZE_MAX]; + +static void srxlInitializeFrame(sbuf_t *dst) +{ + if (srxl2) { +#if defined(USE_SERIALRX_SRXL2) + srxl2InitializeFrame(dst); +#endif + } else { + dst->ptr = srxlFrame; + dst->end = ARRAYEND(srxlFrame); + + sbufWriteU8(dst, SRXL_ADDRESS_FIRST); + sbufWriteU8(dst, SRXL_ADDRESS_SECOND); + sbufWriteU8(dst, SRXL_PACKET_LENGTH); + } +} + +static void srxlFinalize(sbuf_t *dst) +{ + if (srxl2) { +#if defined(USE_SERIALRX_SRXL2) + srxl2FinalizeFrame(dst); +#endif + } else { + crc16_ccitt_sbuf_append(dst, &srxlFrame[3]); // start at byte 3, since CRC does not include device address and packet length + sbufSwitchToReader(dst, srxlFrame); + // write the telemetry frame to the receiver. + srxlRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst)); + } +} + +/* +SRXL frame has the structure: +<0xA5><0x80><16-byte telemetry packet><2 Byte CRC of payload> +The shall be 0x15 (length of the 16-byte telemetry packet + overhead). +*/ + +/* +typedef struct +{ + UINT8 identifier; // Source device = 0x7F + UINT8 sID; // Secondary ID + UINT16 A; + UINT16 B; + UINT16 L; + UINT16 R; + UINT16 F; + UINT16 H; + UINT16 rxVoltage; // Volts, 0.01V increments +} STRU_TELE_QOS; +*/ + +#define STRU_TELE_QOS_EMPTY_FIELDS_COUNT 14 +#define STRU_TELE_QOS_EMPTY_FIELDS_VALUE 0xff + +bool srxlFrameQos(sbuf_t *dst, timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_QOS); + sbufWriteU8(dst, SRXL_FRAMETYPE_SID); + + sbufFill(dst, STRU_TELE_QOS_EMPTY_FIELDS_VALUE, STRU_TELE_QOS_EMPTY_FIELDS_COUNT); // Clear remainder + + // Mandatory frame, send it unconditionally. + return true; +} + + +/* +typedef struct +{ + UINT8 identifier; // Source device = 0x7E + UINT8 sID; // Secondary ID + UINT16 microseconds; // microseconds between pulse leading edges + UINT16 volts; // 0.01V increments + INT16 temperature; // degrees F + INT8 dBm_A, // Average signal for A antenna in dBm + INT8 dBm_B; // Average signal for B antenna in dBm. + // If only 1 antenna, set B = A +} STRU_TELE_RPM; +*/ + +#define STRU_TELE_RPM_EMPTY_FIELDS_COUNT 8 +#define STRU_TELE_RPM_EMPTY_FIELDS_VALUE 0xff + +#define SPEKTRUM_RPM_UNUSED 0xffff +#define SPEKTRUM_TEMP_UNUSED 0x7fff +#define MICROSEC_PER_MINUTE 60000000 + +//Original range of 1 - 65534 uSec gives an RPM range of 915 - 60000000rpm, 60MegaRPM +#define SPEKTRUM_MIN_RPM 999 // Min RPM to show the user, indicating RPM is really below 999 +#define SPEKTRUM_MAX_RPM 60000000 + +uint16_t getMotorAveragePeriod(void) +{ + +#if defined( USE_ESC_SENSOR_TELEMETRY) || defined( USE_DSHOT_TELEMETRY) + uint32_t rpm = 0; + uint16_t period_us = SPEKTRUM_RPM_UNUSED; + +#if defined( USE_ESC_SENSOR_TELEMETRY) + escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); + if (escData != NULL) { + rpm = escData->rpm; + } +#endif + +#if defined(USE_DSHOT_TELEMETRY) + if (useDshotTelemetry) { + uint16_t motors = getMotorCount(); + + if (motors > 0) { + for (int motor = 0; motor < motors; motor++) { + rpm += getDshotTelemetry(motor); + } + rpm = 100.0f / (motorConfig()->motorPoleCount / 2.0f) * rpm; // convert erpm freq to RPM. + rpm /= motors; // Average combined rpm + } + } +#endif + + if (rpm > SPEKTRUM_MIN_RPM && rpm < SPEKTRUM_MAX_RPM) { + period_us = MICROSEC_PER_MINUTE / rpm; // revs/minute -> microSeconds + } else { + period_us = MICROSEC_PER_MINUTE / SPEKTRUM_MIN_RPM; + } + + return period_us; +#else + return SPEKTRUM_RPM_UNUSED; +#endif +} + +bool srxlFrameRpm(sbuf_t *dst, timeUs_t currentTimeUs) +{ + int16_t coreTemp = SPEKTRUM_TEMP_UNUSED; +#if defined(USE_ADC_INTERNAL) + coreTemp = getCoreTemperatureCelsius(); + coreTemp = coreTemp * 9 / 5 + 32; // C -> F +#endif + + UNUSED(currentTimeUs); + + sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_RPM); + sbufWriteU8(dst, SRXL_FRAMETYPE_SID); + sbufWriteU16BigEndian(dst, getMotorAveragePeriod()); // pulse leading edges + if (telemetryConfig()->report_cell_voltage) { + sbufWriteU16BigEndian(dst, getBatteryAverageCellVoltage()); // Cell voltage is in units of 0.01V + } else { + sbufWriteU16BigEndian(dst, getBatteryVoltage()); // vbat is in units of 0.01V + } + sbufWriteU16BigEndian(dst, coreTemp); // temperature + sbufFill(dst, STRU_TELE_RPM_EMPTY_FIELDS_VALUE, STRU_TELE_RPM_EMPTY_FIELDS_COUNT); + + // Mandatory frame, send it unconditionally. + return true; +} + +#if defined(USE_GPS) + +// From Frsky implementation +static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) +{ + int32_t absgps, deg, min; + absgps = ABS(mwiigps); + deg = absgps / GPS_DEGREES_DIVIDER; + absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7 + min = absgps / GPS_DEGREES_DIVIDER; // minutes left + result->dddmm = deg * 100 + min; + result->mmmm = (absgps - min * GPS_DEGREES_DIVIDER) / 1000; +} + +// BCD conversion +static uint32_t dec2bcd(uint16_t dec) +{ + uint32_t result = 0; + uint8_t counter = 0; + + while (dec) { + result |= (dec % 10) << counter * 4; + counter++; + dec /= 10; + } + return result; +} + +/* +typedef struct +{ + UINT8 identifier; // Source device = 0x16 + UINT8 sID; // Secondary ID + UINT16 altitudeLow; // BCD, meters, format 3.1 (Low order of altitude) + UINT32 latitude; // BCD, format 4.4, Degrees * 100 + minutes, less than 100 degrees + UINT32 longitude; // BCD, format 4.4 , Degrees * 100 + minutes, flag indicates > 99 degrees + UINT16 course; // BCD, 3.1 + UINT8 HDOP; // BCD, format 1.1 + UINT8 GPSflags; // see definitions below +} STRU_TELE_GPS_LOC; +*/ + +// GPS flags definitions +#define GPS_FLAGS_IS_NORTH_BIT 0x01 +#define GPS_FLAGS_IS_EAST_BIT 0x02 +#define GPS_FLAGS_LONGITUDE_GREATER_99_BIT 0x04 +#define GPS_FLAGS_GPS_FIX_VALID_BIT 0x08 +#define GPS_FLAGS_GPS_DATA_RECEIVED_BIT 0x10 +#define GPS_FLAGS_3D_FIX_BIT 0x20 +#define GPS_FLAGS_NEGATIVE_ALT_BIT 0x80 + +bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + gpsCoordinateDDDMMmmmm_t coordinate; + uint32_t latitudeBcd, longitudeBcd, altitudeLo; + uint16_t altitudeLoBcd, groundCourseBcd, hdop; + uint8_t hdopBcd, gpsFlags; + + if (!feature(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) { + return false; + } + + // lattitude + GPStoDDDMM_MMMM(gpsSol.llh.lat, &coordinate); + latitudeBcd = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm); + + // longitude + GPStoDDDMM_MMMM(gpsSol.llh.lon, &coordinate); + longitudeBcd = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm); + + // altitude (low order) + altitudeLo = ABS(gpsSol.llh.alt) / 10; + altitudeLoBcd = dec2bcd(altitudeLo % 100000); + + // Ground course + groundCourseBcd = dec2bcd(gpsSol.groundCourse); + + // HDOP + hdop = gpsSol.hdop / 10; + hdop = (hdop > 99) ? 99 : hdop; + hdopBcd = dec2bcd(hdop); + + // flags + gpsFlags = GPS_FLAGS_GPS_DATA_RECEIVED_BIT | GPS_FLAGS_GPS_FIX_VALID_BIT | GPS_FLAGS_3D_FIX_BIT; + gpsFlags |= (gpsSol.llh.lat > 0) ? GPS_FLAGS_IS_NORTH_BIT : 0; + gpsFlags |= (gpsSol.llh.lon > 0) ? GPS_FLAGS_IS_EAST_BIT : 0; + gpsFlags |= (gpsSol.llh.alt < 0) ? GPS_FLAGS_NEGATIVE_ALT_BIT : 0; + gpsFlags |= (gpsSol.llh.lon / GPS_DEGREES_DIVIDER > 99) ? GPS_FLAGS_LONGITUDE_GREATER_99_BIT : 0; + + // SRXL frame + sbufWriteU8(dst, SRXL_FRAMETYPE_GPS_LOC); + sbufWriteU8(dst, SRXL_FRAMETYPE_SID); + sbufWriteU16(dst, altitudeLoBcd); + sbufWriteU32(dst, latitudeBcd); + sbufWriteU32(dst, longitudeBcd); + sbufWriteU16(dst, groundCourseBcd); + sbufWriteU8(dst, hdopBcd); + sbufWriteU8(dst, gpsFlags); + + return true; +} + +/* +typedef struct +{ + UINT8 identifier; // Source device = 0x17 + UINT8 sID; // Secondary ID + UINT16 speed; // BCD, knots, format 3.1 + UINT32 UTC; // BCD, format HH:MM:SS.S, format 6.1 + UINT8 numSats; // BCD, 0-99 + UINT8 altitudeHigh; // BCD, meters, format 2.0 (High bits alt) +} STRU_TELE_GPS_STAT; +*/ + +#define STRU_TELE_GPS_STAT_EMPTY_FIELDS_VALUE 0xff +#define STRU_TELE_GPS_STAT_EMPTY_FIELDS_COUNT 6 +#define SPEKTRUM_TIME_UNKNOWN 0xFFFFFFFF + +bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + uint32_t timeBcd; + uint16_t speedKnotsBcd, speedTmp; + uint8_t numSatBcd, altitudeHighBcd; + bool timeProvided = false; + + if (!feature(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) { + return false; + } + + // Number of sats and altitude (high bits) + numSatBcd = (gpsSol.numSat > 99) ? dec2bcd(99) : dec2bcd(gpsSol.numSat); + altitudeHighBcd = dec2bcd(gpsSol.llh.alt / 100000); + + // Speed (knots) + speedTmp = gpsSol.groundSpeed * 1944 / 1000; + speedKnotsBcd = (speedTmp > 9999) ? dec2bcd(9999) : dec2bcd(speedTmp); + +#ifdef USE_RTC_TIME + dateTime_t dt; + // RTC + if (rtcHasTime()) { + rtcGetDateTime(&dt); + timeBcd = dec2bcd(dt.hours); + timeBcd = timeBcd << 8; + timeBcd = timeBcd | dec2bcd(dt.minutes); + timeBcd = timeBcd << 8; + timeBcd = timeBcd | dec2bcd(dt.seconds); + timeBcd = timeBcd << 4; + timeBcd = timeBcd | dec2bcd(dt.millis / 100); + timeProvided = true; + } +#endif + timeBcd = (timeProvided) ? timeBcd : SPEKTRUM_TIME_UNKNOWN; + + // SRXL frame + sbufWriteU8(dst, SRXL_FRAMETYPE_GPS_STAT); + sbufWriteU8(dst, SRXL_FRAMETYPE_SID); + sbufWriteU16(dst, speedKnotsBcd); + sbufWriteU32(dst, timeBcd); + sbufWriteU8(dst, numSatBcd); + sbufWriteU8(dst, altitudeHighBcd); + sbufFill(dst, STRU_TELE_GPS_STAT_EMPTY_FIELDS_VALUE, STRU_TELE_GPS_STAT_EMPTY_FIELDS_COUNT); + + return true; +} + +#endif + +/* +typedef struct +{ + UINT8 identifier; // Source device = 0x34 + UINT8 sID; // Secondary ID + INT16 current_A; // Instantaneous current, 0.1A (0-3276.8A) + INT16 chargeUsed_A; // Integrated mAh used, 1mAh (0-32.766Ah) + UINT16 temp_A; // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated) + INT16 current_B; // Instantaneous current, 0.1A (0-3276.8A) + INT16 chargeUsed_B; // Integrated mAh used, 1mAh (0-32.766Ah) + UINT16 temp_B; // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated) + UINT16 spare; // Not used +} STRU_TELE_FP_MAH; +*/ +#define STRU_TELE_FP_EMPTY_FIELDS_COUNT 2 +#define STRU_TELE_FP_EMPTY_FIELDS_VALUE 0xff + +#define SPEKTRUM_AMPS_UNUSED 0x7fff +#define SPEKTRUM_AMPH_UNUSED 0x7fff + +#define FP_MAH_KEEPALIVE_TIME_OUT 2000000 // 2s + +bool srxlFrameFlightPackCurrent(sbuf_t *dst, timeUs_t currentTimeUs) +{ + uint16_t amps = getAmperage() / 10; + uint16_t mah = getMAhDrawn(); + static uint16_t sentAmps; + static uint16_t sentMah; + static timeUs_t lastTimeSentFPmAh = 0; + + timeUs_t keepAlive = currentTimeUs - lastTimeSentFPmAh; + + if ( amps != sentAmps || + mah != sentMah || + keepAlive > FP_MAH_KEEPALIVE_TIME_OUT ) { + + sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_FP_MAH); + sbufWriteU8(dst, SRXL_FRAMETYPE_SID); + sbufWriteU16(dst, amps); + sbufWriteU16(dst, mah); + sbufWriteU16(dst, SPEKTRUM_TEMP_UNUSED); // temp A + sbufWriteU16(dst, SPEKTRUM_AMPS_UNUSED); // Amps B + sbufWriteU16(dst, SPEKTRUM_AMPH_UNUSED); // mAH B + sbufWriteU16(dst, SPEKTRUM_TEMP_UNUSED); // temp B + + sbufFill(dst, STRU_TELE_FP_EMPTY_FIELDS_VALUE, STRU_TELE_FP_EMPTY_FIELDS_COUNT); + + sentAmps = amps; + sentMah = mah; + lastTimeSentFPmAh = currentTimeUs; + return true; + } + return false; +} + +#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) + +// Betaflight CMS using Spektrum Tx telemetry TEXT_GEN sensor as display. + +#define SPEKTRUM_SRXL_DEVICE_TEXTGEN (0x0C) // Text Generator +#define SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS (9) // Text Generator ROWS +#define SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS (13) // Text Generator COLS + +/* +typedef struct +{ + UINT8 identifier; + UINT8 sID; // Secondary ID + UINT8 lineNumber; // Line number to display (0 = title, 1-8 for general, 254 = Refresh backlight, 255 = Erase all text on screen) + char text[13]; // 0-terminated text when < 13 chars +} STRU_SPEKTRUM_SRXL_TEXTGEN; +*/ + +#if ( SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS > SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS ) +static char srxlTextBuff[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS][SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS]; +static bool lineSent[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS]; +#else +static char srxlTextBuff[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS][SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS]; +static bool lineSent[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS]; +#endif + +//************************************************************************** +// API Running in external client task context. E.g. in the CMS task +int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c) +{ + if (row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS && col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS) { + // Only update and force a tm transmision if something has actually changed. + if (srxlTextBuff[row][col] != c) { + srxlTextBuff[row][col] = c; + lineSent[row] = false; + } + } + return 0; +} +//************************************************************************** + +bool srxlFrameText(sbuf_t *dst, timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + static uint8_t lineNo = 0; + int lineCount = 0; + + // Skip already sent lines... + while (lineSent[lineNo] && + lineCount < SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS) { + lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS; + lineCount++; + } + + sbufWriteU8(dst, SPEKTRUM_SRXL_DEVICE_TEXTGEN); + sbufWriteU8(dst, SRXL_FRAMETYPE_SID); + sbufWriteU8(dst, lineNo); + sbufWriteData(dst, srxlTextBuff[lineNo], SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS); + + lineSent[lineNo] = true; + lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS; + + // Always send something, Always one user frame after the two mandatory frames + // I.e. All of the three frame prep routines QOS, RPM, TEXT should always return true + // too keep the "Waltz" sequence intact. + return true; +} +#endif + +#if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON) + +static uint8_t vtxDeviceType; + +static void collectVtxTmData(spektrumVtx_t * vtx) +{ + const vtxDevice_t *vtxDevice = vtxCommonDevice(); + vtxDeviceType = vtxCommonGetDeviceType(vtxDevice); + + // Collect all data from VTX, if VTX is ready + unsigned vtxStatus; + if (vtxDevice == NULL || !(vtxCommonGetBandAndChannel(vtxDevice, &vtx->band, &vtx->channel) && + vtxCommonGetStatus(vtxDevice, &vtxStatus) && + vtxCommonGetPowerIndex(vtxDevice, &vtx->power)) ) + { + vtx->band = 0; + vtx->channel = 0; + vtx->power = 0; + vtx->pitMode = 0; + } else { + vtx->pitMode = (vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0; + } + + vtx->powerValue = 0; +#ifdef USE_SPEKTRUM_REGION_CODES + vtx->region = SpektrumRegion; +#else + vtx->region = SPEKTRUM_VTX_REGION_NONE; +#endif +} + +// Reverse lookup, device power index to Spektrum power range index. +static void convertVtxPower(spektrumVtx_t * vtx) + { + uint8_t const * powerIndexTable = NULL; + + vtxCommonLookupPowerValue(vtxCommonDevice(), vtx->power, &vtx->powerValue); + switch (vtxDeviceType) { + +#if defined(USE_VTX_TRAMP) + case VTXDEV_TRAMP: + powerIndexTable = vtxTrampPi; + break; +#endif +#if defined(USE_VTX_SMARTAUDIO) + case VTXDEV_SMARTAUDIO: + powerIndexTable = vtxSaPi; + break; +#endif +#if defined(USE_VTX_RTC6705) + case VTXDEV_RTC6705: + powerIndexTable = vtxRTC6705Pi; + break; +#endif + + case VTXDEV_UNKNOWN: + case VTXDEV_UNSUPPORTED: + default: + break; + + } + + if (powerIndexTable != NULL) { + for (int i = 0; i < SPEKTRUM_VTX_POWER_COUNT; i++) + if (powerIndexTable[i] >= vtx->power) { + vtx->power = i; // Translate device power index to Spektrum power index. + break; + } + } + } + +static void convertVtxTmData(spektrumVtx_t * vtx) +{ + // Convert from internal band indexes to Spektrum indexes + for (int i = 0; i < SPEKTRUM_VTX_BAND_COUNT; i++) { + if (spek2commonBand[i] == vtx->band) { + vtx->band = i; + break; + } + } + + // De-bump channel no 1 based interally, 0-based in Spektrum. + vtx->channel--; + + // Convert Power index to Spektrum ranges, different per brand. + convertVtxPower(vtx); +} + +/* +typedef struct +{ + UINT8 identifier; + UINT8 sID; // Secondary ID + UINT8 band; // VTX Band (0 = Fatshark, 1 = Raceband, 2 = E, 3 = B, 4 = A, 5-7 = Reserved) + UINT8 channel; // VTX Channel (0-7) + UINT8 pit; // Pit/Race mode (0 = Race, 1 = Pit). Race = (normal operating) mode. Pit = (reduced power) mode. When PIT is set, it overrides all other power settings + UINT8 power; // VTX Power (0 = Off, 1 = 1mw to 14mW, 2 = 15mW to 25mW, 3 = 26mW to 99mW, 4 = 100mW to 299mW, 5 = 300mW to 600mW, 6 = 601mW+, 7 = manual control) + UINT16 powerDec; // VTX Power as a decimal 1mw/unit + UINT8 region; // Region (0 = USA, 1 = EU, 0xFF = N/A) + UINT8 rfu[7]; // reserved +} STRU_TELE_VTX; +*/ + +#define STRU_TELE_VTX_EMPTY_COUNT 7 +#define STRU_TELE_VTX_EMPTY_VALUE 0xff + +#define VTX_KEEPALIVE_TIME_OUT 2000000 // uS + +static bool srxlFrameVTX(sbuf_t *dst, timeUs_t currentTimeUs) +{ + static timeUs_t lastTimeSentVtx = 0; + static spektrumVtx_t vtxSent; + + spektrumVtx_t vtx; + collectVtxTmData(&vtx); + + if ((vtxDeviceType != VTXDEV_UNKNOWN) && vtxDeviceType != VTXDEV_UNSUPPORTED) { + convertVtxTmData(&vtx); + + if ((memcmp(&vtxSent, &vtx, sizeof(spektrumVtx_t)) != 0) || + ((currentTimeUs - lastTimeSentVtx) > VTX_KEEPALIVE_TIME_OUT) ) { + // Fill in the VTX tm structure + sbufWriteU8(dst, TELE_DEVICE_VTX); + sbufWriteU8(dst, SRXL_FRAMETYPE_SID); + sbufWriteU8(dst, vtx.band); + sbufWriteU8(dst, vtx.channel); + sbufWriteU8(dst, vtx.pitMode); + sbufWriteU8(dst, vtx.power); + sbufWriteU16(dst, vtx.powerValue); + sbufWriteU8(dst, vtx.region); + + sbufFill(dst, STRU_TELE_VTX_EMPTY_VALUE, STRU_TELE_VTX_EMPTY_COUNT); + + memcpy(&vtxSent, &vtx, sizeof(spektrumVtx_t)); + lastTimeSentVtx = currentTimeUs; + return true; + } + } + return false; +} +#endif // USE_SPEKTRUM_VTX_TELEMETRY && USE_SPEKTRUM_VTX_CONTROL && USE_VTX_COMMON + + +// Schedule array to decide how often each type of frame is sent +// The frames are scheduled in sets of 3 frames, 2 mandatory and 1 user frame. +// The user frame type is cycled for each set. +// Example. QOS, RPM,.CURRENT, QOS, RPM, TEXT. QOS, RPM, CURRENT, etc etc + +#define SRXL_SCHEDULE_MANDATORY_COUNT 2 // Mandatory QOS and RPM sensors + +#define SRXL_FP_MAH_COUNT 1 + +#if defined(USE_GPS) +#define SRXL_GPS_LOC_COUNT 1 +#define SRXL_GPS_STAT_COUNT 1 +#else +#define SRXL_GPS_LOC_COUNT 0 +#define SRXL_GPS_STAT_COUNT 0 +#endif + +#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) +#define SRXL_SCHEDULE_CMS_COUNT 1 +#else +#define SRXL_SCHEDULE_CMS_COUNT 0 +#endif + +#if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON) +#define SRXL_VTX_TM_COUNT 1 +#else +#define SRXL_VTX_TM_COUNT 0 +#endif + +#define SRXL_SCHEDULE_USER_COUNT (SRXL_FP_MAH_COUNT + SRXL_SCHEDULE_CMS_COUNT + SRXL_VTX_TM_COUNT + SRXL_GPS_LOC_COUNT + SRXL_GPS_STAT_COUNT) +#define SRXL_SCHEDULE_COUNT_MAX (SRXL_SCHEDULE_MANDATORY_COUNT + 1) +#define SRXL_TOTAL_COUNT (SRXL_SCHEDULE_MANDATORY_COUNT + SRXL_SCHEDULE_USER_COUNT) + +typedef bool (*srxlScheduleFnPtr)(sbuf_t *dst, timeUs_t currentTimeUs); + +const srxlScheduleFnPtr srxlScheduleFuncs[SRXL_TOTAL_COUNT] = { + /* must send srxlFrameQos, Rpm and then alternating items of our own */ + srxlFrameQos, + srxlFrameRpm, + srxlFrameFlightPackCurrent, +#if defined(USE_GPS) + srxlFrameGpsStat, + srxlFrameGpsLoc, +#endif +#if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON) + srxlFrameVTX, +#endif +#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) + srxlFrameText, +#endif +}; + + +static void processSrxl(timeUs_t currentTimeUs) +{ + static uint8_t srxlScheduleIndex = 0; + static uint8_t srxlScheduleUserIndex = 0; + + sbuf_t srxlPayloadBuf; + sbuf_t *dst = &srxlPayloadBuf; + srxlScheduleFnPtr srxlFnPtr; + + if (srxlScheduleIndex < SRXL_SCHEDULE_MANDATORY_COUNT) { + srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex]; + } else { + srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex + srxlScheduleUserIndex]; + srxlScheduleUserIndex = (srxlScheduleUserIndex + 1) % SRXL_SCHEDULE_USER_COUNT; + +#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) + // Boost CMS performance by sending nothing else but CMS Text frames when in a CMS menu. + // Sideeffect, all other reports are still not sent if user leaves CMS without a proper EXIT. + if (cmsInMenu && + (cmsDisplayPortGetCurrent() == &srxlDisplayPort)) { + srxlFnPtr = srxlFrameText; + } +#endif + + } + + if (srxlFnPtr) { + srxlInitializeFrame(dst); + if (srxlFnPtr(dst, currentTimeUs)) { + srxlFinalize(dst); + } + } + srxlScheduleIndex = (srxlScheduleIndex + 1) % SRXL_SCHEDULE_COUNT_MAX; +} + +void initSrxlTelemetry(void) +{ + // check if there is a serial port open for SRXL telemetry (ie opened by the SRXL RX) + // and feature is enabled, if so, set SRXL telemetry enabled + if (srxlRxIsActive()) { + srxlTelemetryEnabled = true; + srxl2 = false; +#if defined(USE_SERIALRX_SRXL2) + } else if (srxl2RxIsActive()) { + srxlTelemetryEnabled = true; + srxl2 = true; +#endif + } else { + srxlTelemetryEnabled = false; + srxl2 = false; + } + } + +bool checkSrxlTelemetryState(void) +{ + return srxlTelemetryEnabled; +} + +/* + * Called periodically by the scheduler + */ +void handleSrxlTelemetry(timeUs_t currentTimeUs) +{ + if (srxl2) { +#if defined(USE_SERIALRX_SRXL2) + if (srxl2TelemetryRequested()) { + processSrxl(currentTimeUs); + } +#endif + } else { + if (srxlTelemetryBufferEmpty()) { + processSrxl(currentTimeUs); + } + } +} +#endif diff --git a/src/main/telemetry/srxl.h b/src/main/telemetry/srxl.h new file mode 100644 index 0000000000..7719c58f2b --- /dev/null +++ b/src/main/telemetry/srxl.h @@ -0,0 +1,35 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include "common/time.h" + +void srxlCollectTelemetryNow(void); +void initSrxlTelemetry(void); +bool checkSrxlTelemetryState(void); +void handleSrxlTelemetry(timeUs_t currentTimeUs); + +#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS 9 +#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS 12 // Airware 1.20 +//#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS 13 // Airware 1.21 +#define SPEKTRUM_SRXL_TEXTGEN_CLEAR_SCREEN 255 + +int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index aad89f86db..5022a0192e 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -48,6 +48,7 @@ #include "telemetry/jetiexbus.h" #include "telemetry/ibus.h" #include "telemetry/crsf.h" +#include "telemetry/srxl.h" #include "telemetry/sim.h" @@ -124,6 +125,10 @@ void telemetryInit(void) initCrsfTelemetry(); #endif +#ifdef USE_TELEMETRY_SRXL + initSrxlTelemetry(); +#endif + telemetryCheckState(); } @@ -185,6 +190,10 @@ void telemetryCheckState(void) #if defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF) checkCrsfTelemetryState(); #endif + +#ifdef USE_TELEMETRY_SRXL + checkSrxlTelemetryState(); +#endif } void telemetryProcess(timeUs_t currentTimeUs) @@ -226,6 +235,10 @@ void telemetryProcess(timeUs_t currentTimeUs) #if defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF) handleCrsfTelemetry(currentTimeUs); #endif + +#ifdef USE_TELEMETRY_SRXL + handleSrxlTelemetry(currentTimeUs); +#endif } #endif From 28f50f4fa3461a61ce5089b1baa9cc58b338eda6 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Fri, 23 Oct 2020 12:14:52 +0100 Subject: [PATCH 061/109] Update Building in Windows 10 with Linux Subsystem.md Added some code to fix an error where CHMOD does not work with NTFS --- .../Building in Windows 10 with Linux Subsystem.md | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/docs/development/Building in Windows 10 with Linux Subsystem.md b/docs/development/Building in Windows 10 with Linux Subsystem.md index 52dd61527e..6a3a96f7ec 100644 --- a/docs/development/Building in Windows 10 with Linux Subsystem.md +++ b/docs/development/Building in Windows 10 with Linux Subsystem.md @@ -41,6 +41,18 @@ Mount MS windows C drive and clone iNav You are ready! You now have a folder called inav in the root of C drive that you can edit in windows +### If you get a cloning error + +On some installations, you may see the following error: +``` +Cloning into 'inav'... +error: chmod on /mnt/c/inav/.git/config.lock failed: Operation not permitted +fatal: could not set 'core.filemode' to 'false' +``` + +You can fix this with by remounting the drive using the following commands +1. `sudo umount /mnt/c` +2. `sudo mount -t drvfs C: /mnt/c -o metadata` ## Building (example): @@ -92,4 +104,4 @@ This error can be triggered by a Windows PATHs included in the Linux Subsystem. 1. Restart WSL and Windows preferably 1. `cd build` 1. `cmake ..` -1. `make {TARGET}` should be working again \ No newline at end of file +1. `make {TARGET}` should be working again From 59b872030621bb10c3b56784195f565fab6ad748 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Fri, 4 Sep 2020 09:54:46 +0200 Subject: [PATCH 062/109] [GPS] Add MSP_GPS sensor. Enable on F4+ boards; Add extra RTC sanity check --- src/main/fc/fc_msp.c | 18 ++++ src/main/fc/settings.yaml | 2 +- src/main/io/gps.c | 37 +++++--- src/main/io/gps.h | 2 + src/main/io/gps_msp.c | 130 ++++++++++++++++++++++++++ src/main/io/gps_private.h | 3 + src/main/msp/msp_protocol_v2_sensor.h | 5 +- src/main/target/common.h | 1 + 8 files changed, 184 insertions(+), 14 deletions(-) create mode 100644 src/main/io/gps_msp.c diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 16999bd057..0fa160cf8a 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3215,6 +3215,24 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src) mspOpflowReceiveNewData(sbufPtr(src)); break; #endif + +#if defined(USE_GPS_PROTO_MSP) + case MSP2_SENSOR_GPS: + mspGPSReceiveNewData(sbufPtr(src)); + break; +#endif + +#if defined(USE_COMPASS_MSP) + case MSP2_SENSOR_COMPASS: + mspCompassReceiveNewData(sbufPtr(src)); + break; +#endif + +#if defined(USE_BARO_MSP) + case MSP2_SENSOR_BAROMETER: + mspBaroReceiveNewData(sbufPtr(src)); + break; +#endif } return MSP_RESULT_NO_REPLY; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 34ddc5868b..edfe63576d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -44,7 +44,7 @@ tables: values: ["NONE", "ADC", "ESC"] enum: voltageSensor_e - name: gps_provider - values: ["NMEA", "UBLOX", "UNUSED", "NAZA", "UBLOX7", "MTK"] + values: ["NMEA", "UBLOX", "UNUSED", "NAZA", "UBLOX7", "MTK", "MSP"] enum: gpsProvider_e - name: gps_sbas_mode values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "NONE"] diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 107b88118b..fd0294bebc 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -60,6 +60,7 @@ #include "fc/runtime_config.h" typedef struct { + bool isDriverBased; portMode_t portMode; // Port mode RX/TX (only for serial based) bool hasCompass; // Has a compass (NAZA) void (*restart)(void); // Restart protocol driver thread @@ -77,42 +78,48 @@ baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = { /* NMEA GPS */ #ifdef USE_GPS_PROTO_NMEA - { MODE_RX, false, &gpsRestartNMEA_MTK, &gpsHandleNMEA }, + { false, MODE_RX, false, &gpsRestartNMEA_MTK, &gpsHandleNMEA }, #else - { 0, false, NULL, NULL }, + { false, 0, false, NULL, NULL }, #endif /* UBLOX binary */ #ifdef USE_GPS_PROTO_UBLOX - { MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX }, + { false, MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX }, #else - { 0, false, NULL, NULL }, + { false, 0, false, NULL, NULL }, #endif /* Stub */ - { 0, false, NULL, NULL }, + { false, 0, false, NULL, NULL }, /* NAZA GPS module */ #ifdef USE_GPS_PROTO_NAZA - { MODE_RX, true, &gpsRestartNAZA, &gpsHandleNAZA }, + { false, MODE_RX, true, &gpsRestartNAZA, &gpsHandleNAZA }, #else - { 0, false, NULL, NULL }, + { false, 0, false, NULL, NULL }, #endif /* UBLOX7PLUS binary */ #ifdef USE_GPS_PROTO_UBLOX - { MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX }, + { false, MODE_RXTX, false, &gpsRestartUBLOX, &gpsHandleUBLOX }, #else - { 0, false, NULL, NULL }, + { false, 0, false, NULL, NULL }, #endif /* MTK GPS */ #ifdef USE_GPS_PROTO_MTK - { MODE_RXTX, false, &gpsRestartNMEA_MTK, &gpsHandleMTK }, + { false, MODE_RXTX, false, &gpsRestartNMEA_MTK, &gpsHandleMTK }, #else - { 0, false, NULL, NULL }, + { false, 0, false, NULL, NULL }, #endif + /* MSP GPS */ +#ifdef USE_GPS_PROTO_MSP + { true, 0, false, &gpsRestartMSP, &gpsHandleMSP }, +#else + { false, 0, false, NULL, NULL }, +#endif }; PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); @@ -135,7 +142,7 @@ void gpsSetState(gpsState_e state) static void gpsUpdateTime(void) { - if (!rtcHasTime() && gpsSol.flags.validTime) { + if (!rtcHasTime() && gpsSol.flags.validTime && gpsSol.time.year != 0) { rtcSetDateTime(&gpsSol.time); } } @@ -220,6 +227,12 @@ void gpsInit(void) return; } + // Shortcut for driver-based GPS (MSP) + if (gpsProviders[gpsState.gpsConfig->provider].isDriverBased) { + gpsSetState(GPS_INITIALIZING); + return; + } + serialPortConfig_t * gpsPortConfig = findSerialPortConfig(FUNCTION_GPS); if (!gpsPortConfig) { featureClear(FEATURE_GPS); diff --git a/src/main/io/gps.h b/src/main/io/gps.h index d2cc1e28e7..63e02696f2 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -38,6 +38,7 @@ typedef enum { GPS_NAZA, GPS_UBLOX7PLUS, GPS_MTK, + GPS_MSP, GPS_PROVIDER_COUNT } gpsProvider_e; @@ -164,3 +165,4 @@ bool isGPSHealthy(void); bool isGPSHeadingValid(void); struct serialPort_s; void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort); +void mspGPSReceiveNewData(uint8_t * bufferPtr); diff --git a/src/main/io/gps_msp.c b/src/main/io/gps_msp.c new file mode 100644 index 0000000000..5ab4da326e --- /dev/null +++ b/src/main/io/gps_msp.c @@ -0,0 +1,130 @@ +/* + * 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 +#include +#include +#include + +#include "platform.h" +#include "build/build_config.h" + + +#if defined(USE_GPS_PROTO_MSP) + +#include "build/debug.h" + +#include "common/axis.h" +#include "common/gps_conversion.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/serial.h" +#include "drivers/time.h" + +#include "fc/config.h" +#include "fc/runtime_config.h" + +#include "io/gps.h" +#include "io/gps_private.h" +#include "io/serial.h" + +#include "scheduler/protothreads.h" + +typedef struct __attribute__((packed)) { + uint32_t msTOW; + uint8_t fixType; + uint8_t satellitesInView; + uint16_t horizontalPosAccuracy; // [cm] + uint16_t verticalPosAccuracy; // [cm] + uint16_t horizontalVelAccuracy; // [cm/s] + uint16_t hdop; + int32_t longitude; + int32_t latitude; + int32_t mslAltitude; // cm + int32_t nedVelNorth; // cm/s + int32_t nedVelEast; + int32_t nedVelDown; + int16_t groundCourse; // deg * 100 + int16_t trueYaw; // deg * 100, values of 0..36000 are valid. 65535 = no data available + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; +} mspGpsDataMessage_t; + +static bool newDataReady; + +void gpsRestartMSP(void) +{ + // NOP +} + +void gpsHandleMSP(void) +{ + if (newDataReady) { + gpsProcessNewSolutionData(); + newDataReady = false; + } +} + +static uint8_t gpsMapFixType(uint8_t mspFixType) +{ + if (mspFixType == 2) + return GPS_FIX_2D; + if (mspFixType >= 3) + return GPS_FIX_3D; + return GPS_NO_FIX; +} + +void mspGPSReceiveNewData(uint8_t * bufferPtr) +{ + mspGpsDataMessage_t * pkt = (mspGpsDataMessage_t *)bufferPtr; + + gpsSol.fixType = gpsMapFixType(pkt->fixType); + gpsSol.numSat = pkt->satellitesInView; + gpsSol.llh.lon = pkt->longitude; + gpsSol.llh.lat = pkt->latitude; + gpsSol.llh.alt = pkt->mslAltitude; + gpsSol.velNED[X] = pkt->nedVelNorth; + gpsSol.velNED[Y] = pkt->nedVelEast; + gpsSol.velNED[Z] = pkt->nedVelDown; + gpsSol.groundSpeed = sqrtf(sq((float)pkt->nedVelNorth) + sq((float)pkt->nedVelEast)); + gpsSol.groundCourse = pkt->groundCourse / 10; // in deg * 10 + gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10); + gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10); + gpsSol.hdop = gpsConstrainHDOP(pkt->hdop); + gpsSol.flags.validVelNE = 1; + gpsSol.flags.validVelD = 1; + gpsSol.flags.validEPE = 1; + + gpsSol.time.year = pkt->year; + gpsSol.time.month = pkt->month; + gpsSol.time.day = pkt->day; + gpsSol.time.hours = pkt->hour; + gpsSol.time.minutes = pkt->min; + gpsSol.time.seconds = pkt->sec; + gpsSol.time.millis = 0; + + gpsSol.flags.validTime = (pkt->fixType >= 3); + + newDataReady = true; +} +#endif diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index 4195529d05..196404beec 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -78,4 +78,7 @@ extern void gpsHandleMTK(void); extern void gpsRestartNAZA(void); extern void gpsHandleNAZA(void); +extern void gpsRestartMSP(void); +extern void gpsHandleMSP(void); + #endif diff --git a/src/main/msp/msp_protocol_v2_sensor.h b/src/main/msp/msp_protocol_v2_sensor.h index e2741555e0..be5aef9244 100644 --- a/src/main/msp/msp_protocol_v2_sensor.h +++ b/src/main/msp/msp_protocol_v2_sensor.h @@ -18,4 +18,7 @@ #define MSP2_IS_SENSOR_MESSAGE(x) ((x) >= 0x1F00 && (x) <= 0x1FFF) #define MSP2_SENSOR_RANGEFINDER 0x1F01 -#define MSP2_SENSOR_OPTIC_FLOW 0x1F02 \ No newline at end of file +#define MSP2_SENSOR_OPTIC_FLOW 0x1F02 +#define MSP2_SENSOR_GPS 0x1F03 +#define MSP2_SENSOR_COMPASS 0x1F04 +#define MSP2_SENSOR_BAROMETER 0x1F05 \ No newline at end of file diff --git a/src/main/target/common.h b/src/main/target/common.h index 1c70a04218..e655a655fe 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -54,6 +54,7 @@ #define USE_BLACKBOX #define USE_GPS #define USE_GPS_PROTO_UBLOX +#define USE_GPS_PROTO_MSP #define USE_NAV #define USE_TELEMETRY #define USE_TELEMETRY_LTM From 816c739df93f2682c4a711ff999a3ebca1e2a431 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Fri, 4 Sep 2020 09:59:45 +0200 Subject: [PATCH 063/109] [GPS] add instance and week fields --- src/main/io/gps_msp.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/io/gps_msp.c b/src/main/io/gps_msp.c index 5ab4da326e..d72ae1e172 100644 --- a/src/main/io/gps_msp.c +++ b/src/main/io/gps_msp.c @@ -47,6 +47,8 @@ #include "scheduler/protothreads.h" typedef struct __attribute__((packed)) { + uint8_t instance; // sensor instance number to support multi-sensor setups + uint16_t gpsWeek; // GPS week, 0xFFFF if not available uint32_t msTOW; uint8_t fixType; uint8_t satellitesInView; From cf99b7dfef5ee341fff2cacb98d52068737b8e80 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sat, 5 Sep 2020 11:42:39 +0200 Subject: [PATCH 064/109] [BARO/MAG] Move MSP sensor payload structures to a separate file; Implement MSP-based BARO and MAG --- src/main/drivers/barometer/barometer_msp.c | 102 +++++++++++++++++++++ src/main/drivers/barometer/barometer_msp.h | 29 ++++++ src/main/drivers/compass/compass_msp.c | 92 +++++++++++++++++++ src/main/drivers/compass/compass_msp.h | 28 ++++++ src/main/fc/fc_msp.c | 8 +- src/main/fc/settings.yaml | 4 +- src/main/io/gps.h | 2 +- src/main/io/gps_msp.c | 32 +------ src/main/io/opflow_msp.c | 10 +- src/main/io/rangefinder_msp.c | 8 +- src/main/msp/msp_protocol_v2_sensor_msg.h | 77 ++++++++++++++++ src/main/sensors/barometer.c | 14 +++ src/main/sensors/barometer.h | 3 +- src/main/sensors/compass.c | 14 +++ src/main/sensors/compass.h | 3 +- src/main/target/common_post.h | 9 ++ 16 files changed, 387 insertions(+), 48 deletions(-) create mode 100644 src/main/drivers/barometer/barometer_msp.c create mode 100644 src/main/drivers/barometer/barometer_msp.h create mode 100644 src/main/drivers/compass/compass_msp.c create mode 100644 src/main/drivers/compass/compass_msp.h create mode 100644 src/main/msp/msp_protocol_v2_sensor_msg.h diff --git a/src/main/drivers/barometer/barometer_msp.c b/src/main/drivers/barometer/barometer_msp.c new file mode 100644 index 0000000000..be89489cf0 --- /dev/null +++ b/src/main/drivers/barometer/barometer_msp.c @@ -0,0 +1,102 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include + +#include "platform.h" + +#if defined(USE_BARO_MSP) + +#include "build/build_config.h" + +#include "common/utils.h" +#include "common/time.h" + +#include "drivers/time.h" +#include "drivers/barometer/barometer.h" +#include "drivers/barometer/barometer_msp.h" + +#include "msp/msp_protocol_v2_sensor_msg.h" + +#define MSP_BARO_TIMEOUT_MS 250 // Less than 4Hz updates is considered a failure + +static int32_t mspBaroPressure; +static int32_t mspBaroTemperature; +static timeMs_t mspBaroLastUpdateMs; + +static bool mspBaroStartGet(baroDev_t * baro) +{ + UNUSED(baro); + return true; +} + +static bool mspBaroCalculate(baroDev_t * baro, int32_t *pressure, int32_t *temperature) +{ + UNUSED(baro); + + if ((millis() - mspBaroLastUpdateMs) > MSP_BARO_TIMEOUT_MS) { + return false; + } + + if (pressure) + *pressure = mspBaroPressure; + + if (temperature) + *temperature = mspBaroTemperature; + + return true; +} + +void mspBaroReceiveNewData(uint8_t * bufferPtr) +{ + const mspSensorBaroDataMessage_t * pkt = (const mspSensorBaroDataMessage_t *)bufferPtr; + + mspBaroPressure = pkt->pressurePa; + mspBaroTemperature = pkt->temp; + + mspBaroLastUpdateMs = millis(); +} + +bool mspBaroDetect(baroDev_t *baro) +{ + mspBaroPressure = 101325; // pressure in Pa (0m MSL) + mspBaroTemperature = 2500; // temperature in 0.01 C = 25 deg + + // these are dummy as temperature is measured as part of pressure + baro->ut_delay = 10000; + baro->get_ut = mspBaroStartGet; + baro->start_ut = mspBaroStartGet; + + // only _up part is executed, and gets both temperature and pressure + baro->up_delay = 10000; + baro->start_up = mspBaroStartGet; + baro->get_up = mspBaroStartGet; + + baro->calculate = mspBaroCalculate; + + return true; +} + +#endif diff --git a/src/main/drivers/barometer/barometer_msp.h b/src/main/drivers/barometer/barometer_msp.h new file mode 100644 index 0000000000..dcf979be53 --- /dev/null +++ b/src/main/drivers/barometer/barometer_msp.h @@ -0,0 +1,29 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +struct baroDev_s; +bool mspBaroDetect(struct baroDev_s *baro); +void mspBaroReceiveNewData(uint8_t * bufferPtr); \ No newline at end of file diff --git a/src/main/drivers/compass/compass_msp.c b/src/main/drivers/compass/compass_msp.c new file mode 100644 index 0000000000..c8efe627de --- /dev/null +++ b/src/main/drivers/compass/compass_msp.c @@ -0,0 +1,92 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include + +#include "platform.h" + +#if defined(USE_MAG_MSP) + +#include "build/build_config.h" + +#include "common/axis.h" +#include "common/utils.h" +#include "common/time.h" + +#include "drivers/time.h" +#include "drivers/compass/compass.h" +#include "drivers/compass/compass_msp.h" + +#include "msp/msp_protocol_v2_sensor_msg.h" + +#define MSP_MAG_TIMEOUT_MS 250 // Less than 4Hz updates is considered a failure + +static int16_t mspMagData[XYZ_AXIS_COUNT]; +static timeMs_t mspMagLastUpdateMs; + +static bool mspMagInit(magDev_t *magDev) +{ + UNUSED(magDev); + mspMagData[X] = 0; + mspMagData[Y] = 0; + mspMagData[Z] = 0; + mspMagLastUpdateMs = 0; + return true; +} + +void mspMagReceiveNewData(uint8_t * bufferPtr) +{ + const mspSensorCompassDataMessage_t * pkt = (const mspSensorCompassDataMessage_t *)bufferPtr; + + mspMagData[X] = pkt->magX; + mspMagData[Y] = pkt->magY; + mspMagData[Z] = pkt->magZ; + + mspMagLastUpdateMs = millis(); +} + +static bool mspMagRead(magDev_t *magDev) +{ + UNUSED(magDev); + + if ((millis() - mspMagLastUpdateMs) > MSP_MAG_TIMEOUT_MS) { + return false; + } + + magDev->magADCRaw[X] = mspMagData[X]; + magDev->magADCRaw[Y] = mspMagData[Y]; + magDev->magADCRaw[Z] = mspMagData[Z]; + + return true; +} + +bool mspMagDetect(magDev_t *mag) +{ + mag->init = mspMagInit; + mag->read = mspMagRead; + return true; +} + +#endif diff --git a/src/main/drivers/compass/compass_msp.h b/src/main/drivers/compass/compass_msp.h new file mode 100644 index 0000000000..529e0c1df0 --- /dev/null +++ b/src/main/drivers/compass/compass_msp.h @@ -0,0 +1,28 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +bool mspMagDetect(struct magDev_s *mag); +void mspMagReceiveNewData(uint8_t * bufferPtr); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0fa160cf8a..22c357ce91 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -40,8 +40,10 @@ #include "config/parameter_group_ids.h" #include "drivers/accgyro/accgyro.h" -#include "drivers/bus_i2c.h" #include "drivers/compass/compass.h" +#include "drivers/compass/compass_msp.h" +#include "drivers/barometer/barometer_msp.h" +#include "drivers/bus_i2c.h" #include "drivers/display.h" #include "drivers/flash.h" #include "drivers/osd.h" @@ -3222,9 +3224,9 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src) break; #endif -#if defined(USE_COMPASS_MSP) +#if defined(USE_MAG_MSP) case MSP2_SENSOR_COMPASS: - mspCompassReceiveNewData(sbufPtr(src)); + mspMagReceiveNewData(sbufPtr(src)); break; #endif diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index edfe63576d..bb78ac980f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -10,13 +10,13 @@ tables: values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB", "BENEWAKE"] enum: rangefinderType_e - name: mag_hardware - values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "FAKE"] + values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"] enum: magSensor_e - name: opflow_hardware values: ["NONE", "PMW3901", "CXOF", "MSP", "FAKE"] enum: opticalFlowSensor_e - name: baro_hardware - values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "FAKE"] + values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "MSP", "FAKE"] enum: baroSensor_e - name: pitot_hardware values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE"] diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 63e02696f2..6b1e09ad45 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -165,4 +165,4 @@ bool isGPSHealthy(void); bool isGPSHeadingValid(void); struct serialPort_s; void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort); -void mspGPSReceiveNewData(uint8_t * bufferPtr); +void mspGPSReceiveNewData(const uint8_t * bufferPtr); diff --git a/src/main/io/gps_msp.c b/src/main/io/gps_msp.c index d72ae1e172..a5652d4c73 100644 --- a/src/main/io/gps_msp.c +++ b/src/main/io/gps_msp.c @@ -44,33 +44,7 @@ #include "io/gps_private.h" #include "io/serial.h" -#include "scheduler/protothreads.h" - -typedef struct __attribute__((packed)) { - uint8_t instance; // sensor instance number to support multi-sensor setups - uint16_t gpsWeek; // GPS week, 0xFFFF if not available - uint32_t msTOW; - uint8_t fixType; - uint8_t satellitesInView; - uint16_t horizontalPosAccuracy; // [cm] - uint16_t verticalPosAccuracy; // [cm] - uint16_t horizontalVelAccuracy; // [cm/s] - uint16_t hdop; - int32_t longitude; - int32_t latitude; - int32_t mslAltitude; // cm - int32_t nedVelNorth; // cm/s - int32_t nedVelEast; - int32_t nedVelDown; - int16_t groundCourse; // deg * 100 - int16_t trueYaw; // deg * 100, values of 0..36000 are valid. 65535 = no data available - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t min; - uint8_t sec; -} mspGpsDataMessage_t; +#include "msp/msp_protocol_v2_sensor_msg.h" static bool newDataReady; @@ -96,9 +70,9 @@ static uint8_t gpsMapFixType(uint8_t mspFixType) return GPS_NO_FIX; } -void mspGPSReceiveNewData(uint8_t * bufferPtr) +void mspGPSReceiveNewData(const uint8_t * bufferPtr) { - mspGpsDataMessage_t * pkt = (mspGpsDataMessage_t *)bufferPtr; + const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr; gpsSol.fixType = gpsMapFixType(pkt->fixType); gpsSol.numSat = pkt->satellitesInView; diff --git a/src/main/io/opflow_msp.c b/src/main/io/opflow_msp.c index 029fa09573..778ebccca4 100644 --- a/src/main/io/opflow_msp.c +++ b/src/main/io/opflow_msp.c @@ -37,15 +37,13 @@ #include "io/serial.h" #if defined(USE_OPFLOW_MSP) + #include "drivers/opflow/opflow_virtual.h" #include "drivers/time.h" #include "io/opflow.h" -typedef struct __attribute__((packed)) { - uint8_t quality; // [0;255] - int32_t motionX; - int32_t motionY; -} mspOpflowSensor_t; +#include "msp/msp_protocol_v2_sensor_msg.h" + static bool hasNewData = false; static timeUs_t updatedTimeUs = 0; @@ -76,7 +74,7 @@ static bool mspOpflowUpdate(opflowData_t * data) void mspOpflowReceiveNewData(uint8_t * bufferPtr) { const timeUs_t currentTimeUs = micros(); - const mspOpflowSensor_t * pkt = (const mspOpflowSensor_t *)bufferPtr; + const mspSensorOpflowDataMessage_t * pkt = (const mspSensorOpflowDataMessage_t *)bufferPtr; sensorData.deltaTime = currentTimeUs - updatedTimeUs; sensorData.flowRateRaw[0] = pkt->motionX; diff --git a/src/main/io/rangefinder_msp.c b/src/main/io/rangefinder_msp.c index b5e5eae113..5c7f728f2f 100644 --- a/src/main/io/rangefinder_msp.c +++ b/src/main/io/rangefinder_msp.c @@ -37,14 +37,12 @@ #include "io/serial.h" #if defined(USE_RANGEFINDER_MSP) + #include "drivers/rangefinder/rangefinder_virtual.h" #include "drivers/time.h" #include "io/rangefinder.h" +#include "msp/msp_protocol_v2_sensor_msg.h" -typedef struct __attribute__((packed)) { - uint8_t quality; // [0;255] - int32_t distanceMm; // Negative value for out of range -} mspRangefinderSensor_t; static bool hasNewData = false; static int32_t sensorData = RANGEFINDER_NO_NEW_DATA; @@ -76,7 +74,7 @@ static int32_t mspRangefinderGetDistance(void) void mspRangefinderReceiveNewData(uint8_t * bufferPtr) { - const mspRangefinderSensor_t * pkt = (const mspRangefinderSensor_t *)bufferPtr; + const mspSensorRangefinderDataMessage_t * pkt = (const mspSensorRangefinderDataMessage_t *)bufferPtr; sensorData = pkt->distanceMm / 10; hasNewData = true; diff --git a/src/main/msp/msp_protocol_v2_sensor_msg.h b/src/main/msp/msp_protocol_v2_sensor_msg.h new file mode 100644 index 0000000000..5a6f50504f --- /dev/null +++ b/src/main/msp/msp_protocol_v2_sensor_msg.h @@ -0,0 +1,77 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +typedef struct __attribute__((packed)) { + uint8_t quality; // [0;255] + int32_t motionX; + int32_t motionY; +} mspSensorOpflowDataMessage_t; + +typedef struct __attribute__((packed)) { + uint8_t quality; // [0;255] + int32_t distanceMm; // Negative value for out of range +} mspSensorRangefinderDataMessage_t; + +typedef struct __attribute__((packed)) { + uint8_t instance; // sensor instance number to support multi-sensor setups + uint16_t gpsWeek; // GPS week, 0xFFFF if not available + uint32_t msTOW; + uint8_t fixType; + uint8_t satellitesInView; + uint16_t horizontalPosAccuracy; // [cm] + uint16_t verticalPosAccuracy; // [cm] + uint16_t horizontalVelAccuracy; // [cm/s] + uint16_t hdop; + int32_t longitude; + int32_t latitude; + int32_t mslAltitude; // cm + int32_t nedVelNorth; // cm/s + int32_t nedVelEast; + int32_t nedVelDown; + uint16_t groundCourse; // deg * 100, 0..36000 + uint16_t trueYaw; // deg * 100, values of 0..36000 are valid. 65535 = no data available + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; +} mspSensorGpsDataMessage_t; + +typedef struct __attribute__((packed)) { + uint8_t instance; + uint32_t timeMs; + float pressurePa; + int16_t temp; // centi-degrees C +} mspSensorBaroDataMessage_t; + +typedef struct __attribute__((packed)) { + uint8_t instance; + uint32_t timeMs; + int16_t magX; // mGauss, front + int16_t magY; // mGauss, right + int16_t magZ; // mGauss, down +} mspSensorCompassDataMessage_t; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 49d8d0cc59..b6457c9639 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -40,6 +40,7 @@ #include "drivers/barometer/barometer_ms56xx.h" #include "drivers/barometer/barometer_spl06.h" #include "drivers/barometer/barometer_dps310.h" +#include "drivers/barometer/barometer_msp.h" #include "drivers/time.h" #include "fc/runtime_config.h" @@ -187,6 +188,19 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) } FALLTHROUGH; + case BARO_MSP: +#ifdef USE_BARO_MSP + if (mspBaroDetect(dev)) { + baroHardware = BARO_MSP; + break; + } +#endif + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (baroHardwareToUse != BARO_AUTODETECT) { + break; + } + FALLTHROUGH; + case BARO_FAKE: #ifdef USE_FAKE_BARO if (fakeBaroDetect(dev)) { diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 6301c79b0b..2e365fb115 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -32,7 +32,8 @@ typedef enum { BARO_SPL06 = 7, BARO_BMP388 = 8, BARO_DPS310 = 9, - BARO_FAKE = 10, + BARO_MSP = 10, + BARO_FAKE = 11, BARO_MAX = BARO_FAKE } baroSensor_e; diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 842c508820..6e13ede2ca 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -40,6 +40,7 @@ #include "drivers/compass/compass_qmc5883l.h" #include "drivers/compass/compass_mpu9250.h" #include "drivers/compass/compass_lis3mdl.h" +#include "drivers/compass/compass_msp.h" #include "drivers/io.h" #include "drivers/light_led.h" #include "drivers/time.h" @@ -249,6 +250,19 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) } FALLTHROUGH; + case MAG_MSP: +#ifdef USE_MAG_MSP + if (mspMagDetect(dev)) { + magHardware = MAG_MSP; + break; + } +#endif + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (magHardwareToUse != MAG_AUTODETECT) { + break; + } + FALLTHROUGH; + case MAG_FAKE: #ifdef USE_FAKE_MAG if (fakeMagDetect(dev)) { diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 7c9a0fca63..f715638107 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -41,7 +41,8 @@ typedef enum { MAG_MPU9250 = 9, MAG_IST8308 = 10, MAG_LIS3MDL = 11, - MAG_FAKE = 12, + MAG_MSP = 12, + MAG_FAKE = 13, MAG_MAX = MAG_FAKE } magSensor_e; diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 32b6e3531a..d3c56bc596 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -35,6 +35,15 @@ #define USE_CANVAS #endif +// Enable MSP BARO & MAG drivers if BARO and MAG sensors are compiled in +#if defined(USE_MAG) +#define USE_MAG_MSP +#endif + +#if defined(USE_BARO) +#define USE_BARO_MSP +#endif + #ifdef USE_ESC_SENSOR #define USE_RPM_FILTER #endif From f0943875bab00528dd30b1cf3c2cd08cc366dfb7 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Tue, 8 Sep 2020 11:26:50 +0200 Subject: [PATCH 065/109] [MSP SENSOR] Apply default rotation inside of the compass MSP driver; Disable baro median filtering by default; Skip autodetection of MSP-based sensors (only manual selection is allowed) --- src/main/drivers/barometer/barometer_msp.c | 2 +- src/main/drivers/compass/compass_msp.c | 6 ++++- src/main/io/gps_msp.c | 27 ++++++++++++++-------- src/main/sensors/barometer.c | 9 ++++---- src/main/sensors/compass.c | 3 ++- 5 files changed, 30 insertions(+), 17 deletions(-) diff --git a/src/main/drivers/barometer/barometer_msp.c b/src/main/drivers/barometer/barometer_msp.c index be89489cf0..b17023d699 100644 --- a/src/main/drivers/barometer/barometer_msp.c +++ b/src/main/drivers/barometer/barometer_msp.c @@ -30,6 +30,7 @@ #if defined(USE_BARO_MSP) #include "build/build_config.h" +#include "build/debug.h" #include "common/utils.h" #include "common/time.h" @@ -75,7 +76,6 @@ void mspBaroReceiveNewData(uint8_t * bufferPtr) mspBaroPressure = pkt->pressurePa; mspBaroTemperature = pkt->temp; - mspBaroLastUpdateMs = millis(); } diff --git a/src/main/drivers/compass/compass_msp.c b/src/main/drivers/compass/compass_msp.c index c8efe627de..f9c1cb794b 100644 --- a/src/main/drivers/compass/compass_msp.c +++ b/src/main/drivers/compass/compass_msp.c @@ -39,11 +39,13 @@ #include "drivers/compass/compass.h" #include "drivers/compass/compass_msp.h" +#include "sensors/boardalignment.h" + #include "msp/msp_protocol_v2_sensor_msg.h" #define MSP_MAG_TIMEOUT_MS 250 // Less than 4Hz updates is considered a failure -static int16_t mspMagData[XYZ_AXIS_COUNT]; +static int32_t mspMagData[XYZ_AXIS_COUNT]; static timeMs_t mspMagLastUpdateMs; static bool mspMagInit(magDev_t *magDev) @@ -64,6 +66,8 @@ void mspMagReceiveNewData(uint8_t * bufferPtr) mspMagData[Y] = pkt->magY; mspMagData[Z] = pkt->magZ; + applySensorAlignment(mspMagData, mspMagData, CW90_DEG_FLIP); + mspMagLastUpdateMs = millis(); } diff --git a/src/main/io/gps_msp.c b/src/main/io/gps_msp.c index a5652d4c73..35e7d12a1f 100644 --- a/src/main/io/gps_msp.c +++ b/src/main/io/gps_msp.c @@ -1,18 +1,25 @@ /* - * This file is part of Cleanflight. + * This file is part of INAV Project. * - * 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. + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. * - * 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. + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. * * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . + * along with this program. If not, see http://www.gnu.org/licenses/. */ #include diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index b6457c9639..3506e9361a 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -56,7 +56,7 @@ baro_t baro; // barometer access functions -PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 2); #ifdef USE_BARO #define BARO_HARDWARE_DEFAULT BARO_AUTODETECT @@ -65,7 +65,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER #endif PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig, .baro_hardware = BARO_HARDWARE_DEFAULT, - .use_median_filtering = 1, + .use_median_filtering = 0, .baro_calibration_tolerance = 150 ); @@ -190,7 +190,8 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) case BARO_MSP: #ifdef USE_BARO_MSP - if (mspBaroDetect(dev)) { + // Skip autodetection for MSP baro, only allow manual config + if (baroHardwareToUse != BARO_AUTODETECT && mspBaroDetect(dev)) { baroHardware = BARO_MSP; break; } @@ -364,7 +365,7 @@ int32_t baroCalculateAltitude(void) #endif // calculates height from ground via baro readings baro.BaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude; - } + } return baro.BaroAlt; } diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 6e13ede2ca..90a108109d 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -252,7 +252,8 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) case MAG_MSP: #ifdef USE_MAG_MSP - if (mspMagDetect(dev)) { + // Skip autodetection for MSP mag + if (magHardwareToUse != MAG_AUTODETECT && mspMagDetect(dev)) { magHardware = MAG_MSP; break; } From c91b91ae82d87d2206e74e9485cf3f5b1802eece Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sat, 24 Oct 2020 10:08:02 +0200 Subject: [PATCH 066/109] [MSP SENSOR] Add support for MSP Airspeed sensor --- src/main/CMakeLists.txt | 19 ++-- .../drivers/{ => pitotmeter}/pitotmeter.h | 0 .../drivers/{ => pitotmeter}/pitotmeter_adc.c | 6 +- .../drivers/{ => pitotmeter}/pitotmeter_adc.h | 0 .../{ => pitotmeter}/pitotmeter_fake.c | 0 .../{ => pitotmeter}/pitotmeter_fake.h | 0 .../{ => pitotmeter}/pitotmeter_ms4525.c | 2 +- .../{ => pitotmeter}/pitotmeter_ms4525.h | 0 src/main/drivers/pitotmeter/pitotmeter_msp.c | 97 +++++++++++++++++++ src/main/drivers/pitotmeter/pitotmeter_msp.h | 29 ++++++ .../{ => pitotmeter}/pitotmeter_virtual.c | 4 +- .../{ => pitotmeter}/pitotmeter_virtual.h | 0 src/main/fc/fc_msp.c | 7 ++ src/main/fc/settings.yaml | 2 +- src/main/msp/msp_protocol_v2_sensor.h | 3 +- src/main/msp/msp_protocol_v2_sensor_msg.h | 7 ++ src/main/sensors/pitotmeter.c | 23 ++++- src/main/sensors/pitotmeter.h | 3 +- src/main/target/common.h | 2 + 19 files changed, 185 insertions(+), 19 deletions(-) rename src/main/drivers/{ => pitotmeter}/pitotmeter.h (100%) rename src/main/drivers/{ => pitotmeter}/pitotmeter_adc.c (94%) mode change 100755 => 100644 rename src/main/drivers/{ => pitotmeter}/pitotmeter_adc.h (100%) mode change 100755 => 100644 rename src/main/drivers/{ => pitotmeter}/pitotmeter_fake.c (100%) mode change 100755 => 100644 rename src/main/drivers/{ => pitotmeter}/pitotmeter_fake.h (100%) mode change 100755 => 100644 rename src/main/drivers/{ => pitotmeter}/pitotmeter_ms4525.c (99%) rename src/main/drivers/{ => pitotmeter}/pitotmeter_ms4525.h (100%) create mode 100644 src/main/drivers/pitotmeter/pitotmeter_msp.c create mode 100644 src/main/drivers/pitotmeter/pitotmeter_msp.h rename src/main/drivers/{ => pitotmeter}/pitotmeter_virtual.c (96%) rename src/main/drivers/{ => pitotmeter}/pitotmeter_virtual.h (100%) diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 146164679e..0bd201a9fb 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -118,6 +118,8 @@ main_sources(COMMON_SRC drivers/barometer/barometer_ms56xx.h drivers/barometer/barometer_spl06.c drivers/barometer/barometer_spl06.h + drivers/barometer/barometer_msp.c + drivers/barometer/barometer_msp.h drivers/buf_writer.c drivers/buf_writer.h @@ -148,6 +150,8 @@ main_sources(COMMON_SRC drivers/compass/compass_mpu9250.h drivers/compass/compass_qmc5883l.c drivers/compass/compass_qmc5883l.h + drivers/compass/compass_msp.c + drivers/compass/compass_msp.h drivers/display.c drivers/display.h @@ -195,12 +199,14 @@ main_sources(COMMON_SRC drivers/osd.h drivers/persistent.c drivers/persistent.h - drivers/pitotmeter_adc.c - drivers/pitotmeter_adc.h - drivers/pitotmeter_ms4525.c - drivers/pitotmeter_ms4525.h - drivers/pitotmeter_virtual.c - drivers/pitotmeter_virtual.h + drivers/pitotmeter/pitotmeter_adc.c + drivers/pitotmeter/pitotmeter_adc.h + drivers/pitotmeter/pitotmeter_ms4525.c + drivers/pitotmeter/pitotmeter_ms4525.h + drivers/pitotmeter/pitotmeter_msp.c + drivers/pitotmeter/pitotmeter_msp.h + drivers/pitotmeter/pitotmeter_virtual.c + drivers/pitotmeter/pitotmeter_virtual.h drivers/pwm_esc_detect.c drivers/pwm_esc_detect.h drivers/pwm_mapping.c @@ -499,6 +505,7 @@ main_sources(COMMON_SRC io/gps_ublox.c io/gps_nmea.c io/gps_naza.c + io/gps_msp.c io/gps_private.h io/ledstrip.c io/ledstrip.h diff --git a/src/main/drivers/pitotmeter.h b/src/main/drivers/pitotmeter/pitotmeter.h similarity index 100% rename from src/main/drivers/pitotmeter.h rename to src/main/drivers/pitotmeter/pitotmeter.h diff --git a/src/main/drivers/pitotmeter_adc.c b/src/main/drivers/pitotmeter/pitotmeter_adc.c old mode 100755 new mode 100644 similarity index 94% rename from src/main/drivers/pitotmeter_adc.c rename to src/main/drivers/pitotmeter/pitotmeter_adc.c index d0c6a1ac0e..da68570ca9 --- a/src/main/drivers/pitotmeter_adc.c +++ b/src/main/drivers/pitotmeter/pitotmeter_adc.c @@ -24,9 +24,9 @@ #include "common/utils.h" -#include "pitotmeter.h" -#include "pitotmeter_adc.h" -#include "adc.h" +#include "drivers/pitotmeter/pitotmeter.h" +#include "drivers/pitotmeter/pitotmeter_adc.h" +#include "drivers/adc.h" #if defined(USE_ADC) && defined(USE_PITOT_ADC) diff --git a/src/main/drivers/pitotmeter_adc.h b/src/main/drivers/pitotmeter/pitotmeter_adc.h old mode 100755 new mode 100644 similarity index 100% rename from src/main/drivers/pitotmeter_adc.h rename to src/main/drivers/pitotmeter/pitotmeter_adc.h diff --git a/src/main/drivers/pitotmeter_fake.c b/src/main/drivers/pitotmeter/pitotmeter_fake.c old mode 100755 new mode 100644 similarity index 100% rename from src/main/drivers/pitotmeter_fake.c rename to src/main/drivers/pitotmeter/pitotmeter_fake.c diff --git a/src/main/drivers/pitotmeter_fake.h b/src/main/drivers/pitotmeter/pitotmeter_fake.h old mode 100755 new mode 100644 similarity index 100% rename from src/main/drivers/pitotmeter_fake.h rename to src/main/drivers/pitotmeter/pitotmeter_fake.h diff --git a/src/main/drivers/pitotmeter_ms4525.c b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c similarity index 99% rename from src/main/drivers/pitotmeter_ms4525.c rename to src/main/drivers/pitotmeter/pitotmeter_ms4525.c index b21c35bb8f..436bddc3cf 100644 --- a/src/main/drivers/pitotmeter_ms4525.c +++ b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c @@ -24,8 +24,8 @@ #include "common/utils.h" #include "common/maths.h" #include "drivers/bus_i2c.h" -#include "drivers/pitotmeter.h" #include "drivers/time.h" +#include "drivers/pitotmeter/pitotmeter.h" // MS4525, Standard address 0x28 #define MS4525_ADDR 0x28 diff --git a/src/main/drivers/pitotmeter_ms4525.h b/src/main/drivers/pitotmeter/pitotmeter_ms4525.h similarity index 100% rename from src/main/drivers/pitotmeter_ms4525.h rename to src/main/drivers/pitotmeter/pitotmeter_ms4525.h diff --git a/src/main/drivers/pitotmeter/pitotmeter_msp.c b/src/main/drivers/pitotmeter/pitotmeter_msp.c new file mode 100644 index 0000000000..97a1c48699 --- /dev/null +++ b/src/main/drivers/pitotmeter/pitotmeter_msp.c @@ -0,0 +1,97 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include + +#include "platform.h" + +#if defined(USE_BARO_MSP) + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/utils.h" +#include "common/time.h" + +#include "drivers/time.h" +#include "drivers/pitotmeter/pitotmeter.h" +#include "drivers/pitotmeter/pitotmeter_msp.h" + +#include "msp/msp_protocol_v2_sensor_msg.h" + +#define MSP_PITOT_TIMEOUT_MS 500 // Less than 2Hz updates is considered a failure + +static int32_t mspPitotPressure; +static int32_t mspPitotTemperature; +static timeMs_t mspPitotLastUpdateMs; + +static bool mspPitotStart(pitotDev_t *pitot) +{ + UNUSED(pitot); + return true; +} + +static bool mspPitotRead(pitotDev_t *pitot) +{ + UNUSED(pitot); + return true; +} + +static void mspPitotCalculate(pitotDev_t *pitot, float *pressure, float *temperature) +{ + UNUSED(pitot); + + if (pressure) { + *pressure = mspPitotPressure; + } + + if (temperature) { + *temperature = (mspPitotTemperature - 27315) / 100.0f; // Pitot expects temp in Kelvin + } +} + +void mspPitotmeterReceiveNewData(uint8_t * bufferPtr) +{ + const mspSensorAirspeedDataMessage_t * pkt = (const mspSensorAirspeedDataMessage_t *)bufferPtr; + + mspPitotPressure = pkt->diffPressurePa; + mspPitotTemperature = pkt->temp; + mspPitotLastUpdateMs = millis(); +} + +bool mspPitotmeterDetect(pitotDev_t *pitot) +{ + mspPitotPressure = 0; + mspPitotTemperature = 27315; // 0 deg/c + + pitot->delay = 10000; + pitot->start = mspPitotStart; + pitot->get = mspPitotRead; + pitot->calculate = mspPitotCalculate; + + return true; +} + +#endif diff --git a/src/main/drivers/pitotmeter/pitotmeter_msp.h b/src/main/drivers/pitotmeter/pitotmeter_msp.h new file mode 100644 index 0000000000..3e1fabec61 --- /dev/null +++ b/src/main/drivers/pitotmeter/pitotmeter_msp.h @@ -0,0 +1,29 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +struct pitotDev_s; +bool mspPitotmeterDetect(struct pitotDev_s *pitot); +void mspPitotmeterReceiveNewData(uint8_t * bufferPtr); \ No newline at end of file diff --git a/src/main/drivers/pitotmeter_virtual.c b/src/main/drivers/pitotmeter/pitotmeter_virtual.c similarity index 96% rename from src/main/drivers/pitotmeter_virtual.c rename to src/main/drivers/pitotmeter/pitotmeter_virtual.c index 4ef87375e8..3f72828517 100644 --- a/src/main/drivers/pitotmeter_virtual.c +++ b/src/main/drivers/pitotmeter/pitotmeter_virtual.c @@ -42,8 +42,8 @@ #include "sensors/pitotmeter.h" -#include "pitotmeter.h" -#include "pitotmeter_virtual.h" +#include "drivers/pitotmeter/pitotmeter.h" +#include "drivers/pitotmeter/pitotmeter_virtual.h" #if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) diff --git a/src/main/drivers/pitotmeter_virtual.h b/src/main/drivers/pitotmeter/pitotmeter_virtual.h similarity index 100% rename from src/main/drivers/pitotmeter_virtual.h rename to src/main/drivers/pitotmeter/pitotmeter_virtual.h diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 22c357ce91..7e070167ee 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -43,6 +43,7 @@ #include "drivers/compass/compass.h" #include "drivers/compass/compass_msp.h" #include "drivers/barometer/barometer_msp.h" +#include "drivers/pitotmeter/pitotmeter_msp.h" #include "drivers/bus_i2c.h" #include "drivers/display.h" #include "drivers/flash.h" @@ -3235,6 +3236,12 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src) mspBaroReceiveNewData(sbufPtr(src)); break; #endif + +#if defined(USE_PITOT_MSP) + case MSP2_SENSOR_AIRSPEED: + mspPitotmeterReceiveNewData(sbufPtr(src)); + break; +#endif } return MSP_RESULT_NO_REPLY; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index bb78ac980f..a5142b3745 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -19,7 +19,7 @@ tables: values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "MSP", "FAKE"] enum: baroSensor_e - name: pitot_hardware - values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE"] + values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"] enum: pitotSensor_e - name: receiver_type values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"] diff --git a/src/main/msp/msp_protocol_v2_sensor.h b/src/main/msp/msp_protocol_v2_sensor.h index be5aef9244..d913723be7 100644 --- a/src/main/msp/msp_protocol_v2_sensor.h +++ b/src/main/msp/msp_protocol_v2_sensor.h @@ -21,4 +21,5 @@ #define MSP2_SENSOR_OPTIC_FLOW 0x1F02 #define MSP2_SENSOR_GPS 0x1F03 #define MSP2_SENSOR_COMPASS 0x1F04 -#define MSP2_SENSOR_BAROMETER 0x1F05 \ No newline at end of file +#define MSP2_SENSOR_BAROMETER 0x1F05 +#define MSP2_SENSOR_AIRSPEED 0x1F06 \ No newline at end of file diff --git a/src/main/msp/msp_protocol_v2_sensor_msg.h b/src/main/msp/msp_protocol_v2_sensor_msg.h index 5a6f50504f..4ff0324885 100644 --- a/src/main/msp/msp_protocol_v2_sensor_msg.h +++ b/src/main/msp/msp_protocol_v2_sensor_msg.h @@ -68,6 +68,13 @@ typedef struct __attribute__((packed)) { int16_t temp; // centi-degrees C } mspSensorBaroDataMessage_t; +typedef struct __attribute__((packed)) { + uint8_t instance; + uint32_t timeMs; + float diffPressurePa; + int16_t temp; // centi-degrees C +} mspSensorAirspeedDataMessage_t; + typedef struct __attribute__((packed)) { uint8_t instance; uint32_t timeMs; diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index b86f2c280b..60fbaed019 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -29,10 +29,11 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" -#include "drivers/pitotmeter.h" -#include "drivers/pitotmeter_ms4525.h" -#include "drivers/pitotmeter_adc.h" -#include "drivers/pitotmeter_virtual.h" +#include "drivers/pitotmeter/pitotmeter.h" +#include "drivers/pitotmeter/pitotmeter_ms4525.h" +#include "drivers/pitotmeter/pitotmeter_adc.h" +#include "drivers/pitotmeter/pitotmeter_msp.h" +#include "drivers/pitotmeter/pitotmeter_virtual.h" #include "drivers/time.h" #include "fc/config.h" @@ -108,6 +109,20 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse) } FALLTHROUGH; + case PITOT_MSP: +#ifdef USE_PITOT_MSP + // Skip autodetection for MSP baro, only allow manual config + if (pitotHardwareToUse != PITOT_AUTODETECT && mspPitotmeterDetect(dev)) { + pitotHardware = PITOT_MSP; + break; + } +#endif + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (pitotHardwareToUse != PITOT_AUTODETECT) { + break; + } + FALLTHROUGH; + case PITOT_FAKE: #ifdef USE_PITOT_FAKE if (fakePitotDetect(dev)) { diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index 5bdd955ea2..b7fd3c4ca7 100644 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -21,7 +21,7 @@ #include "common/filter.h" #include "common/calibration.h" -#include "drivers/pitotmeter.h" +#include "drivers/pitotmeter/pitotmeter.h" typedef enum { PITOT_NONE = 0, @@ -30,6 +30,7 @@ typedef enum { PITOT_ADC = 3, PITOT_VIRTUAL = 4, PITOT_FAKE = 5, + PITOT_MSP = 6, } pitotSensor_e; #define PITOT_MAX PITOT_FAKE diff --git a/src/main/target/common.h b/src/main/target/common.h index e655a655fe..c403b78ecd 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -94,8 +94,10 @@ #define USE_OPFLOW_CXOF #define USE_OPFLOW_MSP +// Allow default airspeed sensors #define USE_PITOT #define USE_PITOT_MS4525 +#define USE_PITOT_MSP #define USE_1WIRE #define USE_1WIRE_DS2482 From 05737411f0ebf4ca3ef749c87ed4944f05a307c8 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Wed, 28 Oct 2020 10:54:33 +0100 Subject: [PATCH 067/109] Fix wrong ifdef clause --- src/main/drivers/pitotmeter/pitotmeter_msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/pitotmeter/pitotmeter_msp.c b/src/main/drivers/pitotmeter/pitotmeter_msp.c index 97a1c48699..be1cf852c7 100644 --- a/src/main/drivers/pitotmeter/pitotmeter_msp.c +++ b/src/main/drivers/pitotmeter/pitotmeter_msp.c @@ -27,7 +27,7 @@ #include "platform.h" -#if defined(USE_BARO_MSP) +#if defined(USE_PITOT_MSP) #include "build/build_config.h" #include "build/debug.h" From e965eb2db06d2efda337ddbbe0c731f90dc83964 Mon Sep 17 00:00:00 2001 From: scavanger Date: Wed, 28 Oct 2020 23:46:36 +0100 Subject: [PATCH 068/109] Add TPA to inflight adjustment (+ OSD element) --- src/main/fc/rc_adjustments.c | 14 ++++++++++++++ src/main/fc/rc_adjustments.h | 4 +++- src/main/io/osd.c | 22 ++++++++++++++++++++++ src/main/io/osd.h | 1 + 4 files changed, 40 insertions(+), 1 deletion(-) diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 644dd19b4f..13844ac8c2 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -268,6 +268,14 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_VTX_POWER_LEVEL, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_TPA, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_TPA_BREAKPOINT, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 5 }} #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT }, { .adjustmentFunction = ADJUSTMENT_PROFILE, @@ -558,6 +566,12 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t } break; #endif + case ADJUSTMENT_TPA: + applyAdjustmentU8(ADJUSTMENT_TPA, &controlRateConfig->throttle.dynPID, delta, 0, CONTROL_RATE_CONFIG_TPA_MAX); + break; + case ADJUSTMENT_TPA_BREAKPOINT: + applyAdjustmentU16(ADJUSTMENT_TPA_BREAKPOINT, &controlRateConfig->throttle.pa_breakpoint, delta, PWM_RANGE_MIN, PWM_RANGE_MAX); + break; default: break; }; diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index b51cf390d6..75214d6c68 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -75,8 +75,10 @@ typedef enum { ADJUSTMENT_VEL_Z_D = 47, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 48, ADJUSTMENT_VTX_POWER_LEVEL = 49, + ADJUSTMENT_TPA = 50, + ADJUSTMENT_TPA_BREAKPOINT = 51, #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT - ADJUSTMENT_PROFILE = 50, + ADJUSTMENT_PROFILE = 52, #endif ADJUSTMENT_FUNCTION_COUNT // must be last } adjustmentFunction_e; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 300d98fb86..70c6d7fddf 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2411,7 +2411,29 @@ static bool osdDrawSingleElement(uint8_t item) return true; } #endif + case OSD_TPA: + { + char buff[4]; + textAttributes_t attr; + displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA BP"); + + attr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID); + if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA)) { + TEXT_ATTRIBUTES_ADD_BLINK(attr); + } + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, attr); + + attr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "BP %4d", currentControlRateProfile->throttle.pa_breakpoint); + if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) { + TEXT_ATTRIBUTES_ADD_BLINK(attr); + } + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY + 1, buff, attr); + + return true; + } default: return false; } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index dba1d7ec31..64df80b570 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -215,6 +215,7 @@ typedef enum { OSD_GVAR_1, OSD_GVAR_2, OSD_GVAR_3, + OSD_TPA, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; From 20efd8939ccffe0b4a01fad46e5560323908ea51 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Mon, 24 Aug 2020 17:10:28 +0200 Subject: [PATCH 069/109] Make nav_overrides_motor_stop tristate: OFF | AUTO_ONLY | ALL_NAV --- src/main/fc/settings.yaml | 11 +++++++---- src/main/flight/mixer.c | 4 +++- src/main/navigation/navigation.c | 4 ++-- src/main/navigation/navigation.h | 8 +++++++- 4 files changed, 19 insertions(+), 8 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 78dea7a877..e45603fb1b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -149,6 +149,9 @@ tables: values: ["TYPE1", "TYPE2"] - name: off_on values: ["OFF", "ON"] + - name: nav_overrides_motor_stop + enum: navOverridesMotorStop_e + values: ["OFF", "AUTO_ONLY", "ALL_NAV"] groups: - name: PG_GYRO_CONFIG @@ -2101,10 +2104,10 @@ groups: min: 0 max: 5000 - name: nav_overrides_motor_stop - description: "Setting to OFF combined with MOTOR_STOP feature will allow user to stop motor when in autonomous modes. On most places this setting is likely to cause a stall." - default_value: "ON" - field: general.flags.auto_overrides_motor_stop - type: bool + description: "When set OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV" + default_value: "NOMS_ALL_NAV" + field: general.flags.nav_overrides_motor_stop + table: nav_overrides_motor_stop - name: nav_rth_climb_first description: "If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way." default_value: "ON" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a3e6aebd59..cc65c2b258 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -579,7 +579,9 @@ motorStatus_e getMotorStatus(void) } if (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) { - if ((STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) { + navOverridesMotorStop_e motorStopOverride = navConfig()->general.flags.nav_overrides_motor_stop; + if (!failsafeIsActive() && (STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) && + ((motorStopOverride == NOMS_OFF) || ((motorStopOverride == NOMS_ALL_NAV) && !navigationIsControllingThrottle()) || ((motorStopOverride == NOMS_AUTO_ONLY) && !navigationIsFlyingAutonomousMode()))) { return MOTOR_STOPPED_USER; } } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c16aa048eb..6cd74668cb 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -87,7 +87,7 @@ PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CO PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 8); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -102,7 +102,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .rth_tail_first = 0, .disarm_on_landing = 0, .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS, - .auto_overrides_motor_stop = 1, + .nav_overrides_motor_stop = NOMS_ALL_NAV, }, // General navigation parameters diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 1d8b853902..1983e17f73 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -124,6 +124,12 @@ typedef enum { NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR = 4, } navArmingBlocker_e; +typedef enum { + NOMS_OFF, + NOMS_AUTO_ONLY, + NOMS_ALL_NAV +} navOverridesMotorStop_e; + typedef struct positionEstimationConfig_s { uint8_t automatic_mag_declination; uint8_t reset_altitude_type; // from nav_reset_type_e @@ -175,7 +181,7 @@ typedef struct navConfig_s { uint8_t disarm_on_landing; // uint8_t rth_allow_landing; // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e. uint8_t rth_climb_ignore_emerg; // Option to ignore GPS loss on initial climb stage of RTH - uint8_t auto_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor + uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor } flags; uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) From 6600a9cde7873ee3ec64f8d2cb215ef1e1834444 Mon Sep 17 00:00:00 2001 From: mateksys Date: Tue, 8 Sep 2020 15:40:18 +0800 Subject: [PATCH 070/109] Add SDCARD support into MATEKF722PX --- src/main/target/MATEKF722PX/target.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/target/MATEKF722PX/target.h b/src/main/target/MATEKF722PX/target.h index f2f1bca242..bb4d0d6022 100755 --- a/src/main/target/MATEKF722PX/target.h +++ b/src/main/target/MATEKF722PX/target.h @@ -78,12 +78,19 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PC3 +//F722-PX,F722-HD #define USE_FLASHFS #define USE_FLASH_M25P16 #define M25P16_SPI_BUS BUS_SPI2 #define M25P16_CS_PIN PB12 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +//F722-WPX +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI2 +#define SDCARD_CS_PIN PC15 + // *************** UART ***************************** #define USE_VCP #define USB_DETECT_PIN PC14 From bb1d5d7f557097239df8e2d1ae3cbc55a6f10113 Mon Sep 17 00:00:00 2001 From: mateksys Date: Tue, 8 Sep 2020 15:49:35 +0800 Subject: [PATCH 071/109] Add softserial1 and 2 spare ADC into MATEKF765 --- src/main/target/MATEKF765/target.h | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/target/MATEKF765/target.h b/src/main/target/MATEKF765/target.h index 8fefd8d077..48dde29e1e 100644 --- a/src/main/target/MATEKF765/target.h +++ b/src/main/target/MATEKF765/target.h @@ -144,13 +144,13 @@ #define UART8_TX_PIN PE1 #define UART8_RX_PIN PE0 -/* -#define USE_SOFTSERIAL1 -#define SOFTSERIAL_1_TX_PIN PA2 -#define SOFTSERIAL_1_RX_PIN PA2 -*/ -#define SERIAL_PORT_COUNT 8 +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PC6 //TX6 pad +#define SOFTSERIAL_1_RX_PIN PC6 //TX6 pad + + +#define SERIAL_PORT_COUNT 9 #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS @@ -171,6 +171,8 @@ #define ADC_CHANNEL_2_PIN PC3 #define ADC_CHANNEL_3_PIN PC1 #define ADC_CHANNEL_4_PIN PC0 +#define ADC_CHANNEL_5_PIN PA4 //VBAT2 +#define ADC_CHANNEL_6_PIN PC5 //CURR2 #define VBAT_ADC_CHANNEL ADC_CHN_1 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 From 57004c8f0018681cc57a55f2f25837835af6b0be Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Sun, 1 Nov 2020 08:39:19 +0000 Subject: [PATCH 072/109] Create separate target for SDCARD default for MATEKF722WPX --- src/main/target/MATEKF722PX/CMakeLists.txt | 1 + src/main/target/MATEKF722PX/target.h | 28 ++++++++++++---------- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/main/target/MATEKF722PX/CMakeLists.txt b/src/main/target/MATEKF722PX/CMakeLists.txt index 0248902846..069c6db389 100644 --- a/src/main/target/MATEKF722PX/CMakeLists.txt +++ b/src/main/target/MATEKF722PX/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f722xe(MATEKF722PX) +target_stm32f722xe(MATEKF722WPX) diff --git a/src/main/target/MATEKF722PX/target.h b/src/main/target/MATEKF722PX/target.h index bb4d0d6022..24c471860e 100755 --- a/src/main/target/MATEKF722PX/target.h +++ b/src/main/target/MATEKF722PX/target.h @@ -21,7 +21,7 @@ #define TARGET_BOARD_IDENTIFIER "MF7P" #define USBD_PRODUCT_STRING "MATEKF722PX" -#define LED0 PA14 //Blue SWCLK +#define LED0 PA14 //Blue SWCLK #define LED1 PA13 //Green SWDIO #define BEEPER PC13 @@ -79,17 +79,20 @@ #define SPI2_MOSI_PIN PC3 //F722-PX,F722-HD -#define USE_FLASHFS -#define USE_FLASH_M25P16 -#define M25P16_SPI_BUS BUS_SPI2 -#define M25P16_CS_PIN PB12 -#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT - +#if defined(MATEKF722PX) + #define USE_FLASHFS + #define USE_FLASH_M25P16 + #define M25P16_SPI_BUS BUS_SPI2 + #define M25P16_CS_PIN PB12 + #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#else //F722-WPX -#define USE_SDCARD -#define USE_SDCARD_SPI -#define SDCARD_SPI_BUS BUS_SPI2 -#define SDCARD_CS_PIN PC15 + #define USE_SDCARD + #define USE_SDCARD_SPI + #define SDCARD_SPI_BUS BUS_SPI2 + #define SDCARD_CS_PIN PC15 + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#endif // *************** UART ***************************** #define USE_VCP @@ -119,7 +122,7 @@ #define USE_UART6 #define UART6_TX_PIN PC6 #define UART6_RX_PIN PC7 - + #define USE_SOFTSERIAL1 #define SOFTSERIAL_1_TX_PIN PA2 //TX2 pad #define SOFTSERIAL_1_RX_PIN NONE @@ -170,4 +173,3 @@ #define USE_DSHOT #define USE_SERIALSHOT #define USE_ESC_SENSOR - From dd782a0d7c8f546f338e3c04c8d6ef8427becd66 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Mon, 2 Nov 2020 09:59:49 +0000 Subject: [PATCH 073/109] fix incorrect error code F7 / sd mmc --- src/main/drivers/sdcard/sdmmc_sdio_f7xx.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c b/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c index d5b11946b2..e03d11acef 100644 --- a/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c +++ b/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c @@ -485,7 +485,7 @@ static SD_Error_t SD_InitializeCard(void) SD_GetResponse(SD_Handle.CID); } - if ((SD_CardType == SD_STD_CAPACITY_V1_1) || (SD_CardType == SD_STD_CAPACITY_V2_0) || + if ((SD_CardType == SD_STD_CAPACITY_V1_1) || (SD_CardType == SD_STD_CAPACITY_V2_0) || (SD_CardType == SD_SECURE_DIGITAL_IO_COMBO) || (SD_CardType == SD_HIGH_CAPACITY)) { // Send CMD3 SET_REL_ADDR with argument 0 // SD Card publishes its RCA. @@ -1000,7 +1000,7 @@ SD_Error_t SD_GetStatus(void) } } else { - ErrorState = SD_CARD_ERROR; + ErrorState = SD_ERROR; } return ErrorState; From 78cf9d580faa0b5aaf4fbf7180554f4f8f363c09 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 2 Nov 2020 11:46:27 +0000 Subject: [PATCH 074/109] Update Building in Windows 10 with Linux Subsystem.md Added fix for WSL v2 and the wsl.conf option for the fix. --- ...ding in Windows 10 with Linux Subsystem.md | 29 ++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/docs/development/Building in Windows 10 with Linux Subsystem.md b/docs/development/Building in Windows 10 with Linux Subsystem.md index 6a3a96f7ec..2b8439c543 100644 --- a/docs/development/Building in Windows 10 with Linux Subsystem.md +++ b/docs/development/Building in Windows 10 with Linux Subsystem.md @@ -96,7 +96,9 @@ make[1]: *** [CMakeFiles/Makefile2:33290: src/main/target/MATEKF722SE/CMakeFiles make: *** [Makefile:13703: MATEKF722SE] Error 2 ``` -This error can be triggered by a Windows PATHs included in the Linux Subsystem. The solution is to: +This error can be triggered by a Windows PATHs included in the Linux Subsystem. The solution is: + +#### For WSL V1 - Flags set as 7 by default 1. Open Windows RegEdit tool 1. Find `HKEY_CURRENT_USER\Software\Microsoft\Windows\CurrentVersion\Lxss\{GUID}\Flags` @@ -105,3 +107,28 @@ This error can be triggered by a Windows PATHs included in the Linux Subsystem. 1. `cd build` 1. `cmake ..` 1. `make {TARGET}` should be working again + +#### For WSL V2 - Flags set as 0x0000000f (15) by default +1. Open Windows RegEdit tool +1. Find `HKEY_CURRENT_USER\Software\Microsoft\Windows\CurrentVersion\Lxss\{GUID}\Flags` +1. Change `Flags` from `f` to `d`, it is stored as Base Hexadecimal +1. Restart WSL and Windows preferably +1. `cd build` +1. `cmake ..` +1. `make {TARGET}` should be working again + +#### Or, for either version +1. In the Linux Subsystem, `cd /etc/` +2. Create a new file with `sudo nano wsl.conf` +3. Enter the following in to the new file: +``` +[Interop] +appendWindowsPath=false +``` +4. Save the file by holding `Ctrl` and pressing `o` +5. Press `Enter` to confirm the wsl.conf filename. +6. Hit `Ctrl`+`x` to exit nano +7. Restart WSL and Windows preferably +8. `cd build` +9. `cmake ..` +9. `make {TARGET}` should be working again From 1cef921c924a60f7a8f6009c0a2629bc3f70161a Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Mon, 2 Nov 2020 22:46:00 +0100 Subject: [PATCH 075/109] Fix a bunch more compilation warnings --- src/main/common/filter.c | 41 ++++++++++++++++++----------- src/main/io/asyncfatfs/asyncfatfs.c | 4 +++ src/main/io/osd.c | 4 +++ src/main/io/osd_grid.c | 6 +---- src/main/io/smartport_master.c | 3 +++ 5 files changed, 37 insertions(+), 21 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 387d478d39..3502c09f46 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -148,16 +148,18 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp float b0, b1, b2; switch (filterType) { - case FILTER_LPF: - b0 = (1 - cs) / 2; - b1 = 1 - cs; - b2 = (1 - cs) / 2; - break; - case FILTER_NOTCH: - b0 = 1; - b1 = -2 * cs; - b2 = 1; - break; + case FILTER_LPF: + b0 = (1 - cs) / 2; + b1 = 1 - cs; + b2 = (1 - cs) / 2; + break; + case FILTER_NOTCH: + b0 = 1; + b1 = -2 * cs; + b2 = 1; + break; + default: + goto initError; } const float a0 = 1 + alpha; const float a1 = -2 * cs; @@ -172,16 +174,23 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp } else { // Not possible to filter frequencies above Nyquist frequency - passthrough - filter->b0 = 1.0f; - filter->b1 = 0.0f; - filter->b2 = 0.0f; - filter->a1 = 0.0f; - filter->a2 = 0.0f; + goto initError; } // zero initial samples filter->x1 = filter->x2 = 0; filter->y1 = filter->y2 = 0; + + return; + + initError: + + // passthrough + filter->b0 = 1.0f; + filter->b1 = 0.0f; + filter->b2 = 0.0f; + filter->a1 = 0.0f; + filter->a2 = 0.0f; } FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input) @@ -231,4 +240,4 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint filter->x2 = x2; filter->y1 = y1; filter->y2 = y2; -} \ No newline at end of file +} diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index 5f57a10b8b..0d84a756ca 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -1694,6 +1694,8 @@ static afatfsOperationStatus_e afatfs_appendSuperclusterContinue(afatfsFile_t *f // Update the fileSize/firstCluster in the directory entry for the file status = afatfs_saveDirectoryEntry(file, AFATFS_SAVE_DIRECTORY_NORMAL); break; + default: + status = AFATFS_OPERATION_FAILURE; } if ((status == AFATFS_OPERATION_FAILURE || status == AFATFS_OPERATION_SUCCESS) && file->operation.operation == AFATFS_FILE_OPERATION_APPEND_SUPERCLUSTER) { @@ -2487,6 +2489,8 @@ static afatfsOperationStatus_e afatfs_ftruncateContinue(afatfsFilePtr_t file, bo return AFATFS_OPERATION_SUCCESS; break; + default: + status = AFATFS_OPERATION_FAILURE; } if (status == AFATFS_OPERATION_FAILURE && file->operation.operation == AFATFS_FILE_OPERATION_TRUNCATE) { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 300d98fb86..293185685f 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -417,6 +417,7 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) centivalue = (ws * 224) / 100; suffix = SYM_MPH; break; + default: case OSD_UNIT_METRIC: centivalue = (ws * 36) / 10; suffix = SYM_KMH; @@ -1011,6 +1012,7 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente break; case OSD_UNIT_UK: FALLTHROUGH; + default: case OSD_UNIT_METRIC: initialScale = 10; // 10m as initial scale break; @@ -1844,6 +1846,7 @@ static bool osdDrawSingleElement(uint8_t item) value = CENTIMETERS_TO_CENTIFEET(value); sym = SYM_FTS; break; + default: case OSD_UNIT_METRIC: // Already in cm/s sym = SYM_MS; @@ -2325,6 +2328,7 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_UNIT_UK: FALLTHROUGH; + default: case OSD_UNIT_METRIC: scaleToUnit = 100; // scale to cm for osdFormatCentiNumber() scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m diff --git a/src/main/io/osd_grid.c b/src/main/io/osd_grid.c index fe8124b970..971fd1f106 100644 --- a/src/main/io/osd_grid.c +++ b/src/main/io/osd_grid.c @@ -225,7 +225,7 @@ static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *side // Scroll between SYM_AH_DECORATION_MIN and SYM_AH_DECORATION_MAX. // Zero scrolling should draw SYM_AH_DECORATION. uint8_t decoration = SYM_AH_DECORATION; - int offset; + int offset = 0; int steps; switch (scroll) { case OSD_SIDEBAR_SCROLL_NONE: @@ -240,8 +240,6 @@ static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *side case OSD_SIDEBAR_SCROLL_GROUND_SPEED: #if defined(USE_GPS) offset = gpsSol.groundSpeed; -#else - offset = 0; #endif // Move 1 char for every 20 cm/s steps = offset / 20; @@ -249,8 +247,6 @@ static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *side case OSD_SIDEBAR_SCROLL_HOME_DISTANCE: #if defined(USE_GPS) offset = GPS_distanceToHome; -#else - offset = 0; #endif // Move 1 char for every 5m steps = offset / 5; diff --git a/src/main/io/smartport_master.c b/src/main/io/smartport_master.c index cc51b4235d..95e1c74b22 100644 --- a/src/main/io/smartport_master.c +++ b/src/main/io/smartport_master.c @@ -300,6 +300,9 @@ static void smartportMasterPoll(void) break; } + default: // should not happen + return; + } } From 46f7c5af6ca1cebea28264c4a9d8aef49b467a91 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Tue, 3 Nov 2020 18:00:10 +0100 Subject: [PATCH 076/109] Avoid using goto in biquadFilterInit --- src/main/common/filter.c | 31 +++++++++++++++---------------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 3502c09f46..b3683c3edc 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -135,6 +135,17 @@ void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t s biquadFilterInit(filter, filterFreq, samplingIntervalUs, BIQUAD_Q, FILTER_LPF); } + +static void biquadFilterSetupPassthrough(biquadFilter_t *filter) +{ + // By default set as passthrough + filter->b0 = 1.0f; + filter->b1 = 0.0f; + filter->b2 = 0.0f; + filter->a1 = 0.0f; + filter->a2 = 0.0f; +} + void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType) { // Check for Nyquist frequency and if it's not possible to initialize filter as requested - set to no filtering at all @@ -159,7 +170,8 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp b2 = 1; break; default: - goto initError; + biquadFilterSetupPassthrough(filter); + return; } const float a0 = 1 + alpha; const float a1 = -2 * cs; @@ -171,26 +183,13 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp filter->b2 = b2 / a0; filter->a1 = a1 / a0; filter->a2 = a2 / a0; - } - else { - // Not possible to filter frequencies above Nyquist frequency - passthrough - goto initError; + } else { + biquadFilterSetupPassthrough(filter); } // zero initial samples filter->x1 = filter->x2 = 0; filter->y1 = filter->y2 = 0; - - return; - - initError: - - // passthrough - filter->b0 = 1.0f; - filter->b1 = 0.0f; - filter->b2 = 0.0f; - filter->a1 = 0.0f; - filter->a2 = 0.0f; } FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input) From ea59b0e5ec94f75047b8a980c79663089891e7e1 Mon Sep 17 00:00:00 2001 From: Tim O'Brien Date: Tue, 3 Nov 2020 19:15:07 -0800 Subject: [PATCH 077/109] Adds DJI OSD support to 2x F3 boards - SPRACINGF3 and AIRHEROF3 - both boards have no classic OSD - This patch works on master and 2.5.x --- src/main/target/AIRHEROF3/target.h | 4 ++++ src/main/target/SPRACINGF3/target.h | 5 +++++ 2 files changed, 9 insertions(+) diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h index 9356bf0cb2..74ba97293f 100755 --- a/src/main/target/AIRHEROF3/target.h +++ b/src/main/target/AIRHEROF3/target.h @@ -76,6 +76,10 @@ #define VBAT_ADC_CHANNEL ADC_CHN_1 #define AIRSPEED_ADC_CHANNEL ADC_CHN_2 +#define USE_DJI_HD_OSD +#define USE_OSD +#undef USE_CMS +#undef CMS_MENU_OSD /* #define USE_LED_STRIP diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 05727f5d42..3b506c86b4 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -93,6 +93,11 @@ #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 +#define USE_DJI_HD_OSD +#define USE_OSD +#undef USE_CMS +#undef CMS_MENU_OSD + #define USE_LED_STRIP #define WS2811_PIN PA8 From 8b1f7e3b1317c439ce68684c1a7c88dad85d8026 Mon Sep 17 00:00:00 2001 From: Airwide Date: Thu, 5 Nov 2020 08:47:52 +0100 Subject: [PATCH 078/109] Changed centidegrees to decidegrees in comments and docs --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation.c | 2 +- src/main/navigation/navigation.h | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 4b5148a7f1..33f42e97ad 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -272,7 +272,7 @@ | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | | nav_fw_pitch2thr_smoothing | 1 | Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch | -| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch before nav_fw_pitch2thr is applied [centidegrees] | +| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch before nav_fw_pitch2thr is applied [decidegrees] | | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d2d5dea858..d72179fbae 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2267,7 +2267,7 @@ groups: min: 1 max: 4096 - name: nav_fw_pitch2thr_threshold - description: "Threshold from zero pitch before pitch_to_throttle is applied [centidegrees]" + description: "Threshold from zero pitch before pitch_to_throttle is applied [decidegrees]" default_value: "0" field: fw.pitch_to_throttle_thresh min: 0 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 131e9784a2..0a3e401c8f 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -153,7 +153,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .min_throttle = 1200, .pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle) .pitch_to_throttle_smooth = 1, // Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch - .pitch_to_throttle_thresh = 0, // Threshold from average pitch before pitch_to_throttle is applied [centidegrees] + .pitch_to_throttle_thresh = 0, // Threshold from average pitch before pitch_to_throttle is applied [decidegrees] .loiter_radius = 5000, // 50m //Fixed wing landing diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index f49ee82b0d..116b97207e 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -223,7 +223,7 @@ typedef struct navConfig_s { uint16_t max_throttle; // Maximum allowed throttle in auto mode uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10) uint16_t pitch_to_throttle_smooth; // Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch - uint8_t pitch_to_throttle_thresh; // Threshold from zero pitch before pitch_to_throttle is applied [centidegrees] + uint8_t pitch_to_throttle_thresh; // Threshold from zero pitch before pitch_to_throttle is applied [decidegrees] uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing int8_t land_dive_angle; uint16_t launch_velocity_thresh; // Velocity threshold for swing launch detection From 0ba571ee7fcc172fb0cf1f962563e52b80e4b630 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Fri, 6 Nov 2020 16:04:35 -0500 Subject: [PATCH 079/109] Adds proper LQ Alarm Setting This adds CLI setting osd_link_quality_alarm for Crosfire --- src/main/cms/cms_menu_osd.c | 2 +- src/main/fc/settings.yaml | 10 ++++++++-- src/main/io/osd.c | 5 +++-- src/main/io/osd.h | 1 + 4 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index ff972556d6..874c5fde29 100755 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -144,7 +144,7 @@ static const OSD_Entry menuCrsfRxEntries[]= OSD_LABEL_ENTRY("-- CRSF RX --"), OSD_SETTING_ENTRY("LQ FORMAT", SETTING_OSD_CRSF_LQ_FORMAT), - OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_RSSI_ALARM), + OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_LINK_QUALITY_ALARM), OSD_SETTING_ENTRY("SNR ALARM LEVEL", SETTING_OSD_SNR_ALARM), OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_CRSF_RSSI_DBM), OSD_ELEMENT_ENTRY("RX LQ", OSD_CRSF_LQ), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 78dea7a877..eceac3e4bf 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2642,7 +2642,7 @@ groups: type: uint8_t - name: osd_rssi_alarm - description: "Value bellow which to make the OSD RSSI indicator blink" + description: "Value below which to make the OSD RSSI indicator blink" default_value: "20" field: rssi_alarm min: 0 @@ -2666,7 +2666,7 @@ groups: min: 0 max: 50000 - name: osd_neg_alt_alarm - description: "Value bellow which (negative altitude) to make the OSD relative altitude indicator blink (meters)" + description: "Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters)" default_value: "5" field: neg_alt_alarm min: 0 @@ -2739,6 +2739,12 @@ groups: field: snr_alarm min: -12 max: 8 + - name: osd_link_quality_alarm + description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%" + default_value: "70" + field: link_quality_alarm + min: 0 + max: 100 - name: osd_temp_label_align description: "Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT`" default_value: "LEFT" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 300d98fb86..c46bcb17fa 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1680,7 +1680,7 @@ static bool osdDrawSingleElement(uint8_t item) } if (!failsafeIsReceivingRxData()){ TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } else if (rxLinkStatistics.uplinkLQ < osdConfig()->rssi_alarm) { + } else if (rxLinkStatistics.uplinkLQ < osdConfig()->link_quality_alarm) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } break; @@ -2359,7 +2359,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWriteChar(osdDisplayPort, elemPosX, elemPosY + 1, referenceSymbol); return true; } - + case OSD_GVAR_0: { osdFormatGVar(buff, 0); @@ -2526,6 +2526,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, #ifdef USE_SERIALRX_CRSF .snr_alarm = 4, .crsf_lq_format = OSD_CRSF_LQ_TYPE1, + .link_quality_alarm = 70, #endif #ifdef USE_TEMPERATURE_SENSOR .temp_label_align = OSD_ALIGN_LEFT, diff --git a/src/main/io/osd.h b/src/main/io/osd.h index dba1d7ec31..b53d2c31c0 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -289,6 +289,7 @@ typedef struct osdConfig_s { float gforce_axis_alarm_max; #ifdef USE_SERIALRX_CRSF int16_t snr_alarm; //CRSF SNR alarm in dB + int8_t link_quality_alarm; #endif #ifdef USE_BARO int16_t baro_temp_alarm_min; From f15dee463cec4651820e385267420c8dadec5294 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sat, 7 Nov 2020 10:17:47 +0100 Subject: [PATCH 080/109] [GPS] Support for M9N module and other u-blox 9 receivers --- src/main/io/gps_ublox.c | 107 +++++++++++++++++++++++++++------------- 1 file changed, 74 insertions(+), 33 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 37510553f1..89a7cac8ec 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -274,6 +274,8 @@ enum { MSG_VELNED = 0x12, MSG_TIMEUTC = 0x21, MSG_SVINFO = 0x30, + MSG_NAV_SAT = 0x35, + MSG_NAV_SIG = 0x35, MSG_CFG_PRT = 0x00, MSG_CFG_RATE = 0x08, MSG_CFG_SET_RATE = 0x01, @@ -739,36 +741,15 @@ STATIC_PROTOTHREAD(gpsConfigure) // Configure UBX binary messages gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - if ((gpsState.gpsConfig->provider == GPS_UBLOX) || (gpsState.hwVersion < 70000)) { - configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_STATUS, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_SOL, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_VELNED, 1); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 10); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - - // This may fail on old UBLOX units, advance forward on both ACK and NAK - configureMSG(MSG_CLASS_UBX, MSG_PVT, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); - } - else { + // u-Blox 9 + // M9N does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence + if (gpsState.hwVersion >= 190000) { configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0); ptWait(_ack_state == UBX_ACK_GOT_ACK); configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0); ptWait(_ack_state == UBX_ACK_GOT_ACK); - configureMSG(MSG_CLASS_UBX, MSG_SOL, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); - configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0); ptWait(_ack_state == UBX_ACK_GOT_ACK); @@ -777,20 +758,80 @@ STATIC_PROTOTHREAD(gpsConfigure) configureMSG(MSG_CLASS_UBX, MSG_PVT, 1); ptWait(_ack_state == UBX_ACK_GOT_ACK); - } - configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0); - ptWait(_ack_state == UBX_ACK_GOT_ACK); + configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); - // Configure data rate - gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= 70000)) { - configureRATE(100); // 10Hz + configureMSG(MSG_CLASS_UBX, MSG_NAV_SIG, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + // u-Blox 9 receivers such as M9N can do 10Hz as well, but the number of used satellites will be restricted to 16. + // Not mentioned in the datasheet + configureRATE(200); + ptWait(_ack_state == UBX_ACK_GOT_ACK); } else { - configureRATE(200); // 5Hz + // u-Blox 6/7/8 + // u-Blox 6 doesn't support PVT, use legacy config + if ((gpsState.gpsConfig->provider == GPS_UBLOX) || (gpsState.hwVersion < 70000)) { + configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_STATUS, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_SOL, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_VELNED, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 10); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + // This may fail on old UBLOX units, advance forward on both ACK and NAK + configureMSG(MSG_CLASS_UBX, MSG_PVT, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); + + configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + // Configure data rate to 5HZ + configureRATE(200); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + } + // u-Blox 7-8 support PVT + else { + configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_SOL, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_PVT, 1); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0); + ptWait(_ack_state == UBX_ACK_GOT_ACK); + + if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= 70000)) { + configureRATE(100); // 10Hz + } + else { + configureRATE(200); // 5Hz + } + ptWait(_ack_state == UBX_ACK_GOT_ACK); + } } - ptWait(_ack_state == UBX_ACK_GOT_ACK); // Configure SBAS // If particular SBAS setting is not supported by the hardware we'll get a NAK, From 7bef93407e5d859233ae7c1b06b15a44c5b84297 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 7 Nov 2020 10:25:04 +0000 Subject: [PATCH 081/109] Update IDE - Visual Studio Code with Windows 10.md --- .../development/IDE - Visual Studio Code with Windows 10.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/development/IDE - Visual Studio Code with Windows 10.md b/docs/development/IDE - Visual Studio Code with Windows 10.md index 7dff2d7cf7..6095fec4e3 100644 --- a/docs/development/IDE - Visual Studio Code with Windows 10.md +++ b/docs/development/IDE - Visual Studio Code with Windows 10.md @@ -104,13 +104,13 @@ Edit `./.vscode/tasks.json` to enable Building with `Ctrl + Shift + B` keyboard } , { - "label": "CMAKE Update", + "label": "Install/Update CMAKE", "type": "shell", - "command": "cmake ..", + "command": "mkdir -p build && cd build && cmake ..", "group": "build", "problemMatcher": [], "options": { - "cwd": "${workspaceFolder}/build" + "cwd": "${workspaceFolder}" } } ] From 3fcbb4498c0aaaf52977da6b85a9b6d2ccaffb95 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sat, 7 Nov 2020 14:03:38 +0100 Subject: [PATCH 082/109] [GPS] Fix incorrect constant value; Add logging --- src/main/common/log.h | 1 + src/main/io/gps_ublox.c | 20 +++++++++++++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/main/common/log.h b/src/main/common/log.h index 1622063d35..080f36c357 100644 --- a/src/main/common/log.h +++ b/src/main/common/log.h @@ -30,6 +30,7 @@ typedef enum { LOG_TOPIC_POS_ESTIMATOR, // 8, mask = 256 LOG_TOPIC_VTX, // 9, mask = 512 LOG_TOPIC_OSD, // 10, mask = 1024 + LOG_TOPIC_GPS, // 11, mask = 2048 LOG_TOPIC_COUNT, } logTopic_e; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 89a7cac8ec..d94a8a46e3 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -35,6 +35,7 @@ #include "common/gps_conversion.h" #include "common/maths.h" #include "common/utils.h" +#include "common/log.h" #include "drivers/serial.h" #include "drivers/time.h" @@ -275,7 +276,7 @@ enum { MSG_TIMEUTC = 0x21, MSG_SVINFO = 0x30, MSG_NAV_SAT = 0x35, - MSG_NAV_SIG = 0x35, + MSG_NAV_SIG = 0x43, MSG_CFG_PRT = 0x00, MSG_CFG_RATE = 0x08, MSG_CFG_SET_RATE = 0x01, @@ -703,6 +704,7 @@ STATIC_PROTOTHREAD(gpsConfigure) gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); // Set dynamic model + LOG_D(GPS, "UBLOX: Configuring NAV5"); switch (gpsState.gpsConfig->dynModel) { case GPS_DYNMODEL_PEDESTRIAN: configureNAV5(UBX_DYNMODEL_PEDESTRIAN, UBX_FIXMODE_AUTO); @@ -720,6 +722,8 @@ STATIC_PROTOTHREAD(gpsConfigure) // Disable NMEA messages gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); + LOG_D(GPS, "UBLOX: Disable NMEA messages"); + configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GGA, 0); ptWait(_ack_state == UBX_ACK_GOT_ACK); @@ -744,6 +748,8 @@ STATIC_PROTOTHREAD(gpsConfigure) // u-Blox 9 // M9N does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence if (gpsState.hwVersion >= 190000) { + LOG_D(GPS, "UBLOX: Configuring UBX-9 messages"); + configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0); ptWait(_ack_state == UBX_ACK_GOT_ACK); @@ -774,6 +780,8 @@ STATIC_PROTOTHREAD(gpsConfigure) // u-Blox 6/7/8 // u-Blox 6 doesn't support PVT, use legacy config if ((gpsState.gpsConfig->provider == GPS_UBLOX) || (gpsState.hwVersion < 70000)) { + LOG_D(GPS, "UBLOX: Configuring UBX-6 messages"); + configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1); ptWait(_ack_state == UBX_ACK_GOT_ACK); @@ -802,6 +810,8 @@ STATIC_PROTOTHREAD(gpsConfigure) } // u-Blox 7-8 support PVT else { + LOG_D(GPS, "UBLOX: Configuring UBX-7 messages"); + configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0); ptWait(_ack_state == UBX_ACK_GOT_ACK); @@ -946,6 +956,14 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) ptWaitTimeout((gpsState.hwVersion != 0), GPS_CFG_CMD_TIMEOUT_MS); } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == 0); + if (gpsState.hwVersion == 0) { + LOG_E(GPS, "UBLOX: version detection timeout"); + } + else { + LOG_I(GPS, "UBLOX: detected HW: %u", (unsigned)gpsState.hwVersion); + } + + // Configure GPS ptSpawn(gpsConfigure); } From d8e8bcb3d8b60c3fcc53296d4b7c395c6f3a0df2 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 8 Nov 2020 08:59:37 +0000 Subject: [PATCH 083/109] Update Controls.md Fixes #6250 --- docs/Controls.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Controls.md b/docs/Controls.md index 7b3e870597..3757088db3 100644 --- a/docs/Controls.md +++ b/docs/Controls.md @@ -2,9 +2,9 @@ ## Arming -When armed, the aircraft is ready to fly and the motors will spin when throttle is applied. The motors will spin at a slow speed when armed (this feature may be disabled by setting MOTOR_STOP, but for safety reasons, that is not recommended). +When armed, the aircraft is ready to fly and the motors will spin when throttle is applied. With multirotors, the motors will spin at a slow speed when armed (this feature may be disabled by setting MOTOR_STOP, but for safety reasons, that is not recommended). -By default, arming and disarming is done using stick positions. (NOTE: this feature is disabled when using a switch to arm.) +Arming and disarming is done using a switch, set up on the modes page. (NOTE: Stick arming was removed in iNav 2.2) ## Stick Positions From 29d56bab3608dd935c87689d261bd97f9f94f2a4 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 8 Nov 2020 15:51:52 +0100 Subject: [PATCH 084/109] LC Operands update --- docs/Programming Framework.md | 4 ++++ src/main/programming/logic_condition.c | 25 +++++++++++++++++++++++++ src/main/programming/logic_condition.h | 23 ++++++++++++++--------- 3 files changed, 43 insertions(+), 9 deletions(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 80a7320c27..2034bc0da1 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -117,6 +117,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 29 | ACTIVE_WAYPOINT_INDEX | Indexed from `1`. To verify WP is in progress, use `IS_WP` | | 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph | | 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem | +| 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol | +| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol | ##### ACTIVE_WAYPOINT_ACTION @@ -144,6 +146,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 6 | ANGLE | | | 7 | HORIZON | | | 8 | AIR | | +| 9 | USER1 | | +| 10 | USER2 | | ### Flags diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 8c9d7c00de..b9b35d810d 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -36,6 +36,7 @@ #include "fc/fc_core.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "fc/rc_modes.h" #include "navigation/navigation.h" #include "sensors/battery.h" #include "sensors/pitotmeter.h" @@ -482,6 +483,22 @@ static int logicConditionGetFlightOperandValue(int operand) { return constrain(sqrtf(sq(GPS_distanceToHome) + sq(getEstimatedActualPosition(Z)/100)), 0, INT16_MAX); break; + case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ: + #ifdef USE_SERIALRX_CRSF + return rxLinkStatistics.uplinkLQ; + #else + return 0; + #endif + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR: + #ifdef USE_SERIALRX_CRSF + return rxLinkStatistics.uplinkSNR; + #else + return 0; + #endif + break; + default: return 0; break; @@ -528,6 +545,14 @@ static int logicConditionGetFlightModeOperandValue(int operand) { return (bool) FLIGHT_MODE(AIRMODE_ACTIVE); break; + case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1: + return IS_RC_MODE_ACTIVE(BOXUSER1); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2: + return IS_RC_MODE_ACTIVE(BOXUSER2); + break; + default: return 0; break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 0327fdb385..75905c51ca 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -113,18 +113,23 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_INDEX, // 29 LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_ACTION, // 30 LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 31 + LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 32 + LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 33 + } logicFlightOperands_e; typedef enum { - LOGIC_CONDITION_OPERAND_FLIGHT_MODE_FAILSAFE, - LOGIC_CONDITION_OPERAND_FLIGHT_MODE_MANUAL, - LOGIC_CONDITION_OPERAND_FLIGHT_MODE_RTH, - LOGIC_CONDITION_OPERAND_FLIGHT_MODE_POSHOLD, - LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE, - LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD, - LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLE, - LOGIC_CONDITION_OPERAND_FLIGHT_MODE_HORIZON, - LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR, + LOGIC_CONDITION_OPERAND_FLIGHT_MODE_FAILSAFE, // 0 + LOGIC_CONDITION_OPERAND_FLIGHT_MODE_MANUAL, // 1 + LOGIC_CONDITION_OPERAND_FLIGHT_MODE_RTH, // 2 + LOGIC_CONDITION_OPERAND_FLIGHT_MODE_POSHOLD, // 3 + LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE, // 4 + LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD, // 5 + LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLE, // 6 + LOGIC_CONDITION_OPERAND_FLIGHT_MODE_HORIZON, // 7 + LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR, // 8 + LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1, // 9 + LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2, // 10 } logicFlightModeOperands_e; typedef enum { From 2767297970db0e4a0dfd2a8602545b86f6a04035 Mon Sep 17 00:00:00 2001 From: Airwide Date: Wed, 11 Nov 2020 10:36:37 +0100 Subject: [PATCH 085/109] Full/unfiltered throttle correction outside of pitch deadband --- src/main/navigation/navigation_fixedwing.c | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 47c005b484..9935ce97b6 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -463,22 +463,18 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) int16_t fixedWingPitchToThrottleCorrection(int16_t pitch) { - // Calculate base throttle correction from pitch moving average const int16_t movingAverageCycles = navConfig()->fw.pitch_to_throttle_smooth; static int16_t averagePitch = 0; averagePitch = (averagePitch * movingAverageCycles + pitch - averagePitch) / movingAverageCycles; - const int16_t baseThrottleCorrection = DECIDEGREES_TO_DEGREES(averagePitch) * navConfig()->fw.pitch_to_throttle; - // Calculate final throttle correction - if (pitch > (averagePitch + navConfig()->fw.pitch_to_throttle_thresh)) { - return baseThrottleCorrection + DECIDEGREES_TO_DEGREES(pitch - averagePitch - navConfig()->fw.pitch_to_throttle_thresh) * navConfig()->fw.pitch_to_throttle; - } - else if (pitch < (averagePitch - navConfig()->fw.pitch_to_throttle_thresh)) { - return baseThrottleCorrection + DECIDEGREES_TO_DEGREES(pitch - averagePitch + navConfig()->fw.pitch_to_throttle_thresh) * navConfig()->fw.pitch_to_throttle; + if (pitch > (averagePitch + navConfig()->fw.pitch_to_throttle_thresh) || pitch < (averagePitch - navConfig()->fw.pitch_to_throttle_thresh)) { + // Unfiltered throttle correction outside of pitch deadband + return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle; } else { - return baseThrottleCorrection; + // Filtered throttle correction inside of pitch deadband + return DECIDEGREES_TO_DEGREES(averagePitch) * navConfig()->fw.pitch_to_throttle; } } From cc47513004fd3f8371ce8e49a3730e93ebc45930 Mon Sep 17 00:00:00 2001 From: Airwide Date: Wed, 11 Nov 2020 23:07:42 +0100 Subject: [PATCH 086/109] Changed to PT1 filter in fixedWingPitchToThrottleCorrection --- docs/Settings.md | 4 ++-- src/main/fc/settings.yaml | 10 ++++---- src/main/navigation/navigation.c | 4 ++-- src/main/navigation/navigation.h | 4 ++-- src/main/navigation/navigation_fixedwing.c | 27 ++++++++++++++++------ 5 files changed, 31 insertions(+), 18 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 33f42e97ad..61187b64ae 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -271,8 +271,8 @@ | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | -| nav_fw_pitch2thr_smoothing | 1 | Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch | -| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch before nav_fw_pitch2thr is applied [decidegrees] | +| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by nav_fw_pitch2thr_threshold | +| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d72179fbae..2e1bbc3660 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2261,13 +2261,13 @@ groups: min: 0 max: 100 - name: nav_fw_pitch2thr_smoothing - description: "Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch" - default_value: "1" + description: "How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh." + default_value: "0" field: fw.pitch_to_throttle_smooth - min: 1 - max: 4096 + min: 0 + max: 9 - name: nav_fw_pitch2thr_threshold - description: "Threshold from zero pitch before pitch_to_throttle is applied [decidegrees]" + description: "Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]" default_value: "0" field: fw.pitch_to_throttle_thresh min: 0 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 0a3e401c8f..04bce4647d 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -152,8 +152,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .max_throttle = 1700, .min_throttle = 1200, .pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle) - .pitch_to_throttle_smooth = 1, // Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch - .pitch_to_throttle_thresh = 0, // Threshold from average pitch before pitch_to_throttle is applied [decidegrees] + .pitch_to_throttle_smooth = 0, + .pitch_to_throttle_thresh = 0, .loiter_radius = 5000, // 50m //Fixed wing landing diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 116b97207e..cf835dbcd7 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -222,8 +222,8 @@ typedef struct navConfig_s { uint16_t min_throttle; // Minimum allowed throttle in auto mode uint16_t max_throttle; // Maximum allowed throttle in auto mode uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10) - uint16_t pitch_to_throttle_smooth; // Number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average pitch - uint8_t pitch_to_throttle_thresh; // Threshold from zero pitch before pitch_to_throttle is applied [decidegrees] + uint16_t pitch_to_throttle_smooth; // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. + uint8_t pitch_to_throttle_thresh; // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing int8_t land_dive_angle; uint16_t launch_velocity_thresh; // Velocity threshold for swing launch detection diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 9935ce97b6..0784373f91 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -79,6 +79,15 @@ static float getSmoothnessCutoffFreq(float baseFreq) return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f; } +// Calculates the cutoff frequency for smoothing out pitchToThrottleCorrection +// pitch_to_throttle_smooth valid range from 0 to 9 +// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz +static float getPitchToThrottleSmoothnessCutoffFreq(float baseFreq) +{ + uint16_t smoothness = 10 - navConfig()->fw.pitch_to_throttle_smooth; + return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f; +} + /*----------------------------------------------------------- * Altitude controller *-----------------------------------------------------------*/ @@ -461,20 +470,24 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) return throttleSpeedAdjustment; } -int16_t fixedWingPitchToThrottleCorrection(int16_t pitch) +int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs) { - const int16_t movingAverageCycles = navConfig()->fw.pitch_to_throttle_smooth; - static int16_t averagePitch = 0; + static timeUs_t previousTimePitchToThrCorr = 0; + const timeDelta_t deltaMicrosPitchToThrCorr = currentTimeUs - previousTimePitchToThrCorr; + previousTimePitchToThrCorr = currentTimeUs; - averagePitch = (averagePitch * movingAverageCycles + pitch - averagePitch) / movingAverageCycles; + static pt1Filter_t pitchToThrFilterState; - if (pitch > (averagePitch + navConfig()->fw.pitch_to_throttle_thresh) || pitch < (averagePitch - navConfig()->fw.pitch_to_throttle_thresh)) { + // Apply low-pass filter to pitch angle to smooth throttle correction + filteredPitch = pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr)); + + if (abs(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) { // Unfiltered throttle correction outside of pitch deadband return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle; } else { // Filtered throttle correction inside of pitch deadband - return DECIDEGREES_TO_DEGREES(averagePitch) * navConfig()->fw.pitch_to_throttle; + return DECIDEGREES_TO_DEGREES(filteredPitch) * navConfig()->fw.pitch_to_throttle; } } @@ -497,7 +510,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat // PITCH >0 dive, <0 climb int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle)); rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]); - int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection); + int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs); #ifdef NAV_FIXED_WING_LANDING if (navStateFlags & NAV_CTL_LAND) { From dab9f95c696d6581f85c7c2d8387dbf19a39879f Mon Sep 17 00:00:00 2001 From: Airwide Date: Thu, 12 Nov 2020 03:02:03 +0100 Subject: [PATCH 087/109] fixedWingPitchToThrottleCorrection definition bugfix --- src/main/flight/rth_estimator.c | 2 +- src/main/navigation/navigation.h | 2 +- src/main/navigation/navigation_fixedwing.c | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index d9897c32c7..4ddc719037 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -77,7 +77,7 @@ static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) { // pitch in degrees // output in Watt static float estimatePitchPower(float pitch) { - int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch)); + int16_t altitudeChangeThrottle = (int16_t)pitch * navConfig()->fw.pitch_to_throttle; altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle); const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue()); return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index cf835dbcd7..fea8b8e7b6 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -463,7 +463,7 @@ bool loadNonVolatileWaypointList(void); bool saveNonVolatileWaypointList(void); float getFinalRTHAltitude(void); -int16_t fixedWingPitchToThrottleCorrection(int16_t pitch); +int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs); /* Geodetic functions */ typedef enum { diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 0784373f91..c9a63653e0 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -479,9 +479,9 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs static pt1Filter_t pitchToThrFilterState; // Apply low-pass filter to pitch angle to smooth throttle correction - filteredPitch = pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr)); + int16_t filteredPitch = (int16_t)pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr)); - if (abs(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) { + if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) { // Unfiltered throttle correction outside of pitch deadband return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle; } From 2131cf92bf0136d31ce4c6d6c3a116068e5857e7 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Fri, 13 Nov 2020 15:18:34 +0000 Subject: [PATCH 088/109] [DOC] add some basic information about SRXL2 (#6284) * [DOC] add some basic information about SRXL2 (Rx.md) --- docs/Rx.md | 75 +++++++++++++++++++++++++++++++++++++++++------------- 1 file changed, 58 insertions(+), 17 deletions(-) diff --git a/docs/Rx.md b/docs/Rx.md index 569b9e3112..586e6ece42 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -33,6 +33,8 @@ http://www.frsky-rc.com/product/pro.php?pro_id=21 ### Spektrum +This section describes the legacy Spektrum satellite capability; the newer SRXL2 protocol is described [later in this document](#srxl2) . + 8 channels via serial currently supported. These receivers are reported working: @@ -47,10 +49,10 @@ As of iNav 1.6, a pseudo RSSI, based on satellite fade count is supported and re * Bind the satellite receiver using a physical RX; the bind function provided by the flight controller is not sufficient. * The CLI variable `rssi_channel` is set to channel 9: -```` +``` set rssi_channel = 9 -```` -This pseudo-RSSI should work on all makes of Spektrum satellite RX; it is tested as working on Lemon RX satellites http://www.lemon-rx.com/index.php?route=product/product&path=72&product_id=109 and http://www.lemon-rx.com/index.php?route=product/product&path=72&product_id=135 (recommended). +``` +This pseudo-RSSI should work on all makes of Spektrum satellite RX; it is tested as working on [Lemon RX satellites](http://www.lemon-rx.com/index.php?route=product/product&path=72&product_id=109 and http://www.lemon-rx.com/index.php?route=product/product&path=72&product_id=135) (recommended). ### S.BUS @@ -170,6 +172,46 @@ After flash "10ch Timer Mod i6 Updater", it is passible to get RSSI signal on se It is possible to use IBUS RX and IBUS telemetry on only one port of the hardware UART. More information in Telemetry.md.obj/inav_2.2.2_SPRACINGF3EVO.hex +### SRXL2 + +SRXL2 is a newer Spektrum protocol that provides a bidirectional link between the FC and the receiver, allowing the user to get FC telemetry data and basic settings on Spektrum Gen 2 airware TX. SRXL2 is supported in inav 2.6 and later. It offers improved performance and features compared to earlier Spektrum RX. + +#### Wiring + +Signal pin on receiver (labeled "S") must be wired to a **UART TX** pin on the FC. Voltage can be 3.3V (4.0V for SPM4651T) to 8.4V. On some F4 FCs, the TX pin may have a signal inverter (such as for S.Port). Make sure this isn't the case for the pin you intend to use. + +#### Configuration + +Selection of SXRL2 is provided in the inav 2.6 and later configurators. It is necessary to complete the configuration via the CLI; the following settings are recommended: + +``` +feature TELEMETRY +feature -RSSI_ADC +map TAER +set receiver_type = SERIAL +set serialrx_provider = SRXL2 +set serialrx_inverted = OFF +set srxl2_unit_id = 1 +set srxl2_baud_fast = ON +set rssi_source = PROTOCOL +set rssi_channel = 0 +``` + +#### Notes: + +* RSSI_ADC is disabled, as this would override the value provided through SRXL2 +* `rssi_channel = 0` is required, unlike earlier Spektrum devices (e.g. SPM4649T). + +Setting these values differently may have an adverse effects on RSSI readings. + +#### CLI Bind Command + +This command will put the receiver into bind mode without the need to reboot the FC as it was required with the older `spektrum_sat_bind` command. + +``` +bind_rx +``` + ## MultiWii serial protocol (MSP) Allows you to use MSP commands as the RC input. Only 8 channel support to maintain compatibility with MSP. @@ -215,21 +257,20 @@ To setup spectrum in the GUI: 2. Move to the "Configuration" page and in the upper lefthand corner choose Serial RX as the receiver type. 3. Below that choose the type of serial receiver that you are using. Save and reboot. -Using CLI: -For Serial RX enable `RX_SERIAL` and set the `serialrx_provider` CLI setting as follows. +#### Using CLI: -| Serial RX Provider | Value | -| ------------------ | ----- | -| SPEKTRUM1024 | 0 | -| SPEKTRUM2048 | 1 | -| SBUS | 2 | -| SUMD | 3 | -| SUMH | 4 | -| XBUS_MODE_B | 5 | -| XBUS_MODE_B_RJ01 | 6 | -| SERIALRX_IBUS | 7 | -| SERIALRX_JETIEXBUS | 8 | -| SERIALRX_CRSF | 9 | +For Serial RX set the `receiver_type` and `serialrx_provider` setting as appropriate for your RX. + +``` +# get rec +receiver_type = SERIAL +Allowed values: NONE, PPM, SERIAL, MSP, SPI, UIB + +# get serialrx +serialrx_provider = SBUS +Allowed values: SPEK1024, SPEK2048, SBUS, SUMD, SUMH, XB-B, XB-B-RJ01, IBUS, JETIEXBUS, CRSF, FPORT, SBUS_FAST, FPORT2, SRXL2 + +``` ### PPM/PWM input filtering. From e7a8cb4aad45c928a117481365d5637156f3da4f Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Fri, 13 Nov 2020 17:31:25 +0000 Subject: [PATCH 089/109] Update beeper.h Added BEEPER_LAUNCH_MODE_LOW_THROTTLE --- src/main/io/beeper.h | 49 ++++++++++++++++++++++---------------------- 1 file changed, 25 insertions(+), 24 deletions(-) diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 5dcff77f0a..0fcc8249dd 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -21,33 +21,34 @@ typedef enum { // IMPORTANT: these are in priority order, 0 = Highest - BEEPER_SILENCE = 0, // Silence, see beeperSilence() + BEEPER_SILENCE = 0, // Silence, see beeperSilence() BEEPER_RUNTIME_CALIBRATION_DONE, - BEEPER_HARDWARE_FAILURE, // HW failure - BEEPER_RX_LOST, // Beeps when TX is turned off or signal lost (repeat until TX is okay) - BEEPER_RX_LOST_LANDING, // Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm) - BEEPER_DISARMING, // Beep when disarming the board - BEEPER_ARMING, // Beep when arming the board - BEEPER_ARMING_GPS_FIX, // Beep a special tone when arming the board and GPS has fix - BEEPER_BAT_CRIT_LOW, // Longer warning beeps when battery is critically low (repeats) - BEEPER_BAT_LOW, // Warning beeps when battery is getting low (repeats) - BEEPER_GPS_STATUS, // FIXME **** Disable beeper when connected to USB **** - BEEPER_RX_SET, // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled - BEEPER_ACTION_SUCCESS, // Action success (various actions) - BEEPER_ACTION_FAIL, // Action fail (varions actions) - BEEPER_READY_BEEP, // Ring a tone when GPS is locked and ready - BEEPER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'. - BEEPER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position - BEEPER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased) - BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on - BEEPER_USB, // Some boards have beeper powered USB connected - BEEPER_LAUNCH_MODE_ENABLED, // Fixed-wing launch mode enabled - BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated - BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop + BEEPER_HARDWARE_FAILURE, // HW failure + BEEPER_RX_LOST, // Beeps when TX is turned off or signal lost (repeat until TX is okay) + BEEPER_RX_LOST_LANDING, // Beeps SOS when armed and TX is turned off or signal lost (autolanding/autodisarm) + BEEPER_DISARMING, // Beep when disarming the board + BEEPER_ARMING, // Beep when arming the board + BEEPER_ARMING_GPS_FIX, // Beep a special tone when arming the board and GPS has fix + BEEPER_BAT_CRIT_LOW, // Longer warning beeps when battery is critically low (repeats) + BEEPER_BAT_LOW, // Warning beeps when battery is getting low (repeats) + BEEPER_GPS_STATUS, // FIXME **** Disable beeper when connected to USB **** + BEEPER_RX_SET, // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled + BEEPER_ACTION_SUCCESS, // Action success (various actions) + BEEPER_ACTION_FAIL, // Action fail (varions actions) + BEEPER_READY_BEEP, // Ring a tone when GPS is locked and ready + BEEPER_MULTI_BEEPS, // Internal value used by 'beeperConfirmationBeeps()'. + BEEPER_DISARM_REPEAT, // Beeps sounded while stick held in disarm position + BEEPER_ARMED, // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased) + BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on + BEEPER_USB, // Some boards have beeper powered USB connected + BEEPER_LAUNCH_MODE_ENABLED, // Fixed-wing launch mode enabled + BEEPER_LAUNCH_MODE_LOW_THROTTLE, // Fixed-wing launch mode enabled, but throttle is low + BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated + BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop - BEEPER_ALL, // Turn ON or OFF all beeper conditions - BEEPER_PREFERENCE, // Save preferred beeper configuration + BEEPER_ALL, // Turn ON or OFF all beeper conditions + BEEPER_PREFERENCE, // Save preferred beeper configuration // BEEPER_ALL and BEEPER_PREFERENCE must remain at the bottom of this enum } beeperMode_e; From 5331fd216a3b0a9fcc3c66787cfd6bdbf770fc3a Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Fri, 13 Nov 2020 17:33:04 +0000 Subject: [PATCH 090/109] Update beeper.c Added table entry and sequence for BEEPER_LAUNCH_MODE_LOW_THROTTLE --- src/main/io/beeper.c | 53 ++++++++++++++++++++++++-------------------- 1 file changed, 29 insertions(+), 24 deletions(-) diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 4306d101f5..e24216cda0 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -121,6 +121,10 @@ static const uint8_t beep_runtimeCalibrationDone[] = { static const uint8_t beep_launchModeBeep[] = { 5, 5, 5, 100, BEEPER_COMMAND_STOP }; +// Two short beeps, then one shorter beep. Beeps to show the throttle needs to be raised. Will repeat every second while the throttle is low. +static const uint8_t beep_launchModeLowThrottleBeep[] = { + 5, 5, 5, 5, 3, 100, BEEPER_COMMAND_STOP +}; // short beeps static const uint8_t beep_hardwareFailure[] = { 10, 10, BEEPER_COMMAND_STOP @@ -164,31 +168,32 @@ typedef struct beeperTableEntry_s { #define BEEPER_ENTRY(a,b,c,d) a,b,c,d /*static*/ const beeperTableEntry_t beeperTable[] = { - { BEEPER_ENTRY(BEEPER_RUNTIME_CALIBRATION_DONE, 0, beep_runtimeCalibrationDone, "RUNTIME_CALIBRATION") }, - { BEEPER_ENTRY(BEEPER_HARDWARE_FAILURE , 1, beep_hardwareFailure, "HW_FAILURE") }, - { BEEPER_ENTRY(BEEPER_RX_LOST, 2, beep_txLostBeep, "RX_LOST") }, - { BEEPER_ENTRY(BEEPER_RX_LOST_LANDING, 3, beep_sos, "RX_LOST_LANDING") }, - { BEEPER_ENTRY(BEEPER_DISARMING, 4, beep_disarmBeep, "DISARMING") }, - { BEEPER_ENTRY(BEEPER_ARMING, 5, beep_armingBeep, "ARMING") }, - { BEEPER_ENTRY(BEEPER_ARMING_GPS_FIX, 6, beep_armedGpsFix, "ARMING_GPS_FIX") }, - { BEEPER_ENTRY(BEEPER_BAT_CRIT_LOW, 7, beep_critBatteryBeep, "BAT_CRIT_LOW") }, - { BEEPER_ENTRY(BEEPER_BAT_LOW, 8, beep_lowBatteryBeep, "BAT_LOW") }, - { BEEPER_ENTRY(BEEPER_GPS_STATUS, 9, beep_multiBeeps, "GPS_STATUS") }, - { BEEPER_ENTRY(BEEPER_RX_SET, 10, beep_shortBeep, "RX_SET") }, - { BEEPER_ENTRY(BEEPER_ACTION_SUCCESS, 11, beep_2shortBeeps, "ACTION_SUCCESS") }, - { BEEPER_ENTRY(BEEPER_ACTION_FAIL, 12, beep_2longerBeeps, "ACTION_FAIL") }, - { BEEPER_ENTRY(BEEPER_READY_BEEP, 13, beep_readyBeep, "READY_BEEP") }, - { BEEPER_ENTRY(BEEPER_MULTI_BEEPS, 14, beep_multiBeeps, "MULTI_BEEPS") }, // FIXME having this listed makes no sense since the beep array will not be initialised. - { BEEPER_ENTRY(BEEPER_DISARM_REPEAT, 15, beep_disarmRepeatBeep, "DISARM_REPEAT") }, - { BEEPER_ENTRY(BEEPER_ARMED, 16, beep_armedBeep, "ARMED") }, - { BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 17, NULL, "SYSTEM_INIT") }, - { BEEPER_ENTRY(BEEPER_USB, 18, NULL, "ON_USB") }, - { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_ENABLED, 19, beep_launchModeBeep, "LAUNCH_MODE") }, - { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 20, beep_camOpenBeep, "CAM_CONNECTION_OPEN") }, - { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 21, beep_camCloseBeep, "CAM_CONNECTION_CLOSED") }, + { BEEPER_ENTRY(BEEPER_RUNTIME_CALIBRATION_DONE, 0, beep_runtimeCalibrationDone, "RUNTIME_CALIBRATION") }, + { BEEPER_ENTRY(BEEPER_HARDWARE_FAILURE , 1, beep_hardwareFailure, "HW_FAILURE") }, + { BEEPER_ENTRY(BEEPER_RX_LOST, 2, beep_txLostBeep, "RX_LOST") }, + { BEEPER_ENTRY(BEEPER_RX_LOST_LANDING, 3, beep_sos, "RX_LOST_LANDING") }, + { BEEPER_ENTRY(BEEPER_DISARMING, 4, beep_disarmBeep, "DISARMING") }, + { BEEPER_ENTRY(BEEPER_ARMING, 5, beep_armingBeep, "ARMING") }, + { BEEPER_ENTRY(BEEPER_ARMING_GPS_FIX, 6, beep_armedGpsFix, "ARMING_GPS_FIX") }, + { BEEPER_ENTRY(BEEPER_BAT_CRIT_LOW, 7, beep_critBatteryBeep, "BAT_CRIT_LOW") }, + { BEEPER_ENTRY(BEEPER_BAT_LOW, 8, beep_lowBatteryBeep, "BAT_LOW") }, + { BEEPER_ENTRY(BEEPER_GPS_STATUS, 9, beep_multiBeeps, "GPS_STATUS") }, + { BEEPER_ENTRY(BEEPER_RX_SET, 10, beep_shortBeep, "RX_SET") }, + { BEEPER_ENTRY(BEEPER_ACTION_SUCCESS, 11, beep_2shortBeeps, "ACTION_SUCCESS") }, + { BEEPER_ENTRY(BEEPER_ACTION_FAIL, 12, beep_2longerBeeps, "ACTION_FAIL") }, + { BEEPER_ENTRY(BEEPER_READY_BEEP, 13, beep_readyBeep, "READY_BEEP") }, + { BEEPER_ENTRY(BEEPER_MULTI_BEEPS, 14, beep_multiBeeps, "MULTI_BEEPS") }, // FIXME having this listed makes no sense since the beep array will not be initialised. + { BEEPER_ENTRY(BEEPER_DISARM_REPEAT, 15, beep_disarmRepeatBeep, "DISARM_REPEAT") }, + { BEEPER_ENTRY(BEEPER_ARMED, 16, beep_armedBeep, "ARMED") }, + { BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 17, NULL, "SYSTEM_INIT") }, + { BEEPER_ENTRY(BEEPER_USB, 18, NULL, "ON_USB") }, + { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_ENABLED, 19, beep_launchModeBeep, "LAUNCH_MODE") }, + { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_LOW_THROTTLE, 20, beep_launchModeLowThrottleBeep, "LAUNCH_MODE_LOW_THROTTLE") }, + { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 21, beep_camOpenBeep, "CAM_CONNECTION_OPEN") }, + { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 22, beep_camCloseBeep, "CAM_CONNECTION_CLOSED") }, - { BEEPER_ENTRY(BEEPER_ALL, 22, NULL, "ALL") }, - { BEEPER_ENTRY(BEEPER_PREFERENCE, 23, NULL, "PREFERED") }, + { BEEPER_ENTRY(BEEPER_ALL, 23, NULL, "ALL") }, + { BEEPER_ENTRY(BEEPER_PREFERENCE, 24, NULL, "PREFERED") }, }; static const beeperTableEntry_t *currentBeeperEntry = NULL; From f38b2c02ab9ac87be1b6c2ba93cc0cc4f74dfc64 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Fri, 13 Nov 2020 17:34:39 +0000 Subject: [PATCH 091/109] Update navigation_fw_launch.c Added logic to play the BEEPER_LAUNCH_MODE_LOW_THROTTLE when the throttle is low and launch mode is active. --- src/main/navigation/navigation_fw_launch.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 843d27f650..309bef0cf0 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -226,7 +226,11 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs) // Control beeper if (!launchState.launchFinished) { - beeper(BEEPER_LAUNCH_MODE_ENABLED); + if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) { + beeper(BEEPER_LAUNCH_MODE_ENABLED); + } else { + beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); + } } // Lock out controls From 2dfa42dc2cfac8a098554add1b8ff5a2d50d8804 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sat, 14 Nov 2020 16:42:51 +0100 Subject: [PATCH 092/109] [DJI] Add ESC temperature to DJI OSD. Add overrides to show IMU/BARO temp instead; Add a protocol workaround because DJI is using a non-standard MSP_ESC_SENSOR_DATA --- src/main/config/parameter_group_ids.h | 3 +- src/main/fc/settings.yaml | 24 ++++++ src/main/io/osd_dji_hd.c | 103 +++++++++++++++++++++++--- src/main/io/osd_dji_hd.h | 20 +++++ 4 files changed, 139 insertions(+), 11 deletions(-) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 67d86a1b9e..47c1faeb05 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -114,7 +114,8 @@ #define PG_SMARTPORT_MASTER_CONFIG 1024 #define PG_OSD_LAYOUTS_CONFIG 1025 #define PG_SAFE_HOME_CONFIG 1026 -#define PG_INAV_END 1026 +#define PG_DJI_OSD_CONFIG 1027 +#define PG_INAV_END 1027 // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e0e04d7471..15a187f9bd 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -149,6 +149,9 @@ tables: values: ["TYPE1", "TYPE2"] - name: off_on values: ["OFF", "ON"] + - name: djiOsdTempSource + values: ["ESC", "IMU", "BARO"] + enum: djiOsdTempSource_e groups: - name: PG_GYRO_CONFIG @@ -3126,6 +3129,7 @@ groups: max: UINT32_MAX description: "Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage." default_value: "0" + - name: PG_ESC_SENSOR_CONFIG type: escSensorConfig_t headers: ["sensors/esc_sensor.h"] @@ -3146,3 +3150,23 @@ groups: - name: smartport_master_inverted field: inverted type: bool + + - name: PG_DJI_OSD_CONFIG + type: djiOsdConfig_t + headers: ["io/osd_dji_hd.h"] + condition: USE_DJI_HD_OSD + members: + - name: dji_workarounds + description: "Enables workarounds for different versions of MSP protocol used" + field: proto_workarounds + - name: dji_use_name_for_messages + description: "Re-purpose the craft name field for messages" + default_value: "ON" + field: use_name_for_messages + type: bool + - name: dji_esc_temp_source + description: "Re-purpose the ESC temperature field for IMU/BARO temperature" + default_value: "ESC" + field: esc_temperature_source + table: djiOsdTempSource + type: uint8_t diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 7c2d03e9d7..b92b6dcd38 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -39,6 +39,9 @@ #include "common/time.h" #include "common/crc.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "fc/fc_core.h" #include "fc/config.h" #include "fc/controlrate_profile.h" @@ -64,6 +67,7 @@ #include "sensors/rangefinder.h" #include "sensors/acceleration.h" #include "sensors/esc_sensor.h" +#include "sensors/temperature.h" #include "msp/msp.h" #include "msp/msp_protocol.h" @@ -111,6 +115,13 @@ * but reuse the packet decoder to minimize code duplication */ +PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 1); +PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, + .use_name_for_messages = true, + .esc_temperature_source = DJI_OSD_TEMP_ESC, + .proto_workarounds = DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA, +); + // External dependency on looptime extern timeDelta_t cycleTime; @@ -167,7 +178,7 @@ const djiOsdMapping_t djiOSDItemIndexMap[] = { { OSD_HEADING, 0 }, // DJI: OSD_NUMERICAL_HEADING { OSD_VARIO_NUM, 0 }, // DJI: OSD_NUMERICAL_VARIO { -1, 0 }, // DJI: OSD_COMPASS_BAR - { -1, 0 }, // DJI: OSD_ESC_TMP + { OSD_ESC_TEMPERATURE, 0 }, // DJI: OSD_ESC_TEMPERATURE { OSD_ESC_RPM, 0 }, // DJI: OSD_ESC_RPM { OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH, FEATURE_CURRENT_METER }, // DJI: OSD_REMAINING_TIME_ESTIMATE { OSD_RTC_TIME, 0 }, // DJI: OSD_RTC_DATETIME @@ -1025,21 +1036,93 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms } break; -#if defined(USE_ESC_SENSOR) case DJI_MSP_ESC_SENSOR_DATA: - if (STATE(ESC_SENSOR_ENABLED)) { - sbufWriteU8(dst, getMotorCount()); - for (int i = 0; i < getMotorCount(); i++) { - const escSensorData_t * escSensor = getEscTelemetry(i); - sbufWriteU8(dst, escSensor->temperature); - sbufWriteU16(dst, escSensor->rpm); + if (djiOsdConfig()->proto_workarounds & DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA) { + // Version 1.00.06 of DJI firmware is not using the standard MSP_ESC_SENSOR_DATA + uint16_t protoRpm = 0; + int16_t protoTemp = 0; + +#if defined(USE_ESC_SENSOR) + if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) { + uint32_t motorRpmAcc = 0; + int32_t motorTempAcc = 0; + + for (int i = 0; i < getMotorCount(); i++) { + const escSensorData_t * escSensor = getEscTelemetry(i); + motorRpmAcc += escSensor->rpm; + motorTempAcc += escSensor->temperature; + } + + protoRpm = motorRpmAcc / getMotorCount(); + protoTemp = motorTempAcc / getMotorCount(); } +#endif + + switch (djiOsdConfig()->esc_temperature_source) { + // This is ESC temperature (as intended) + case DJI_OSD_TEMP_ESC: + // No-op, temperature is already set to ESC + break; + + // Re-purpose the field for core temperature + case DJI_OSD_TEMP_CORE: + getIMUTemperature(&protoTemp); + protoTemp = protoTemp / 10; + break; + + // Re-purpose the field for baro temperature + case DJI_OSD_TEMP_BARO: + getBaroTemperature(&protoTemp); + protoTemp = protoTemp / 10; + break; + } + + // No motor count, just raw temp and RPM data + sbufWriteU8(dst, protoTemp); + sbufWriteU16(dst, protoRpm); } else { - reply->result = MSP_RESULT_ERROR; + // Use standard MSP_ESC_SENSOR_DATA message + sbufWriteU8(dst, getMotorCount()); + for (int i = 0; i < getMotorCount(); i++) { + uint16_t motorRpm = 0; + int16_t motorTemp = 0; + + // If ESC_SENSOR is enabled, pull the telemetry data and get motor RPM +#if defined(USE_ESC_SENSOR) + if (STATE(ESC_SENSOR_ENABLED)) { + const escSensorData_t * escSensor = getEscTelemetry(i); + motorRpm = escSensor->rpm; + motorTemp = escSensor->temperature; + } +#endif + + // Now populate temperature field (which we may override for different purposes) + switch (djiOsdConfig()->esc_temperature_source) { + // This is ESC temperature (as intended) + case DJI_OSD_TEMP_ESC: + // No-op, temperature is already set to ESC + break; + + // Re-purpose the field for core temperature + case DJI_OSD_TEMP_CORE: + getIMUTemperature(&motorTemp); + motorTemp = motorTemp / 10; + break; + + // Re-purpose the field for baro temperature + case DJI_OSD_TEMP_BARO: + getBaroTemperature(&motorTemp); + motorTemp = motorTemp / 10; + break; + } + + // Add data for this motor to the packet + sbufWriteU8(dst, motorTemp); + sbufWriteU16(dst, motorRpm); + } } break; -#endif case DJI_MSP_OSD_CONFIG: #if defined(USE_OSD) diff --git a/src/main/io/osd_dji_hd.h b/src/main/io/osd_dji_hd.h index d1105783d3..d105b275b6 100644 --- a/src/main/io/osd_dji_hd.h +++ b/src/main/io/osd_dji_hd.h @@ -29,6 +29,8 @@ #include "msp/msp.h" #include "msp/msp_serial.h" +#include "config/parameter_group.h" + #if defined(USE_DJI_HD_OSD) #define DJI_API_VERSION_MAJOR 1 @@ -60,6 +62,24 @@ #define DJI_MSP_SET_PID 202 #define DJI_MSP_SET_RC_TUNING 204 +enum djiOsdTempSource_e { + DJI_OSD_TEMP_ESC = 0, + DJI_OSD_TEMP_CORE = 1, + DJI_OSD_TEMP_BARO = 2 +}; + +enum djiOsdProtoWorkarounds_e { + DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA = 1 << 0, +}; + +typedef struct djiOsdConfig_s { + uint8_t use_name_for_messages; + uint8_t esc_temperature_source; + uint8_t proto_workarounds; +} djiOsdConfig_t; + +PG_DECLARE(djiOsdConfig_t, djiOsdConfig); + void djiOsdSerialInit(void); void djiOsdSerialProcess(void); From 3f93c7d9509aa366b613da8aed387cf06144802d Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sat, 14 Nov 2020 17:04:42 +0100 Subject: [PATCH 093/109] [DJI] Refactoring DJI craft name override to show informational messages --- src/main/fc/settings.yaml | 2 +- src/main/io/osd_dji_hd.c | 424 ++++++++++++++++++++------------------ 2 files changed, 227 insertions(+), 199 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 15a187f9bd..dd1e5e1f49 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3160,7 +3160,7 @@ groups: description: "Enables workarounds for different versions of MSP protocol used" field: proto_workarounds - name: dji_use_name_for_messages - description: "Re-purpose the craft name field for messages" + description: "Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance" default_value: "ON" field: use_name_for_messages type: bool diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index b92b6dcd38..423108928b 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -91,14 +91,17 @@ #define DJI_OSD_WARNING_COUNT 16 #define DJI_OSD_TIMER_COUNT 2 #define DJI_OSD_FLAGS_OSD_FEATURE (1 << 0) -#define EFFICIENCY_UPDATE_INTERVAL (5 * 1000) +#define EFFICIENCY_UPDATE_INTERVAL (5 * 1000) #define RC_RX_LINK_LOST_MSG "!RC RX LINK LOST!" + // Adjust OSD_MESSAGE's default position when // changing OSD_MESSAGE_LENGTH #define OSD_MESSAGE_LENGTH 28 + #define OSD_ALTERNATING_CHOICES(ms, num_choices) ((millis() / ms) % num_choices) #define _CONST_STR_SIZE(s) ((sizeof(s)/sizeof(s[0]))-1) // -1 to avoid counting final '\0' + // Wrap all string constants intenteded for display as messages with // this macro to ensure compile time length validation. #define OSD_MESSAGE_STR(x) ({ \ @@ -409,7 +412,6 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst) } #endif -#if defined(USE_OSD) //testme static const char * osdArmingDisabledReasonMessage(void) { switch (isArmingDisabledReason()) { @@ -433,7 +435,6 @@ static const char * osdArmingDisabledReasonMessage(void) case ARMING_DISABLED_SYSTEM_OVERLOADED: return OSD_MESSAGE_STR("OVERLOAD"); case ARMING_DISABLED_NAVIGATION_UNSAFE: -#if defined(USE_NAV) // Check the exact reason switch (navigationIsBlockingArming(NULL)) { case NAV_ARMING_BLOCKER_NONE: @@ -447,7 +448,6 @@ static const char * osdArmingDisabledReasonMessage(void) case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR: return OSD_MESSAGE_STR("WYP MISCONFIGURED"); } -#endif break; case ARMING_DISABLED_COMPASS_NOT_CALIBRATED: return OSD_MESSAGE_STR("COMPS CALIB"); @@ -513,6 +513,7 @@ static const char * osdArmingDisabledReasonMessage(void) case WAS_EVER_ARMED: break; } + return NULL; } @@ -520,11 +521,9 @@ static const char * osdFailsafePhaseMessage(void) { // See failsafe.h for each phase explanation switch (failsafePhase()) { -#ifdef USE_NAV case FAILSAFE_RETURN_TO_HOME: // XXX: Keep this in sync with OSD_FLYMODE. return OSD_MESSAGE_STR("(RTH)"); -#endif case FAILSAFE_LANDING: // This should be considered an emergengy landing return OSD_MESSAGE_STR("(EMRGY LANDING)"); @@ -555,6 +554,7 @@ static const char * osdFailsafePhaseMessage(void) // Exiting failsafe break; } + return NULL; } @@ -564,6 +564,7 @@ static const char * osdFailsafeInfoMessage(void) // User must move sticks to exit FS mode return OSD_MESSAGE_STR("!MOVE STICKS TO EXIT FS!"); } + return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG); } @@ -623,22 +624,27 @@ static const char * navigationStateMessage(void) static int32_t osdConvertVelocityToUnit(int32_t vel) { switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - return (vel * 224) / 10000; // Convert to mph - case OSD_UNIT_METRIC: - return (vel * 36) / 1000; // Convert to kmh + case OSD_UNIT_UK: + FALLTHROUGH; + + case OSD_UNIT_IMPERIAL: + return (vel * 224) / 10000; // Convert to mph + + case OSD_UNIT_METRIC: + return (vel * 36) / 1000; // Convert to kmh } + // Unreachable return -1; } + static int16_t osdDJIGet3DSpeed(void) { int16_t vert_speed = getEstimatedActualVelocity(Z); int16_t hor_speed = gpsSol.groundSpeed; return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed)); } + /** * Converts velocity into a string based on the current unit system. * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds) @@ -646,23 +652,23 @@ static int16_t osdDJIGet3DSpeed(void) void osdDJIFormatVelocityStr(char* buff, int32_t vel ) { switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "MPH"); - break; - case OSD_UNIT_METRIC: - tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "KMH"); - break; + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "MPH"); + break; + case OSD_UNIT_METRIC: + tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "KMH"); + break; } } static void osdDJIFormatThrottlePosition(char *buff, bool autoThr ) { - int16_t thr = rxGetChannelValue(THROTTLE); if (autoThr && navigationIsControllingThrottle()) { thr = rcCommand[THROTTLE]; } + tfp_sprintf(buff, "%3d%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR"); } @@ -672,35 +678,38 @@ static void osdDJIFormatThrottlePosition(char *buff, bool autoThr ) */ static void osdDJIFormatDistanceStr(char *buff, int32_t dist) { - int32_t centifeet; - switch (osdConfig()->units) { - case OSD_UNIT_IMPERIAL: - centifeet = CENTIMETERS_TO_CENTIFEET(dist); - if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { - // Show feet when dist < 0.5mi - tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT"); - } else { - // Show miles when dist >= 0.5mi - tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)), - (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi"); + int32_t centifeet; + + switch (osdConfig()->units) { + case OSD_UNIT_IMPERIAL: + centifeet = CENTIMETERS_TO_CENTIFEET(dist); + if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { + // Show feet when dist < 0.5mi + tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT"); + } + else { + // Show miles when dist >= 0.5mi + tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)), + (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi"); + } + break; + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_METRIC: + if (abs(dist) < METERS_PER_KILOMETER * 100) { + // Show meters when dist < 1km + tfp_sprintf(buff, "%d%s", (int)(dist / 100), "M"); + } + else { + // Show kilometers when dist >= 1km + tfp_sprintf(buff, "%d.%02d%s", (int)(dist / (100*METERS_PER_KILOMETER)), + (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, "KM"); + } + break; } - break; - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_METRIC: - if (abs(dist) < METERS_PER_KILOMETER * 100) { - // Show meters when dist < 1km - tfp_sprintf(buff, "%d%s", (int)(dist / 100), "M"); - } else { - // Show kilometers when dist >= 1km - tfp_sprintf(buff, "%d.%02d%s", (int)(dist / (100*METERS_PER_KILOMETER)), - (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, "KM"); - } - break; - } } -static void osdDJIEfficiencyMahPerKM (char *buff) +static void osdDJIEfficiencyMahPerKM(char *buff) { // amperage is in centi amps, speed is in cms/s. We want // mah/km. Values over 999 are considered useless and @@ -710,23 +719,183 @@ static void osdDJIEfficiencyMahPerKM (char *buff) int32_t value = 0; timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); + if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1, efficiencyTimeDelta * 1e-6f); efficiencyUpdated = currentTimeUs; - } else { + } + else { value = eFilterState.state; } } + if (value > 0 && value <= 999) { tfp_sprintf(buff, "%3d%s", (int)value, "mAhKM"); - } else { + } + else { tfp_sprintf(buff, "%s", "---mAhKM"); } } -#endif + +static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name) +{ + // :W T S E D + // | | | | Distance Trip + // | | | Efficiency mA/KM + // | | S 3dSpeed + // | Throttle + // Warnings + const char *message = " "; + const char *enabledElements = name + 1; + char djibuf[24]; + + // clear name from chars : and leading W + if (enabledElements[0] == 'W') { + enabledElements += 1; + } + + int elemLen = strlen(enabledElements); + + if (elemLen > 0) { + switch (enabledElements[OSD_ALTERNATING_CHOICES(3000, elemLen)]){ + case 'T': + osdDJIFormatThrottlePosition(djibuf,true); + break; + case 'S': + osdDJIFormatVelocityStr(djibuf, osdDJIGet3DSpeed()); + break; + case 'E': + osdDJIEfficiencyMahPerKM(djibuf); + break; + case 'D': + osdDJIFormatDistanceStr(djibuf, getTotalTravelDistance()); + break; + case 'W': + tfp_sprintf(djibuf, "%s", "MAKE_W_FIRST"); + break; + default: + tfp_sprintf(djibuf, "%s", "UNKOWN_ELEM"); + break; + } + + if (djibuf[0] != '\0') { + message = djibuf; + } + } + + if (name[1] == 'W') { + char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; + if (ARMING_FLAG(ARMED)) { + // Aircraft is armed. We might have up to 5 + // messages to show. + const char *messages[5]; + unsigned messageCount = 0; + + if (FLIGHT_MODE(FAILSAFE_MODE)) { + // In FS mode while being armed too + const char *failsafePhaseMessage = osdFailsafePhaseMessage(); + const char *failsafeInfoMessage = osdFailsafeInfoMessage(); + const char *navStateFSMessage = navigationStateMessage(); + + if (failsafePhaseMessage) { + messages[messageCount++] = failsafePhaseMessage; + } + + if (failsafeInfoMessage) { + messages[messageCount++] = failsafeInfoMessage; + } + + if (navStateFSMessage) { + messages[messageCount++] = navStateFSMessage; + } + + if (messageCount > 0) { + message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; + if (message == failsafeInfoMessage) { + // failsafeInfoMessage is not useful for recovering + // a lost model, but might help avoiding a crash. + // Blink to grab user attention. + //doesnt work TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + // We're shoing either failsafePhaseMessage or + // navStateFSMessage. Don't BLINK here since + // having this text available might be crucial + // during a lost aircraft recovery and blinking + // will cause it to be missing from some frames. + } + } + else { + if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { + const char *navStateMessage = navigationStateMessage(); + if (navStateMessage) { + messages[messageCount++] = navStateMessage; + } + } + else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { + messages[messageCount++] = "AUTOLAUNCH"; + } + else { + if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { + // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO + // when it doesn't require ANGLE mode (required only in FW + // right now). If if requires ANGLE, its display is handled + // by OSD_FLYMODE. + messages[messageCount++] = "(ALT HOLD)"; + } + if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { + messages[messageCount++] = "(AUTOTRIM)"; + } + if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { + messages[messageCount++] = "(AUTOTUNE)"; + } + if (FLIGHT_MODE(HEADFREE_MODE)) { + messages[messageCount++] = "(HEADFREE)"; + } + } + // Pick one of the available messages. Each message lasts + // a second. + if (messageCount > 0) { + message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; + } + } + } + else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { + unsigned invalidIndex; + // Check if we're unable to arm for some reason + if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) { + if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) { + const setting_t *setting = settingGet(invalidIndex); + settingGetName(setting, messageBuf); + for (int ii = 0; messageBuf[ii]; ii++) { + messageBuf[ii] = sl_toupper(messageBuf[ii]); + } + message = messageBuf; + } + else { + message = "ERR SETTING"; + // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); + } + } + else { + if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) { + message = "CANT ARM"; + // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); + } else { + // Show the reason for not arming + message = osdArmingDisabledReasonMessage(); + } + } + } + } + + if (message[0] != '\0') { + sbufWriteData(dst, message, strlen(message)); + } +} + static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) { @@ -762,155 +931,14 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms case DJI_MSP_NAME: { const char * name = systemConfig()->name; - int len = strlen(name); - - if(name[0] != ':'){ - if (len > 12) len = 12; - sbufWriteData(dst, name, len); + + if (djiOsdConfig()->use_name_for_messages && name[0] == ':') { + djiSerializeCraftNameOverride(dst, name); + } + else { + int len = strlen(name); + sbufWriteData(dst, name, MAX(len, 12)); break; - }else{ -#if defined(USE_OSD) - // :W T S E D - // | | | | Distance Trip - // | | | Efficiency mA/KM - // | | S 3dSpeed - // | Throttle - // Warnings - const char *message = " "; - const char *enabledElements = name + 1; - char djibuf[24]; - // clear name from chars : and leading W - if(enabledElements[0] == 'W'){ - enabledElements += 1; - } - - int elemLen = strlen(enabledElements); - - if(elemLen > 0){ - switch ( enabledElements[OSD_ALTERNATING_CHOICES(3000, elemLen )] ){ - case 'T': - osdDJIFormatThrottlePosition(djibuf,true); - break; - case 'S': - osdDJIFormatVelocityStr(djibuf, osdDJIGet3DSpeed() ); - break; - case 'E': - osdDJIEfficiencyMahPerKM(djibuf); - break; - case 'D': - osdDJIFormatDistanceStr( djibuf, getTotalTravelDistance()); - break; - case 'W': - tfp_sprintf(djibuf, "%s", "MAKE_W_FIRST"); - break; - default: - tfp_sprintf(djibuf, "%s", "UNKOWN_ELEM"); - break; - } - - if(djibuf[0] != '\0'){ - message = djibuf; - } - } - - if (name[1] == 'W' ){ - char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; - if (ARMING_FLAG(ARMED)) { - // Aircraft is armed. We might have up to 5 - // messages to show. - const char *messages[5]; - unsigned messageCount = 0; - if (FLIGHT_MODE(FAILSAFE_MODE)) { - // In FS mode while being armed too - const char *failsafePhaseMessage = osdFailsafePhaseMessage(); - const char *failsafeInfoMessage = osdFailsafeInfoMessage(); - const char *navStateFSMessage = navigationStateMessage(); - if (failsafePhaseMessage) { - messages[messageCount++] = failsafePhaseMessage; - } - if (failsafeInfoMessage) { - messages[messageCount++] = failsafeInfoMessage; - } - if (navStateFSMessage) { - messages[messageCount++] = navStateFSMessage; - } - if (messageCount > 0) { - message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; - if (message == failsafeInfoMessage) { - // failsafeInfoMessage is not useful for recovering - // a lost model, but might help avoiding a crash. - // Blink to grab user attention. - //doesnt work TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - // We're shoing either failsafePhaseMessage or - // navStateFSMessage. Don't BLINK here since - // having this text available might be crucial - // during a lost aircraft recovery and blinking - // will cause it to be missing from some frames. - } - } else { - if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { - const char *navStateMessage = navigationStateMessage(); - if (navStateMessage) { - messages[messageCount++] = navStateMessage; - } - } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { - messages[messageCount++] = "AUTOLAUNCH"; - } else { - if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { - // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO - // when it doesn't require ANGLE mode (required only in FW - // right now). If if requires ANGLE, its display is handled - // by OSD_FLYMODE. - messages[messageCount++] = "(ALT HOLD)"; - } - if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { - messages[messageCount++] = "(AUTOTRIM)"; - } - if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { - messages[messageCount++] = "(AUTOTUNE)"; - } - if (FLIGHT_MODE(HEADFREE_MODE)) { - messages[messageCount++] = "(HEADFREE)"; - } - } - // Pick one of the available messages. Each message lasts - // a second. - if (messageCount > 0) { - message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; - } - } - } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { - unsigned invalidIndex; - // Check if we're unable to arm for some reason - if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) { - if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) { - const setting_t *setting = settingGet(invalidIndex); - settingGetName(setting, messageBuf); - for (int ii = 0; messageBuf[ii]; ii++) { - messageBuf[ii] = sl_toupper(messageBuf[ii]); - } - message = messageBuf; - } else { - message = "ERR SETTING"; - // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); - } - } else { - if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) { - message = "CANT ARM"; - // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); - } else { - // Show the reason for not arming - message = osdArmingDisabledReasonMessage(); - } - } - } - } - - if(message[0] != '\0'){ - sbufWriteData(dst, message, strlen(message)); - } -#endif } } break; From 99b8ff0dd9033ccd8b8c77ee5807146b7d51acd5 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sat, 14 Nov 2020 17:19:38 +0100 Subject: [PATCH 094/109] [DJI] If craft name isn't a valid element list - fall back to warnings when craft name is repurposed for messages; Fix compilation on non-OSD targets --- src/main/io/osd_dji_hd.c | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 423108928b..574c5710a4 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -410,7 +410,6 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst) //sbufWriteU8(dst, DJI_OSD_SCREEN_WIDTH); // osdConfig()->camera_frame_width //sbufWriteU8(dst, DJI_OSD_SCREEN_HEIGHT); // osdConfig()->camera_frame_height } -#endif static const char * osdArmingDisabledReasonMessage(void) { @@ -895,6 +894,7 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name) sbufWriteData(dst, message, strlen(message)); } } +#endif static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) @@ -932,10 +932,20 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms { const char * name = systemConfig()->name; - if (djiOsdConfig()->use_name_for_messages && name[0] == ':') { - djiSerializeCraftNameOverride(dst, name); +#if defined(USE_OSD) + if (djiOsdConfig()->use_name_for_messages) { + if (name[0] == ':') { + // If craft name starts with a semicolon - use it as a template for what we want to show + djiSerializeCraftNameOverride(dst, name); + } + else { + // Otherwise fall back to just warnings + djiSerializeCraftNameOverride(dst, ":W"); + } } - else { + else +#endif + { int len = strlen(name); sbufWriteData(dst, name, MAX(len, 12)); break; From 6d4c369d7764a35103dc4efcde045e363c3f2f41 Mon Sep 17 00:00:00 2001 From: dragnea Date: Sat, 14 Nov 2020 22:19:50 +0200 Subject: [PATCH 095/109] Add low throttle beeper --- src/main/cms/cms_menu_navigation.c | 1 + src/main/fc/settings.yaml | 2 +- src/main/io/beeper.c | 53 ++++++++++++---------- src/main/io/beeper.h | 1 + src/main/navigation/navigation_fw_launch.c | 6 ++- 5 files changed, 37 insertions(+), 26 deletions(-) diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index a60e8e7ed5..cd7831c793 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -132,6 +132,7 @@ static const OSD_Entry cmsx_menuFWLaunchEntries[] = OSD_SETTING_ENTRY("IDLE THROTTLE", SETTING_NAV_FW_LAUNCH_IDLE_THR), OSD_SETTING_ENTRY("MOTOR SPINUP TIME", SETTING_NAV_FW_LAUNCH_SPINUP_TIME), OSD_SETTING_ENTRY("TIMEOUT", SETTING_NAV_FW_LAUNCH_TIMEOUT), + OSD_SETTING_ENTRY("END TRANSITION TIME", SETTING_NAV_FW_LAUNCH_END_TIME), OSD_SETTING_ENTRY("MAX ALTITUDE", SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE), OSD_SETTING_ENTRY("CLIMB ANGLE", SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE), OSD_SETTING_ENTRY("MAX BANK ANGLE", SETTING_NAV_FW_LAUNCH_MAX_ANGLE), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e731390b74..a7cafbef58 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2337,7 +2337,7 @@ groups: max: 1000 - name: nav_fw_launch_end_time description: "Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms]" - default_value: "3000" + default_value: "2000" field: fw.launch_end_time min: 0 max: 5000 diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 4306d101f5..e08412db92 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -121,6 +121,10 @@ static const uint8_t beep_runtimeCalibrationDone[] = { static const uint8_t beep_launchModeBeep[] = { 5, 5, 5, 100, BEEPER_COMMAND_STOP }; +// Two short beeps, then one shorter beep. Beeps to show the throttle needs to be raised. Will repeat every second while the throttle is low. +static const uint8_t beep_launchModeLowThrottleBeep[] = { + 5, 5, 5, 5, 3, 100, BEEPER_COMMAND_STOP +}; // short beeps static const uint8_t beep_hardwareFailure[] = { 10, 10, BEEPER_COMMAND_STOP @@ -164,31 +168,32 @@ typedef struct beeperTableEntry_s { #define BEEPER_ENTRY(a,b,c,d) a,b,c,d /*static*/ const beeperTableEntry_t beeperTable[] = { - { BEEPER_ENTRY(BEEPER_RUNTIME_CALIBRATION_DONE, 0, beep_runtimeCalibrationDone, "RUNTIME_CALIBRATION") }, - { BEEPER_ENTRY(BEEPER_HARDWARE_FAILURE , 1, beep_hardwareFailure, "HW_FAILURE") }, - { BEEPER_ENTRY(BEEPER_RX_LOST, 2, beep_txLostBeep, "RX_LOST") }, - { BEEPER_ENTRY(BEEPER_RX_LOST_LANDING, 3, beep_sos, "RX_LOST_LANDING") }, - { BEEPER_ENTRY(BEEPER_DISARMING, 4, beep_disarmBeep, "DISARMING") }, - { BEEPER_ENTRY(BEEPER_ARMING, 5, beep_armingBeep, "ARMING") }, - { BEEPER_ENTRY(BEEPER_ARMING_GPS_FIX, 6, beep_armedGpsFix, "ARMING_GPS_FIX") }, - { BEEPER_ENTRY(BEEPER_BAT_CRIT_LOW, 7, beep_critBatteryBeep, "BAT_CRIT_LOW") }, - { BEEPER_ENTRY(BEEPER_BAT_LOW, 8, beep_lowBatteryBeep, "BAT_LOW") }, - { BEEPER_ENTRY(BEEPER_GPS_STATUS, 9, beep_multiBeeps, "GPS_STATUS") }, - { BEEPER_ENTRY(BEEPER_RX_SET, 10, beep_shortBeep, "RX_SET") }, - { BEEPER_ENTRY(BEEPER_ACTION_SUCCESS, 11, beep_2shortBeeps, "ACTION_SUCCESS") }, - { BEEPER_ENTRY(BEEPER_ACTION_FAIL, 12, beep_2longerBeeps, "ACTION_FAIL") }, - { BEEPER_ENTRY(BEEPER_READY_BEEP, 13, beep_readyBeep, "READY_BEEP") }, - { BEEPER_ENTRY(BEEPER_MULTI_BEEPS, 14, beep_multiBeeps, "MULTI_BEEPS") }, // FIXME having this listed makes no sense since the beep array will not be initialised. - { BEEPER_ENTRY(BEEPER_DISARM_REPEAT, 15, beep_disarmRepeatBeep, "DISARM_REPEAT") }, - { BEEPER_ENTRY(BEEPER_ARMED, 16, beep_armedBeep, "ARMED") }, - { BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 17, NULL, "SYSTEM_INIT") }, - { BEEPER_ENTRY(BEEPER_USB, 18, NULL, "ON_USB") }, - { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_ENABLED, 19, beep_launchModeBeep, "LAUNCH_MODE") }, - { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 20, beep_camOpenBeep, "CAM_CONNECTION_OPEN") }, - { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 21, beep_camCloseBeep, "CAM_CONNECTION_CLOSED") }, + { BEEPER_ENTRY(BEEPER_RUNTIME_CALIBRATION_DONE, 0, beep_runtimeCalibrationDone, "RUNTIME_CALIBRATION") }, + { BEEPER_ENTRY(BEEPER_HARDWARE_FAILURE , 1, beep_hardwareFailure, "HW_FAILURE") }, + { BEEPER_ENTRY(BEEPER_RX_LOST, 2, beep_txLostBeep, "RX_LOST") }, + { BEEPER_ENTRY(BEEPER_RX_LOST_LANDING, 3, beep_sos, "RX_LOST_LANDING") }, + { BEEPER_ENTRY(BEEPER_DISARMING, 4, beep_disarmBeep, "DISARMING") }, + { BEEPER_ENTRY(BEEPER_ARMING, 5, beep_armingBeep, "ARMING") }, + { BEEPER_ENTRY(BEEPER_ARMING_GPS_FIX, 6, beep_armedGpsFix, "ARMING_GPS_FIX") }, + { BEEPER_ENTRY(BEEPER_BAT_CRIT_LOW, 7, beep_critBatteryBeep, "BAT_CRIT_LOW") }, + { BEEPER_ENTRY(BEEPER_BAT_LOW, 8, beep_lowBatteryBeep, "BAT_LOW") }, + { BEEPER_ENTRY(BEEPER_GPS_STATUS, 9, beep_multiBeeps, "GPS_STATUS") }, + { BEEPER_ENTRY(BEEPER_RX_SET, 10, beep_shortBeep, "RX_SET") }, + { BEEPER_ENTRY(BEEPER_ACTION_SUCCESS, 11, beep_2shortBeeps, "ACTION_SUCCESS") }, + { BEEPER_ENTRY(BEEPER_ACTION_FAIL, 12, beep_2longerBeeps, "ACTION_FAIL") }, + { BEEPER_ENTRY(BEEPER_READY_BEEP, 13, beep_readyBeep, "READY_BEEP") }, + { BEEPER_ENTRY(BEEPER_MULTI_BEEPS, 14, beep_multiBeeps, "MULTI_BEEPS") }, // FIXME having this listed makes no sense since the beep array will not be initialised. + { BEEPER_ENTRY(BEEPER_DISARM_REPEAT, 15, beep_disarmRepeatBeep, "DISARM_REPEAT") }, + { BEEPER_ENTRY(BEEPER_ARMED, 16, beep_armedBeep, "ARMED") }, + { BEEPER_ENTRY(BEEPER_SYSTEM_INIT, 17, NULL, "SYSTEM_INIT") }, + { BEEPER_ENTRY(BEEPER_USB, 18, NULL, "ON_USB") }, + { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_ENABLED, 19, beep_launchModeBeep, "LAUNCH_MODE") }, + { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_LOW_THROTTLE, 20, beep_launchModeLowThrottleBeep, "LAUNCH_MODE_LOW_THROTTLE") }, + { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 20, beep_camOpenBeep, "CAM_CONNECTION_OPEN") }, + { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 21, beep_camCloseBeep, "CAM_CONNECTION_CLOSED") }, - { BEEPER_ENTRY(BEEPER_ALL, 22, NULL, "ALL") }, - { BEEPER_ENTRY(BEEPER_PREFERENCE, 23, NULL, "PREFERED") }, + { BEEPER_ENTRY(BEEPER_ALL, 22, NULL, "ALL") }, + { BEEPER_ENTRY(BEEPER_PREFERENCE, 23, NULL, "PREFERED") }, }; static const beeperTableEntry_t *currentBeeperEntry = NULL; diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 5dcff77f0a..5687fe6a93 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -43,6 +43,7 @@ typedef enum { BEEPER_SYSTEM_INIT, // Initialisation beeps when board is powered on BEEPER_USB, // Some boards have beeper powered USB connected BEEPER_LAUNCH_MODE_ENABLED, // Fixed-wing launch mode enabled + BEEPER_LAUNCH_MODE_LOW_THROTTLE,// Fixed-wing launch mode enabled, but throttle is low BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index ace3e17d3d..d87454baa1 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -390,7 +390,11 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs) { updateRcCommand(); // Control beeper - beeper(BEEPER_LAUNCH_MODE_ENABLED); + if (fwLaunch.currentState == FW_LAUNCH_STATE_WAIT_THROTTLE) { + beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); + } else { + beeper(BEEPER_LAUNCH_MODE_ENABLED); + } } void resetFixedWingLaunchController(timeUs_t currentTimeUs) { From 1b44bd569c12c812ca978da296f00c9eb876725b Mon Sep 17 00:00:00 2001 From: Airwide Date: Sun, 15 Nov 2020 09:17:02 +0100 Subject: [PATCH 096/109] Changed limits for feature settings --- src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation_fixedwing.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2e1bbc3660..e7c7205694 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2271,7 +2271,7 @@ groups: default_value: "0" field: fw.pitch_to_throttle_thresh min: 0 - max: 100 + max: 900 - name: nav_fw_loiter_radius description: "PosHold radius. 3000 to 7500 is a good value (30-75m) [cm]" default_value: "5000" diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index c9a63653e0..dffa74bc02 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -76,7 +76,7 @@ static int8_t loiterDirYaw = 1; static float getSmoothnessCutoffFreq(float baseFreq) { uint16_t smoothness = 10 - navConfig()->fw.control_smoothness; - return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f; + return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.01f; } // Calculates the cutoff frequency for smoothing out pitchToThrottleCorrection From ab60dfb0e21b889009e3605c742931d1331d4b67 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Sun, 15 Nov 2020 12:31:38 +0000 Subject: [PATCH 097/109] [DOC] Cli.md: add missing 2.6 items (bind_rx, safehome), make references clickable. (#6290) --- docs/Cli.md | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index 3b059bb6b6..04332e2fbf 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -65,7 +65,8 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | `1wire ` | passthrough 1wire to the specified esc | | `adjrange` | show/set adjustment ranges settings | | `aux` | show/set aux settings | -| `beeper` | show/set beeper (buzzer) usage (see docs/Buzzer.md) | +| `beeper` | show/set beeper (buzzer) [usage](Buzzer.md) | +| `bind_rx` | Initiate binding for RX_SPI or SRXL2 receivers | | `mmix` | design custom motor mixer | | `smix` | design custom servo mixer | | `color` | configure colors | @@ -80,18 +81,19 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | `led` | configure leds | | `map` | mapping of rc channel order | | `motor` | get/set motor output value | -| `msc` | Enter USB Mass storage mode. See docs/USB_Mass_Storage_(MSC)_mode.md for usage information.| +| `msc` | Enter USB Mass storage mode. See [USB MSC documentation](USB_Mass_Storage_(MSC)_mode.md) for usage information.| | `play_sound` | index, or none for next | | `profile` | index (0 to 2) | | `rxrange` | configure rx channel ranges (end-points) | +| `safehome` | Define safe home locations. See the [safehome documentation](Safehomes.md) for usage information. | | `save` | save and reboot | | `serial` | Configure serial ports | | `serialpassthrough `| where `id` is the zero based port index, `baud` is a standard baud rate, and mode is `rx`, `tx`, or both (`rxtx`) | | `set` | name=value or blank or * for list | | `status` | show system status | -| `temp_sensor` | list or configure temperature sensor(s). See docs/Temperature sensors.md | -| `wp` | list or configure waypoints. See more in docs/Navigation.md section NAV WP | -| `version` | | +| `temp_sensor` | list or configure temperature sensor(s). See [temperature sensors documentation](Temperature sensors.md) for more information. | +| `wp` | list or configure waypoints. See more in the [navigation documentation](Navigation.md#cli-command-wp-to-manage-waypoints). | +| `version` | Displays version information, | ### serial From 3747b1e434f25b05fdfe5045c0a596c7893f5298 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Sun, 15 Nov 2020 17:01:49 +0100 Subject: [PATCH 098/109] Fix nav_overrides_motor_stop default docs value --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 447381166a..077c22e529 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2108,7 +2108,7 @@ groups: max: 5000 - name: nav_overrides_motor_stop description: "When set OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV" - default_value: "NOMS_ALL_NAV" + default_value: "ALL_NAV" field: general.flags.nav_overrides_motor_stop table: nav_overrides_motor_stop - name: nav_rth_climb_first From 6285b7a92984b483d60dcac4442fc2f762742f84 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Sun, 15 Nov 2020 17:02:37 +0100 Subject: [PATCH 099/109] Update Settings.md --- docs/Settings.md | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index d82e08a80f..c2f09410cd 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -61,6 +61,9 @@ | debug_mode | NONE | Defines debug values exposed in debug variables (developer / debugging setting) | | disarm_kill_switch | ON | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | | display_force_sw_blink | OFF | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON | +| dji_esc_temp_source | ESC | Re-purpose the ESC temperature field for IMU/BARO temperature | +| dji_use_name_for_messages | ON | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance | +| dji_workarounds | | Enables workarounds for different versions of MSP protocol used | | dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter | | dterm_lpf2_type | `BIQUAD` | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | | dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value | @@ -125,6 +128,7 @@ | fw_p_yaw | 6 | Fixed-wing rate stabilisation P-gain for YAW | | fw_reference_airspeed | 1000 | Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present. | | fw_tpa_time_constant | 0 | TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups | +| fw_turn_assist_pitch_gain | 1 | Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | | fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | | gps_auto_baud | ON | Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports | | gps_auto_config | ON | Enable automatic configuration of UBlox GPS receivers. | @@ -187,9 +191,9 @@ | mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you acquire valid GPS fix. | | mag_hardware | AUTO | Selection of mag hardware. See Wiki Sensor auto detect and hardware failure detection for more info | | mag_to_use | | Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target | -| maggain_x | 0 | Magnetometer calibration X gain. If 0, no calibration or calibration failed | -| maggain_y | 0 | Magnetometer calibration Y gain. If 0, no calibration or calibration failed | -| maggain_z | 0 | Magnetometer calibration Z gain. If 0, no calibration or calibration failed | +| maggain_x | 1024 | Magnetometer calibration X gain. If 1024, no calibration or calibration failed | +| maggain_y | 1024 | Magnetometer calibration Y gain. If 1024, no calibration or calibration failed | +| maggain_z | 1024 | Magnetometer calibration Z gain. If 1024, no calibration or calibration failed | | magzero_x | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. | | magzero_y | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. | | magzero_z | 0 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. | @@ -201,6 +205,7 @@ | mavlink_ext_status_rate | | | | mavlink_extra1_rate | | | | mavlink_extra2_rate | | | +| mavlink_extra3_rate | | | | mavlink_pos_rate | | | | mavlink_rc_chan_rate | | | | max_angle_inclination_pit | 300 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° | @@ -258,13 +263,13 @@ | nav_fw_launch_accel | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s | | nav_fw_launch_climb_angle | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit | | nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] | +| nav_fw_launch_end_time | 2000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | | nav_fw_launch_idle_thr | 1000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) | | nav_fw_launch_max_altitude | 0 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. | | nav_fw_launch_max_angle | 45 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] | | nav_fw_launch_min_time | 0 | Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]. | | nav_fw_launch_motor_delay | 500 | Delay between detected launch and launch sequence start and throttling up (ms) | | nav_fw_launch_spinup_time | 100 | Time to bring power from minimum throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller | -| nav_fw_launch_end_time | 3000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | | nav_fw_launch_thr | 1700 | Launch throttle - throttle to be set during launch sequence (pwm units) | | nav_fw_launch_timeout | 5000 | Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms) | | nav_fw_launch_velocity | 300 | Forward velocity threshold for swing-launch detection [cm/s] | @@ -272,7 +277,7 @@ | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | -| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by nav_fw_pitch2thr_threshold | +| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | | nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | @@ -319,7 +324,7 @@ | nav_mc_vel_z_i | 50 | I gain of velocity PID controller | | nav_mc_vel_z_p | 100 | P gain of velocity PID controller | | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. | -| nav_overrides_motor_stop | ON | Setting to OFF combined with MOTOR_STOP feature will allow user to stop motor when in autonomous modes. On most places this setting is likely to cause a stall. | +| nav_overrides_motor_stop | ALL_NAV | When set OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] | | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. | @@ -351,6 +356,7 @@ | osd_camera_uptilt | 0 | Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal | | osd_coordinate_digits | | | | osd_crosshairs_style | DEFAULT | To set the visual type for the crosshair | +| osd_crsf_lq_format | TYPE1 | To select LQ format | | osd_current_alarm | 0 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. | | osd_dist_alarm | 1000 | Value above which to make the OSD distance from home indicator blink (meters) | | osd_esc_temp_alarm_max | 900 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | @@ -375,15 +381,17 @@ | osd_imu_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | | osd_left_sidebar_scroll | | | | osd_left_sidebar_scroll_step | 0 | How many units each sidebar step represents. 0 means the default value for the scroll type. | +| osd_link_quality_alarm | 70 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% | | osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | -| osd_neg_alt_alarm | 5 | Value bellow which (negative altitude) to make the OSD relative altitude indicator blink (meters) | +| osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | | osd_plus_code_digits | | | | osd_right_sidebar_scroll | | | | osd_right_sidebar_scroll_step | 0 | Same as left_sidebar_scroll_step, but for the right sidebar | | osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) | -| osd_rssi_alarm | 20 | Value bellow which to make the OSD RSSI indicator blink | +| osd_rssi_alarm | 20 | Value below which to make the OSD RSSI indicator blink | | osd_sidebar_horizontal_offset | 0 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. | | osd_sidebar_scroll_arrows | | | +| osd_snr_alarm | 4 | Value below which Crossfire SNR Alarm pops-up. (dB) | | osd_stats_energy_unit | MAH | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) | | osd_temp_label_align | LEFT | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` | | osd_time_alarm | 10 | Value above which to make the OSD flight time indicator blink (minutes) | @@ -451,6 +459,8 @@ | smartport_master_halfduplex | | | | smartport_master_inverted | | | | spektrum_sat_bind | 0 | 0 = disabled. Used to bind the spektrum satellite to RX | +| srxl2_baud_fast | | | +| srxl2_unit_id | | | | stats | OFF | General switch of the statistics recording feature (a.k.a. odometer) | | stats_total_dist | 0 | Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. | | stats_total_energy | | | @@ -488,4 +498,4 @@ | yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | | yaw_rate | 20 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | -> Note: this table is autogenerated. Do not edit it manually. +> Note: this table is autogenerated. Do not edit it manually. \ No newline at end of file From a3ada125d62663767f5013694e6b18dd08d8dc9f Mon Sep 17 00:00:00 2001 From: Airwide Date: Sun, 15 Nov 2020 17:51:25 +0100 Subject: [PATCH 100/109] Fix for control smoothness bug introduced in PR #6104 --- src/main/navigation/navigation_fixedwing.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index dffa74bc02..51c25d77cc 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -76,16 +76,16 @@ static int8_t loiterDirYaw = 1; static float getSmoothnessCutoffFreq(float baseFreq) { uint16_t smoothness = 10 - navConfig()->fw.control_smoothness; - return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.01f; + return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f; } // Calculates the cutoff frequency for smoothing out pitchToThrottleCorrection // pitch_to_throttle_smooth valid range from 0 to 9 -// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz +// resulting cutoff_freq ranging from baseFreq downwards to ~0.01Hz static float getPitchToThrottleSmoothnessCutoffFreq(float baseFreq) { uint16_t smoothness = 10 - navConfig()->fw.pitch_to_throttle_smooth; - return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f; + return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.01f; } /*----------------------------------------------------------- From 27ed750e736c5050bfdd1f92ca869b71d0912451 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sun, 15 Nov 2020 18:36:25 +0100 Subject: [PATCH 101/109] [DJI] Fix dji_worksrounds min/max range --- src/main/fc/settings.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 447381166a..fa69bc6293 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3180,6 +3180,8 @@ groups: - name: dji_workarounds description: "Enables workarounds for different versions of MSP protocol used" field: proto_workarounds + min: 0 + max: UINT8_MAX - name: dji_use_name_for_messages description: "Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance" default_value: "ON" From e07cbeb49be5b03332eeadfe6075d9ab35d53bb7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Mon, 16 Nov 2020 11:37:13 +0100 Subject: [PATCH 102/109] Migrate from set-env to env files --- .github/workflows/ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index f8f2bdd8da..63362e6468 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -27,8 +27,8 @@ jobs: COMMIT_ID=${COMMIT_ID:-${{ github.sha }}} BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID}) VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }') - echo "::set-env name=BUILD_SUFFIX::${BUILD_SUFFIX}" - echo "::set-env name=BUILD_NAME::inav-${VERSION}-${BUILD_SUFFIX}" + echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV + echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV - uses: actions/cache@v1 with: path: downloads From cf4e2dced46900975e3d7dc514801f1ca71ea6da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Mon, 16 Nov 2020 12:15:08 +0100 Subject: [PATCH 103/109] Make CI check if Settings docs need to be updated --- .github/workflows/docs.yml | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 .github/workflows/docs.yml diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml new file mode 100644 index 0000000000..674155dacc --- /dev/null +++ b/.github/workflows/docs.yml @@ -0,0 +1,23 @@ +name: Make sure docs are updated +on: + push: + paths: + - src/main/fc/settings.yaml + - docs/Settings.md + +jobs: + settings_md: + runs-on: ubuntu-18.04 + + steps: + - uses: actions/checkout@v2 + - name: Install dependencies + run: sudo apt-get update && sudo apt-get -y install python3-yaml + - name: Check that Settings.md is up to date + run: | + cp docs/Settings.md{,.ci} + python3 src/utils/update_cli_docs.py -q + if ! diff -q docs/Settings.md{,.ci} >/dev/null; then + echo "::error ::\"docs/Settings.md\" is not up to date, please run \"src/utils/update_cli_docs.py\"" + exit 1 + fi From d028b19ec7efc7e91bf76bb4ac4063fef1e6b936 Mon Sep 17 00:00:00 2001 From: Konstantin Sharlaimov Date: Mon, 16 Nov 2020 14:04:13 +0100 Subject: [PATCH 104/109] Revert "Autolaunch - Added a smooth end launch feature and code refactor" --- docs/Cli.md | 1 + src/main/cms/cms_menu_navigation.c | 1 - src/main/fc/settings.yaml | 6 - src/main/io/osd.c | 4 - src/main/navigation/navigation.c | 1 - src/main/navigation/navigation.h | 4 +- src/main/navigation/navigation_fw_launch.c | 511 +++++++-------------- 7 files changed, 161 insertions(+), 367 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index 04332e2fbf..d177c717c7 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -147,4 +147,5 @@ For targets that have a flash data chip, typically used for blackbox logs, the f | `flash_write
` | Writes `data` to `address` | ## CLI Variable Reference + See [Settings.md](Settings.md). diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 2a70ce6fe6..5e09da1066 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -134,7 +134,6 @@ static const OSD_Entry cmsx_menuFWLaunchEntries[] = OSD_SETTING_ENTRY("IDLE THROTTLE", SETTING_NAV_FW_LAUNCH_IDLE_THR), OSD_SETTING_ENTRY("MOTOR SPINUP TIME", SETTING_NAV_FW_LAUNCH_SPINUP_TIME), OSD_SETTING_ENTRY("TIMEOUT", SETTING_NAV_FW_LAUNCH_TIMEOUT), - OSD_SETTING_ENTRY("END TRANSITION TIME", SETTING_NAV_FW_LAUNCH_END_TIME), OSD_SETTING_ENTRY("MAX ALTITUDE", SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE), OSD_SETTING_ENTRY("CLIMB ANGLE", SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE), OSD_SETTING_ENTRY("MAX BANK ANGLE", SETTING_NAV_FW_LAUNCH_MAX_ANGLE), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 62c5496df7..c432341cf3 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2371,12 +2371,6 @@ groups: field: fw.launch_motor_spinup_time min: 0 max: 1000 - - name: nav_fw_launch_end_time - description: "Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms]" - default_value: "2000" - field: fw.launch_end_time - min: 0 - max: 5000 - name: nav_fw_launch_min_time description: "Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]." default_value: "0" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b951ec7ebc..80c10d85fb 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3327,10 +3327,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); - const char *launchStateMessage = fixedWingLaunchStateMessage(); - if (launchStateMessage) { - messages[messageCount++] = launchStateMessage; - } } else { if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index ba319442ca..a287162174 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -171,7 +171,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .launch_idle_throttle = 1000, // Motor idle or MOTOR_STOP .launch_motor_timer = 500, // ms .launch_motor_spinup_time = 100, // ms, time to gredually increase throttle from idle to launch - .launch_end_time = 3000, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode .launch_min_time = 0, // ms, min time in launch mode .launch_timeout = 5000, // ms, timeout for launch procedure .launch_max_altitude = 0, // cm, altitude where to consider launch ended diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index f5f0aa5990..36fbd2b29e 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -239,8 +239,7 @@ typedef struct navConfig_s { uint16_t launch_throttle; // Launch throttle uint16_t launch_motor_timer; // Time to wait before setting launch_throttle (ms) uint16_t launch_motor_spinup_time; // Time to speed-up motors from idle to launch_throttle (ESC desync prevention) - uint16_t launch_end_time; // Time to make the transition from launch angle to leveled and throttle transition from launch throttle to the stick position - uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early + uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early uint16_t launch_timeout; // Launch timeout to disable launch mode and swith to normal flight (ms) uint16_t launch_max_altitude; // cm, altitude where to consider launch ended uint8_t launch_climb_angle; // Target climb angle for launch (deg) @@ -525,7 +524,6 @@ bool navigationRTHAllowsLanding(void); bool isNavLaunchEnabled(void); bool isFixedWingLaunchDetected(void); bool isFixedWingLaunchFinishedOrAborted(void); -const char * fixedWingLaunchStateMessage(void); float calculateAverageSpeed(void); diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index d87454baa1..309bef0cf0 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -33,6 +33,15 @@ #include "config/feature.h" #include "drivers/time.h" +#include "drivers/sensor.h" +#include "drivers/accgyro/accgyro.h" + +#include "sensors/sensors.h" +#include "sensors/rangefinder.h" +#include "sensors/barometer.h" +#include "sensors/acceleration.h" +#include "sensors/boardalignment.h" +#include "sensors/compass.h" #include "io/gps.h" #include "io/beeper.h" @@ -48,238 +57,25 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" + +typedef struct FixedWingLaunchState_s { + /* Launch detection */ + timeUs_t launchDetectorPreviousUpdate; + timeUs_t launchDetectionTimeAccum; + bool launchDetected; + + /* Launch progress */ + timeUs_t launchStartedTime; + bool launchFinished; + bool motorControlAllowed; +} FixedWingLaunchState_t; + +static EXTENDED_FASTRAM FixedWingLaunchState_t launchState; + +#define COS_MAX_LAUNCH_ANGLE 0.70710678f // cos(45), just to be safe #define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate -#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms -#define UNUSED(x) ((void)(x)) -#define FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE "RAISE THE THROTTLE" -#define FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION "READY" -#define FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS "MOVE THE STICKS TO ABORT" -#define FW_LAUNCH_MESSAGE_TEXT_FINISHING "FINISHING" - -typedef enum { - FW_LAUNCH_MESSAGE_TYPE_NONE = 0, - FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE, - FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION, - FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS, - FW_LAUNCH_MESSAGE_TYPE_FINISHING -} fixedWingLaunchMessage_t; - -typedef enum { - FW_LAUNCH_EVENT_NONE = 0, - FW_LAUNCH_EVENT_SUCCESS, - FW_LAUNCH_EVENT_GOTO_DETECTION, - FW_LAUNCH_EVENT_ABORT, - FW_LAUNCH_EVENT_THROTTLE_LOW, - FW_LAUNCH_EVENT_COUNT -} fixedWingLaunchEvent_t; - -typedef enum { - FW_LAUNCH_STATE_IDLE = 0, - FW_LAUNCH_STATE_WAIT_THROTTLE, - FW_LAUNCH_STATE_MOTOR_IDLE, - FW_LAUNCH_STATE_WAIT_DETECTION, - FW_LAUNCH_STATE_DETECTED, - FW_LAUNCH_STATE_MOTOR_DELAY, - FW_LAUNCH_STATE_MOTOR_SPINUP, - FW_LAUNCH_STATE_IN_PROGRESS, - FW_LAUNCH_STATE_FINISH, - FW_LAUNCH_STATE_COUNT -} fixedWingLaunchState_t; - -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs); - -typedef struct fixedWingLaunchStateDescriptor_s { - fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs); - fixedWingLaunchState_t onEvent[FW_LAUNCH_EVENT_COUNT]; - fixedWingLaunchMessage_t messageType; -} fixedWingLaunchStateDescriptor_t; - -typedef struct fixedWingLaunchData_s { - timeUs_t currentStateTimeUs; - fixedWingLaunchState_t currentState; - uint8_t pitchAngle; // used to smooth the transition of the pitch angle -} fixedWingLaunchData_t; - -static EXTENDED_FASTRAM fixedWingLaunchData_t fwLaunch; - -static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = { - - [FW_LAUNCH_STATE_IDLE] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE, - .onEvent = { - - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_NONE - }, - [FW_LAUNCH_STATE_WAIT_THROTTLE] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE, - .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_IDLE, - [FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_WAIT_DETECTION - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE - }, - [FW_LAUNCH_STATE_MOTOR_IDLE] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE, - .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_DETECTION, - [FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE - }, - [FW_LAUNCH_STATE_WAIT_DETECTION] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION, - .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_DETECTED, - [FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION - }, - [FW_LAUNCH_STATE_DETECTED] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_DETECTED, - .onEvent = { - // waiting for the navigation to move on the next state FW_LAUNCH_STATE_MOTOR_DELAY - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION - }, - [FW_LAUNCH_STATE_MOTOR_DELAY] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY, - .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_SPINUP, - [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS - }, - [FW_LAUNCH_STATE_MOTOR_SPINUP] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP, - .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IN_PROGRESS, - [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS - }, - [FW_LAUNCH_STATE_IN_PROGRESS] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS, - .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISH, - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS - }, - [FW_LAUNCH_STATE_FINISH] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH, - .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_FINISHING - } -}; - -/* Current State Handlers */ - -static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) { - return US2MS(currentTimeUs - fwLaunch.currentStateTimeUs); -} - -static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) { - fwLaunch.currentState = nextState; - fwLaunch.currentStateTimeUs = currentTimeUs; -} - -/* Wing control Helpers */ - -static bool isThrottleIdleEnabled(void) { - return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); -} - -static void applyThrottleIdle(void) { - if (isThrottleIdleEnabled()) { - rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle; - } else { - ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped - rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle - } -} - -static inline bool isThrottleLow(void) { - return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW; -} - -static inline bool isLaunchMaxAltitudeReached(void) { - return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); -} - -static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) { - return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband(); -} - -static void resetPidsIfNeeded(void) { - // Until motors are started don't use PID I-term and reset TPA filter - if (fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) { - pidResetErrorAccumulators(); - pidResetTPAFilter(); - } -} - -static void updateRcCommand(void) { - // lock roll and yaw and apply needed pitch angle - rcCommand[ROLL] = 0; - rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(fwLaunch.pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]); - rcCommand[YAW] = 0; -} - -/* onEntry state handlers */ - -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); - - return FW_LAUNCH_EVENT_NONE; -} - -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); - - if (!isThrottleLow()) { - if (isThrottleIdleEnabled()) { - return FW_LAUNCH_EVENT_SUCCESS; - } else { - fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle; - return FW_LAUNCH_EVENT_GOTO_DETECTION; - } - } - fwLaunch.pitchAngle = 0; - - return FW_LAUNCH_EVENT_NONE; -} - -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) { - if (isThrottleLow()) { - return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE - } - const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); - if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) { - applyThrottleIdle(); - return FW_LAUNCH_EVENT_SUCCESS; - } else { - rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); - fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle); - } - - return FW_LAUNCH_EVENT_NONE; -} - -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) { - if (isThrottleLow()) { - return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE - } - +static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs) +{ const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0; const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh); const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle))); @@ -287,149 +83,160 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); - applyThrottleIdle(); - if (isBungeeLaunched || isSwingLaunched) { - if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { - return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED + launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate); + launchState.launchDetectorPreviousUpdate = currentTimeUs; + if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) { + launchState.launchDetected = true; } - } else { - fwLaunch.currentStateTimeUs = currentTimeUs; // reset the state counter } - - return FW_LAUNCH_EVENT_NONE; + else { + launchState.launchDetectorPreviousUpdate = currentTimeUs; + launchState.launchDetectionTimeAccum = 0; + } } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); - // waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY - applyThrottleIdle(); - - return FW_LAUNCH_EVENT_NONE; +void resetFixedWingLaunchController(timeUs_t currentTimeUs) +{ + launchState.launchDetectorPreviousUpdate = currentTimeUs; + launchState.launchDetectionTimeAccum = 0; + launchState.launchStartedTime = 0; + launchState.launchDetected = false; + launchState.launchFinished = false; + launchState.motorControlAllowed = false; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) { - applyThrottleIdle(); - - if (areSticksMoved(0, currentTimeUs)) { - return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE - } - if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) { - return FW_LAUNCH_EVENT_SUCCESS; - } - - return FW_LAUNCH_EVENT_NONE; +bool isFixedWingLaunchDetected(void) +{ + return launchState.launchDetected; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) { - if (areSticksMoved(navConfig()->fw.launch_motor_timer, currentTimeUs)) { - return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE - } - - const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); - const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time; - const uint16_t launchThrottle = navConfig()->fw.launch_throttle; - - if (elapsedTimeMs > motorSpinUpMs) { - rcCommand[THROTTLE] = launchThrottle; - return FW_LAUNCH_EVENT_SUCCESS; - } else { - const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); - rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle); - } - - return FW_LAUNCH_EVENT_NONE; +void enableFixedWingLaunchController(timeUs_t currentTimeUs) +{ + launchState.launchStartedTime = currentTimeUs; + launchState.motorControlAllowed = true; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) { - rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; - - if (isLaunchMaxAltitudeReached()) { - return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state - } - if (areSticksMoved(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) { - return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state - } - if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) { - return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH - } - - return FW_LAUNCH_EVENT_NONE; +bool isFixedWingLaunchFinishedOrAborted(void) +{ + return launchState.launchFinished; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs) { - const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); - const timeMs_t endTimeMs = navConfig()->fw.launch_end_time; - - if (elapsedTimeMs > endTimeMs) { - return FW_LAUNCH_EVENT_SUCCESS; - } else { - // make a smooth transition from the launch state to the current state for throttle and the pitch angle - rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]); - fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]); - } - - return FW_LAUNCH_EVENT_NONE; +void abortFixedWingLaunch(void) +{ + launchState.launchFinished = true; } -// Public methods --------------------------------------------------------------- +#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 //ms -void applyFixedWingLaunchController(timeUs_t currentTimeUs) { +static void applyFixedWingLaunchIdleLogic(void) +{ + // Until motors are started don't use PID I-term + pidResetErrorAccumulators(); + + // We're not flying yet, reset TPA filter + pidResetTPAFilter(); + + // Throttle control logic + if (navConfig()->fw.launch_idle_throttle <= getThrottleIdleValue()) + { + ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped + rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle + } + else + { + static float timeThrottleRaisedMs; + if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) + { + timeThrottleRaisedMs = millis(); + } + else + { + const float timeSinceMotorStartMs = MIN(millis() - timeThrottleRaisedMs, LAUNCH_MOTOR_IDLE_SPINUP_TIME); + rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs, + 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, + getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); + } + } +} + +static inline bool isFixedWingLaunchMaxAltitudeReached(void) +{ + return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); +} +static inline bool isLaunchModeMinTimeElapsed(float timeSinceLaunchMs) +{ + return timeSinceLaunchMs > navConfig()->fw.launch_min_time; +} + +static inline bool isLaunchModeMaxTimeElapsed(float timeSinceLaunchMs) +{ + return timeSinceLaunchMs >= navConfig()->fw.launch_timeout; +} + +static inline bool isFixedWingLaunchCompleted(float timeSinceLaunchMs) +{ + return (isLaunchModeMaxTimeElapsed(timeSinceLaunchMs)) || ((isLaunchModeMinTimeElapsed(timeSinceLaunchMs)) && (areSticksDeflectedMoreThanPosHoldDeadband())) || isFixedWingLaunchMaxAltitudeReached(); +} + +void applyFixedWingLaunchController(timeUs_t currentTimeUs) +{ // Called at PID rate - - // process the current state, set the next state or exit if FW_LAUNCH_EVENT_NONE - while (launchStateMachine[fwLaunch.currentState].onEntry) { - fixedWingLaunchEvent_t newEvent = launchStateMachine[fwLaunch.currentState].onEntry(currentTimeUs); - if (newEvent == FW_LAUNCH_EVENT_NONE) { - break; - } - setCurrentState(launchStateMachine[fwLaunch.currentState].onEvent[newEvent], currentTimeUs); - } - resetPidsIfNeeded(); - updateRcCommand(); + if (launchState.launchDetected) { + bool applyLaunchIdleLogic = true; + + if (launchState.motorControlAllowed) { + // If launch detected we are in launch procedure - control airplane + const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs - launchState.launchStartedTime); + + launchState.launchFinished = isFixedWingLaunchCompleted(timeElapsedSinceLaunchMs); + + // Motor control enabled + if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_motor_timer) { + // Don't apply idle logic anymore + applyLaunchIdleLogic = false; + + // Increase throttle gradually over `launch_motor_spinup_time` milliseconds + if (navConfig()->fw.launch_motor_spinup_time > 0) { + const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time); + const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); + rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs, + 0.0f, navConfig()->fw.launch_motor_spinup_time, + minIdleThrottle, navConfig()->fw.launch_throttle); + } + else { + rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; + } + } + } + + if (applyLaunchIdleLogic) { + // Launch idle logic - low throttle and zero out PIDs + applyFixedWingLaunchIdleLogic(); + } + } + else { + // We are waiting for launch - update launch detector + updateFixedWingLaunchDetector(currentTimeUs); + + // Launch idle logic - low throttle and zero out PIDs + applyFixedWingLaunchIdleLogic(); + } // Control beeper - if (fwLaunch.currentState == FW_LAUNCH_STATE_WAIT_THROTTLE) { - beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); - } else { - beeper(BEEPER_LAUNCH_MODE_ENABLED); + if (!launchState.launchFinished) { + if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) { + beeper(BEEPER_LAUNCH_MODE_ENABLED); + } else { + beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); + } } -} -void resetFixedWingLaunchController(timeUs_t currentTimeUs) { - setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs); -} - -bool isFixedWingLaunchDetected(void) { - return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED; -} - -void enableFixedWingLaunchController(timeUs_t currentTimeUs) { - setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs); -} - -bool isFixedWingLaunchFinishedOrAborted(void) { - return fwLaunch.currentState == FW_LAUNCH_STATE_IDLE; -} - -void abortFixedWingLaunch(void) { - setCurrentState(FW_LAUNCH_STATE_IDLE, 0); -} - -const char * fixedWingLaunchStateMessage(void) { - switch (launchStateMachine[fwLaunch.currentState].messageType) { - case FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE: - return FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE; - case FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION: - return FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION; - case FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS: - return FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS; - case FW_LAUNCH_MESSAGE_TYPE_FINISHING: - return FW_LAUNCH_MESSAGE_TEXT_FINISHING; - default: - return NULL; - } + // Lock out controls + rcCommand[ROLL] = 0; + rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), pidProfile()->max_angle_inclination[FD_PITCH]); + rcCommand[YAW] = 0; } #endif From 41e721fed34f0701096f58f20f5f1f77f8e7f3ae Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Mon, 16 Nov 2020 15:06:10 +0100 Subject: [PATCH 105/109] Update CLI docs --- docs/Settings.md | 1 - 1 file changed, 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index c2f09410cd..a816b04841 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -263,7 +263,6 @@ | nav_fw_launch_accel | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s | | nav_fw_launch_climb_angle | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit | | nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] | -| nav_fw_launch_end_time | 2000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | | nav_fw_launch_idle_thr | 1000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) | | nav_fw_launch_max_altitude | 0 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. | | nav_fw_launch_max_angle | 45 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] | From c1c5ed15001798560ca3c4abcb771ebd296489bd Mon Sep 17 00:00:00 2001 From: Konstantin Sharlaimov Date: Mon, 16 Nov 2020 20:12:48 +0100 Subject: [PATCH 106/109] Revert "Revert "Autolaunch - Added a smooth end launch feature and code refactor"" --- docs/Cli.md | 1 - src/main/cms/cms_menu_navigation.c | 1 + src/main/fc/settings.yaml | 6 + src/main/io/osd.c | 4 + src/main/navigation/navigation.c | 1 + src/main/navigation/navigation.h | 4 +- src/main/navigation/navigation_fw_launch.c | 513 ++++++++++++++------- 7 files changed, 368 insertions(+), 162 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index d177c717c7..04332e2fbf 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -147,5 +147,4 @@ For targets that have a flash data chip, typically used for blackbox logs, the f | `flash_write
` | Writes `data` to `address` | ## CLI Variable Reference - See [Settings.md](Settings.md). diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 5e09da1066..2a70ce6fe6 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -134,6 +134,7 @@ static const OSD_Entry cmsx_menuFWLaunchEntries[] = OSD_SETTING_ENTRY("IDLE THROTTLE", SETTING_NAV_FW_LAUNCH_IDLE_THR), OSD_SETTING_ENTRY("MOTOR SPINUP TIME", SETTING_NAV_FW_LAUNCH_SPINUP_TIME), OSD_SETTING_ENTRY("TIMEOUT", SETTING_NAV_FW_LAUNCH_TIMEOUT), + OSD_SETTING_ENTRY("END TRANSITION TIME", SETTING_NAV_FW_LAUNCH_END_TIME), OSD_SETTING_ENTRY("MAX ALTITUDE", SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE), OSD_SETTING_ENTRY("CLIMB ANGLE", SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE), OSD_SETTING_ENTRY("MAX BANK ANGLE", SETTING_NAV_FW_LAUNCH_MAX_ANGLE), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c432341cf3..62c5496df7 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2371,6 +2371,12 @@ groups: field: fw.launch_motor_spinup_time min: 0 max: 1000 + - name: nav_fw_launch_end_time + description: "Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms]" + default_value: "2000" + field: fw.launch_end_time + min: 0 + max: 5000 - name: nav_fw_launch_min_time description: "Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]." default_value: "0" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 80c10d85fb..b951ec7ebc 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3327,6 +3327,10 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); + const char *launchStateMessage = fixedWingLaunchStateMessage(); + if (launchStateMessage) { + messages[messageCount++] = launchStateMessage; + } } else { if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a287162174..ba319442ca 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -171,6 +171,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .launch_idle_throttle = 1000, // Motor idle or MOTOR_STOP .launch_motor_timer = 500, // ms .launch_motor_spinup_time = 100, // ms, time to gredually increase throttle from idle to launch + .launch_end_time = 3000, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode .launch_min_time = 0, // ms, min time in launch mode .launch_timeout = 5000, // ms, timeout for launch procedure .launch_max_altitude = 0, // cm, altitude where to consider launch ended diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 36fbd2b29e..f5f0aa5990 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -239,7 +239,8 @@ typedef struct navConfig_s { uint16_t launch_throttle; // Launch throttle uint16_t launch_motor_timer; // Time to wait before setting launch_throttle (ms) uint16_t launch_motor_spinup_time; // Time to speed-up motors from idle to launch_throttle (ESC desync prevention) - uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early + uint16_t launch_end_time; // Time to make the transition from launch angle to leveled and throttle transition from launch throttle to the stick position + uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early uint16_t launch_timeout; // Launch timeout to disable launch mode and swith to normal flight (ms) uint16_t launch_max_altitude; // cm, altitude where to consider launch ended uint8_t launch_climb_angle; // Target climb angle for launch (deg) @@ -524,6 +525,7 @@ bool navigationRTHAllowsLanding(void); bool isNavLaunchEnabled(void); bool isFixedWingLaunchDetected(void); bool isFixedWingLaunchFinishedOrAborted(void); +const char * fixedWingLaunchStateMessage(void); float calculateAverageSpeed(void); diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 309bef0cf0..d87454baa1 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -33,15 +33,6 @@ #include "config/feature.h" #include "drivers/time.h" -#include "drivers/sensor.h" -#include "drivers/accgyro/accgyro.h" - -#include "sensors/sensors.h" -#include "sensors/rangefinder.h" -#include "sensors/barometer.h" -#include "sensors/acceleration.h" -#include "sensors/boardalignment.h" -#include "sensors/compass.h" #include "io/gps.h" #include "io/beeper.h" @@ -57,25 +48,238 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" - -typedef struct FixedWingLaunchState_s { - /* Launch detection */ - timeUs_t launchDetectorPreviousUpdate; - timeUs_t launchDetectionTimeAccum; - bool launchDetected; - - /* Launch progress */ - timeUs_t launchStartedTime; - bool launchFinished; - bool motorControlAllowed; -} FixedWingLaunchState_t; - -static EXTENDED_FASTRAM FixedWingLaunchState_t launchState; - -#define COS_MAX_LAUNCH_ANGLE 0.70710678f // cos(45), just to be safe #define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate -static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs) -{ +#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms +#define UNUSED(x) ((void)(x)) +#define FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE "RAISE THE THROTTLE" +#define FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION "READY" +#define FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS "MOVE THE STICKS TO ABORT" +#define FW_LAUNCH_MESSAGE_TEXT_FINISHING "FINISHING" + +typedef enum { + FW_LAUNCH_MESSAGE_TYPE_NONE = 0, + FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE, + FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION, + FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS, + FW_LAUNCH_MESSAGE_TYPE_FINISHING +} fixedWingLaunchMessage_t; + +typedef enum { + FW_LAUNCH_EVENT_NONE = 0, + FW_LAUNCH_EVENT_SUCCESS, + FW_LAUNCH_EVENT_GOTO_DETECTION, + FW_LAUNCH_EVENT_ABORT, + FW_LAUNCH_EVENT_THROTTLE_LOW, + FW_LAUNCH_EVENT_COUNT +} fixedWingLaunchEvent_t; + +typedef enum { + FW_LAUNCH_STATE_IDLE = 0, + FW_LAUNCH_STATE_WAIT_THROTTLE, + FW_LAUNCH_STATE_MOTOR_IDLE, + FW_LAUNCH_STATE_WAIT_DETECTION, + FW_LAUNCH_STATE_DETECTED, + FW_LAUNCH_STATE_MOTOR_DELAY, + FW_LAUNCH_STATE_MOTOR_SPINUP, + FW_LAUNCH_STATE_IN_PROGRESS, + FW_LAUNCH_STATE_FINISH, + FW_LAUNCH_STATE_COUNT +} fixedWingLaunchState_t; + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs); + +typedef struct fixedWingLaunchStateDescriptor_s { + fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs); + fixedWingLaunchState_t onEvent[FW_LAUNCH_EVENT_COUNT]; + fixedWingLaunchMessage_t messageType; +} fixedWingLaunchStateDescriptor_t; + +typedef struct fixedWingLaunchData_s { + timeUs_t currentStateTimeUs; + fixedWingLaunchState_t currentState; + uint8_t pitchAngle; // used to smooth the transition of the pitch angle +} fixedWingLaunchData_t; + +static EXTENDED_FASTRAM fixedWingLaunchData_t fwLaunch; + +static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = { + + [FW_LAUNCH_STATE_IDLE] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE, + .onEvent = { + + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_NONE + }, + [FW_LAUNCH_STATE_WAIT_THROTTLE] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_IDLE, + [FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_WAIT_DETECTION + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE + }, + [FW_LAUNCH_STATE_MOTOR_IDLE] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_DETECTION, + [FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE + }, + [FW_LAUNCH_STATE_WAIT_DETECTION] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_DETECTED, + [FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION + }, + [FW_LAUNCH_STATE_DETECTED] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_DETECTED, + .onEvent = { + // waiting for the navigation to move on the next state FW_LAUNCH_STATE_MOTOR_DELAY + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION + }, + [FW_LAUNCH_STATE_MOTOR_DELAY] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_SPINUP, + [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS + }, + [FW_LAUNCH_STATE_MOTOR_SPINUP] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IN_PROGRESS, + [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS + }, + [FW_LAUNCH_STATE_IN_PROGRESS] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISH, + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS + }, + [FW_LAUNCH_STATE_FINISH] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_FINISHING + } +}; + +/* Current State Handlers */ + +static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) { + return US2MS(currentTimeUs - fwLaunch.currentStateTimeUs); +} + +static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) { + fwLaunch.currentState = nextState; + fwLaunch.currentStateTimeUs = currentTimeUs; +} + +/* Wing control Helpers */ + +static bool isThrottleIdleEnabled(void) { + return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); +} + +static void applyThrottleIdle(void) { + if (isThrottleIdleEnabled()) { + rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle; + } else { + ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped + rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle + } +} + +static inline bool isThrottleLow(void) { + return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW; +} + +static inline bool isLaunchMaxAltitudeReached(void) { + return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); +} + +static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) { + return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband(); +} + +static void resetPidsIfNeeded(void) { + // Until motors are started don't use PID I-term and reset TPA filter + if (fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) { + pidResetErrorAccumulators(); + pidResetTPAFilter(); + } +} + +static void updateRcCommand(void) { + // lock roll and yaw and apply needed pitch angle + rcCommand[ROLL] = 0; + rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(fwLaunch.pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]); + rcCommand[YAW] = 0; +} + +/* onEntry state handlers */ + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); + + return FW_LAUNCH_EVENT_NONE; +} + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); + + if (!isThrottleLow()) { + if (isThrottleIdleEnabled()) { + return FW_LAUNCH_EVENT_SUCCESS; + } else { + fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle; + return FW_LAUNCH_EVENT_GOTO_DETECTION; + } + } + fwLaunch.pitchAngle = 0; + + return FW_LAUNCH_EVENT_NONE; +} + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) { + if (isThrottleLow()) { + return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE + } + const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); + if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) { + applyThrottleIdle(); + return FW_LAUNCH_EVENT_SUCCESS; + } else { + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); + fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle); + } + + return FW_LAUNCH_EVENT_NONE; +} + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) { + if (isThrottleLow()) { + return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE + } + const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0; const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh); const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle))); @@ -83,160 +287,149 @@ static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs) const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); + applyThrottleIdle(); + if (isBungeeLaunched || isSwingLaunched) { - launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate); - launchState.launchDetectorPreviousUpdate = currentTimeUs; - if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) { - launchState.launchDetected = true; + if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { + return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED } + } else { + fwLaunch.currentStateTimeUs = currentTimeUs; // reset the state counter } - else { - launchState.launchDetectorPreviousUpdate = currentTimeUs; - launchState.launchDetectionTimeAccum = 0; + + return FW_LAUNCH_EVENT_NONE; +} + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); + // waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY + applyThrottleIdle(); + + return FW_LAUNCH_EVENT_NONE; +} + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) { + applyThrottleIdle(); + + if (areSticksMoved(0, currentTimeUs)) { + return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE } -} - -void resetFixedWingLaunchController(timeUs_t currentTimeUs) -{ - launchState.launchDetectorPreviousUpdate = currentTimeUs; - launchState.launchDetectionTimeAccum = 0; - launchState.launchStartedTime = 0; - launchState.launchDetected = false; - launchState.launchFinished = false; - launchState.motorControlAllowed = false; -} - -bool isFixedWingLaunchDetected(void) -{ - return launchState.launchDetected; -} - -void enableFixedWingLaunchController(timeUs_t currentTimeUs) -{ - launchState.launchStartedTime = currentTimeUs; - launchState.motorControlAllowed = true; -} - -bool isFixedWingLaunchFinishedOrAborted(void) -{ - return launchState.launchFinished; -} - -void abortFixedWingLaunch(void) -{ - launchState.launchFinished = true; -} - -#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 //ms - -static void applyFixedWingLaunchIdleLogic(void) -{ - // Until motors are started don't use PID I-term - pidResetErrorAccumulators(); - - // We're not flying yet, reset TPA filter - pidResetTPAFilter(); - - // Throttle control logic - if (navConfig()->fw.launch_idle_throttle <= getThrottleIdleValue()) - { - ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped - rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle + if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) { + return FW_LAUNCH_EVENT_SUCCESS; } - else - { - static float timeThrottleRaisedMs; - if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) - { - timeThrottleRaisedMs = millis(); - } - else - { - const float timeSinceMotorStartMs = MIN(millis() - timeThrottleRaisedMs, LAUNCH_MOTOR_IDLE_SPINUP_TIME); - rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs, - 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, - getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); - } + + return FW_LAUNCH_EVENT_NONE; +} + +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) { + if (areSticksMoved(navConfig()->fw.launch_motor_timer, currentTimeUs)) { + return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE } + + const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); + const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time; + const uint16_t launchThrottle = navConfig()->fw.launch_throttle; + + if (elapsedTimeMs > motorSpinUpMs) { + rcCommand[THROTTLE] = launchThrottle; + return FW_LAUNCH_EVENT_SUCCESS; + } else { + const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle); + } + + return FW_LAUNCH_EVENT_NONE; } -static inline bool isFixedWingLaunchMaxAltitudeReached(void) -{ - return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); -} -static inline bool isLaunchModeMinTimeElapsed(float timeSinceLaunchMs) -{ - return timeSinceLaunchMs > navConfig()->fw.launch_min_time; +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) { + rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; + + if (isLaunchMaxAltitudeReached()) { + return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state + } + if (areSticksMoved(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) { + return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state + } + if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) { + return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH + } + + return FW_LAUNCH_EVENT_NONE; } -static inline bool isLaunchModeMaxTimeElapsed(float timeSinceLaunchMs) -{ - return timeSinceLaunchMs >= navConfig()->fw.launch_timeout; +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs) { + const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); + const timeMs_t endTimeMs = navConfig()->fw.launch_end_time; + + if (elapsedTimeMs > endTimeMs) { + return FW_LAUNCH_EVENT_SUCCESS; + } else { + // make a smooth transition from the launch state to the current state for throttle and the pitch angle + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]); + fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]); + } + + return FW_LAUNCH_EVENT_NONE; } -static inline bool isFixedWingLaunchCompleted(float timeSinceLaunchMs) -{ - return (isLaunchModeMaxTimeElapsed(timeSinceLaunchMs)) || ((isLaunchModeMinTimeElapsed(timeSinceLaunchMs)) && (areSticksDeflectedMoreThanPosHoldDeadband())) || isFixedWingLaunchMaxAltitudeReached(); -} +// Public methods --------------------------------------------------------------- -void applyFixedWingLaunchController(timeUs_t currentTimeUs) -{ +void applyFixedWingLaunchController(timeUs_t currentTimeUs) { // Called at PID rate - - if (launchState.launchDetected) { - bool applyLaunchIdleLogic = true; - - if (launchState.motorControlAllowed) { - // If launch detected we are in launch procedure - control airplane - const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs - launchState.launchStartedTime); - - launchState.launchFinished = isFixedWingLaunchCompleted(timeElapsedSinceLaunchMs); - - // Motor control enabled - if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_motor_timer) { - // Don't apply idle logic anymore - applyLaunchIdleLogic = false; - - // Increase throttle gradually over `launch_motor_spinup_time` milliseconds - if (navConfig()->fw.launch_motor_spinup_time > 0) { - const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time); - const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); - rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs, - 0.0f, navConfig()->fw.launch_motor_spinup_time, - minIdleThrottle, navConfig()->fw.launch_throttle); - } - else { - rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; - } - } - } - - if (applyLaunchIdleLogic) { - // Launch idle logic - low throttle and zero out PIDs - applyFixedWingLaunchIdleLogic(); + + // process the current state, set the next state or exit if FW_LAUNCH_EVENT_NONE + while (launchStateMachine[fwLaunch.currentState].onEntry) { + fixedWingLaunchEvent_t newEvent = launchStateMachine[fwLaunch.currentState].onEntry(currentTimeUs); + if (newEvent == FW_LAUNCH_EVENT_NONE) { + break; } + setCurrentState(launchStateMachine[fwLaunch.currentState].onEvent[newEvent], currentTimeUs); } - else { - // We are waiting for launch - update launch detector - updateFixedWingLaunchDetector(currentTimeUs); - // Launch idle logic - low throttle and zero out PIDs - applyFixedWingLaunchIdleLogic(); - } + resetPidsIfNeeded(); + updateRcCommand(); // Control beeper - if (!launchState.launchFinished) { - if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) { - beeper(BEEPER_LAUNCH_MODE_ENABLED); - } else { - beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); - } + if (fwLaunch.currentState == FW_LAUNCH_STATE_WAIT_THROTTLE) { + beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); + } else { + beeper(BEEPER_LAUNCH_MODE_ENABLED); } +} - // Lock out controls - rcCommand[ROLL] = 0; - rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), pidProfile()->max_angle_inclination[FD_PITCH]); - rcCommand[YAW] = 0; +void resetFixedWingLaunchController(timeUs_t currentTimeUs) { + setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs); +} + +bool isFixedWingLaunchDetected(void) { + return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED; +} + +void enableFixedWingLaunchController(timeUs_t currentTimeUs) { + setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs); +} + +bool isFixedWingLaunchFinishedOrAborted(void) { + return fwLaunch.currentState == FW_LAUNCH_STATE_IDLE; +} + +void abortFixedWingLaunch(void) { + setCurrentState(FW_LAUNCH_STATE_IDLE, 0); +} + +const char * fixedWingLaunchStateMessage(void) { + switch (launchStateMachine[fwLaunch.currentState].messageType) { + case FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE: + return FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE; + case FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION: + return FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION; + case FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS: + return FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS; + case FW_LAUNCH_MESSAGE_TYPE_FINISHING: + return FW_LAUNCH_MESSAGE_TEXT_FINISHING; + default: + return NULL; + } } #endif From 94332bc2864af99a1e01c6415fd48e1066b603e2 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Mon, 16 Nov 2020 20:29:11 +0100 Subject: [PATCH 107/109] [NAV/MIXER] Refactor getMotorStop() code; Avoid recursion and crash when nav_overrides_motor_stop is set to NOMS_ALL_NAV --- docs/Settings.md | 1 + src/main/flight/mixer.c | 29 +++++++++++++++++++++++------ src/main/navigation/navigation.c | 10 ++++++++-- src/main/navigation/navigation.h | 1 + 4 files changed, 33 insertions(+), 8 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index a816b04841..c2f09410cd 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -263,6 +263,7 @@ | nav_fw_launch_accel | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s | | nav_fw_launch_climb_angle | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit | | nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] | +| nav_fw_launch_end_time | 2000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | | nav_fw_launch_idle_thr | 1000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) | | nav_fw_launch_max_altitude | 0 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. | | nav_fw_launch_max_angle | 45 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] | diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index cc65c2b258..346de5cb07 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -574,15 +574,32 @@ void FAST_CODE mixTable(const float dT) motorStatus_e getMotorStatus(void) { - if (failsafeRequiresMotorStop() || (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE))) { + if (failsafeRequiresMotorStop()) { return MOTOR_STOPPED_AUTO; } - if (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) { - navOverridesMotorStop_e motorStopOverride = navConfig()->general.flags.nav_overrides_motor_stop; - if (!failsafeIsActive() && (STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) && - ((motorStopOverride == NOMS_OFF) || ((motorStopOverride == NOMS_ALL_NAV) && !navigationIsControllingThrottle()) || ((motorStopOverride == NOMS_AUTO_ONLY) && !navigationIsFlyingAutonomousMode()))) { - return MOTOR_STOPPED_USER; + if (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE)) { + return MOTOR_STOPPED_AUTO; + } + + const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE); + const bool throttleStickLow = + (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW); + + if (throttleStickLow && fixedWingOrAirmodeNotActive && !failsafeIsActive()) { + // If user is holding stick low, we are not in failsafe and either on a plane or on a quad with inactive + // airmode - we need to check if we are allowing navigation to override MOTOR_STOP + + switch (navConfig()->general.flags.nav_overrides_motor_stop) { + case NOMS_ALL_NAV: + return navigationInAutomaticThrottleMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER; + + case NOMS_AUTO_ONLY: + return navigationIsFlyingAutonomousMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER; + + case NOMS_OFF: + default: + return MOTOR_STOPPED_USER; } } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index ba319442ca..da53648ab7 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3649,10 +3649,16 @@ bool navigationIsExecutingAnEmergencyLanding(void) return navGetCurrentStateFlags() & NAV_CTL_EMERG; } -bool navigationIsControllingThrottle(void) +bool navigationInAutomaticThrottleMode(void) { navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); - return ((stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND)) && (getMotorStatus() != MOTOR_STOPPED_USER)); + return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND)); +} + +bool navigationIsControllingThrottle(void) +{ + // Note that this makes a detour into mixer code to evaluate actual motor status + return navigationInAutomaticThrottleMode() && (getMotorStatus() != MOTOR_STOPPED_USER); } bool navigationIsFlyingAutonomousMode(void) diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index f5f0aa5990..c50f03b86b 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -513,6 +513,7 @@ void abortForcedRTH(void); rthState_e getStateOfForcedRTH(void); /* Getter functions which return data about the state of the navigation system */ +bool navigationInAutomaticThrottleMode(void); bool navigationIsControllingThrottle(void); bool isFixedWingAutoThrottleManuallyIncreased(void); bool navigationIsFlyingAutonomousMode(void); From 7ca1dda83e08b02e4edda9379aa8f9299bcc014c Mon Sep 17 00:00:00 2001 From: dragnea Date: Mon, 16 Nov 2020 23:31:01 +0200 Subject: [PATCH 108/109] Add Allow Manual Throttle Increase in the Fixed Wing/Cruise CMS menu --- src/main/cms/cms_menu_navigation.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 2a70ce6fe6..d17d3df290 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -111,6 +111,7 @@ static const OSD_Entry cmsx_menuFWCruiseEntries[] = OSD_SETTING_ENTRY("CONTROL SMOOTHNESS", SETTING_NAV_FW_CONTROL_SMOOTHNESS), OSD_SETTING_ENTRY("PITCH TO THR SMOOTHING", SETTING_NAV_FW_PITCH2THR_SMOOTHING), OSD_SETTING_ENTRY("PITCH TO THR THRESHOLD", SETTING_NAV_FW_PITCH2THR_THRESHOLD), + OSD_SETTING_ENTRY("MANUAL THR INCREASE", SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE), OSD_BACK_AND_END_ENTRY, }; From 62374122f0d3a8f4c6cda7d2bb28a56c9c1e5262 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Tue, 17 Nov 2020 08:13:54 +0100 Subject: [PATCH 109/109] [RC] Remove ADJUSTMENT_PROFILE which was never functional --- src/main/fc/rc_adjustments.c | 6 ------ src/main/fc/rc_adjustments.h | 3 --- 2 files changed, 9 deletions(-) diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 13844ac8c2..415032f758 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -276,12 +276,6 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_TPA_BREAKPOINT, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 5 }} -#ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT - }, { - .adjustmentFunction = ADJUSTMENT_PROFILE, - .mode = ADJUSTMENT_MODE_SELECT, - .data = { .selectConfig = { .switchPositions = 3 }} -#endif } }; diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index 75214d6c68..625f152ff1 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -77,9 +77,6 @@ typedef enum { ADJUSTMENT_VTX_POWER_LEVEL = 49, ADJUSTMENT_TPA = 50, ADJUSTMENT_TPA_BREAKPOINT = 51, -#ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT - ADJUSTMENT_PROFILE = 52, -#endif ADJUSTMENT_FUNCTION_COUNT // must be last } adjustmentFunction_e;