diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 98a43854cf2..4e776621da9 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -209,7 +209,7 @@ main_sources(COMMON_SRC drivers/pitotmeter/pitotmeter_ms4525.c drivers/pitotmeter/pitotmeter_ms4525.h drivers/pitotmeter/pitotmeter_dlvr_l10d.c - drivers/pitotmeter/pitotmeter_dlvr_l10d.h + drivers/pitotmeter/pitotmeter_dlvr_l10d.h drivers/pitotmeter/pitotmeter_msp.c drivers/pitotmeter/pitotmeter_msp.h drivers/pitotmeter/pitotmeter_virtual.c diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 453b8fda0cc..886bb2b79e1 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -474,7 +474,7 @@ typedef struct blackboxMainState_s { int32_t mcSurfacePID[3]; int32_t mcSurfacePIDOutput; - int32_t fwAltPID[3]; + int32_t fwAltPID[3]; // CR97 int32_t fwAltPIDOutput; int32_t fwPosPID[3]; int32_t fwPosPIDOutput; diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 7612ec44181..740f9e2a945 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -198,6 +198,8 @@ static const OSD_Entry cmsx_menuPidAltMagEntries[] = { OSD_LABEL_DATA_ENTRY("-- ALT&MAG --", profileIndexString), + OSD_SETTING_ENTRY("FW ALT RESPONSE", SETTING_NAV_FW_ALT_CONTROL_RESPONSE), // CR97A + OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P), OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I), OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D), @@ -256,7 +258,6 @@ static const OSD_Entry cmsx_menuPidGpsnavEntries[] = OTHER_PIDFF_ENTRY("VEL I", &cmsx_pidVelXY.I), OTHER_PIDFF_ENTRY("VEL D", &cmsx_pidVelXY.D), OTHER_PIDFF_ENTRY("VEL FF", &cmsx_pidVelXY.FF), - OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index f5b12301028..cfd5e8690d5 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -51,7 +51,10 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] = OSD_SETTING_ENTRY("MC MAX BANK ANGLE", SETTING_NAV_MC_BANK_ANGLE), OSD_SETTING_ENTRY("MC ALTHOLD THROT", SETTING_NAV_MC_ALTHOLD_THROTTLE), OSD_SETTING_ENTRY("MC HOVER THR", SETTING_NAV_MC_HOVER_THR), + OSD_SETTING_ENTRY("USE FW YAW CONTROL", SETTING_NAV_USE_FW_YAW_CONTROL), + OSD_SETTING_ENTRY("LAND SENSITIVITY", SETTING_NAV_LAND_DETECT_SENSITIVITY), OSD_SETTING_ENTRY("LANDING DISARM", SETTING_NAV_DISARM_ON_LANDING), + OSD_SETTING_ENTRY("MC DTERM ATT START", SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START), // CR47 OSD_BACK_AND_END_ENTRY, }; @@ -106,7 +109,7 @@ static const CMS_Menu cmsx_menuRTH = { static const OSD_Entry cmsx_menuFWCruiseEntries[] = { - OSD_LABEL_ENTRY("-- CRUISE --"), + OSD_LABEL_ENTRY("-- GENERAL --"), OSD_SETTING_ENTRY("CRUISE THROTTLE", SETTING_NAV_FW_CRUISE_THR), OSD_SETTING_ENTRY("MIN THROTTLE", SETTING_NAV_FW_MIN_THR), @@ -172,7 +175,7 @@ static const OSD_Entry cmsx_menuFWSettingsEntries[] = OSD_LABEL_ENTRY("-- FIXED WING --"), OSD_SUBMENU_ENTRY("AUTOLAUNCH", &cmsx_menuFWLaunch), - OSD_SUBMENU_ENTRY("CRUISE", &cmsx_menuFWCruise), + OSD_SUBMENU_ENTRY("GENERAL", &cmsx_menuFWCruise), OSD_BACK_AND_END_ENTRY, }; @@ -193,6 +196,7 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] = OSD_LABEL_ENTRY("-- MISSIONS --"), OSD_SETTING_ENTRY("MC WP SLOWDOWN", SETTING_NAV_MC_WP_SLOWDOWN), + OSD_SETTING_ENTRY("MISSION RESTART", SETTING_NAV_WP_MISSION_RESTART), OSD_SETTING_ENTRY("WP FAILSAFE DELAY", SETTING_FAILSAFE_MISSION_DELAY), OSD_SETTING_ENTRY("WP LOAD ON BOOT", SETTING_NAV_WP_LOAD_ON_BOOT), OSD_SETTING_ENTRY("WP REACHED RADIUS", SETTING_NAV_WP_RADIUS), @@ -201,7 +205,6 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] = #ifdef USE_MULTI_MISSION OSD_SETTING_ENTRY("MULTI MISSION NUMBER", SETTING_NAV_WP_MULTI_MISSION_INDEX), #endif - OSD_SETTING_ENTRY("MISSION RESTART", SETTING_NAV_WP_MISSION_RESTART), OSD_SETTING_ENTRY("WP TURN SMOOTHING", SETTING_NAV_FW_WP_TURN_SMOOTHING), OSD_SETTING_ENTRY("WP TRACKING ACCURACY", SETTING_NAV_FW_WP_TRACKING_ACCURACY), OSD_BACK_AND_END_ENTRY, diff --git a/src/main/common/fp_pid.c b/src/main/common/fp_pid.c index df8ccaad6b7..870fbf564d9 100644 --- a/src/main/common/fp_pid.c +++ b/src/main/common/fp_pid.c @@ -40,13 +40,13 @@ float navPidApply3( const float dt, const float outMin, const float outMax, - const pidControllerFlags_e pidFlags, + pidControllerFlags_e pidFlags, // CR99 const float gainScaler, const float dTermScaler ) { float newProportional, newDerivative, newFeedForward; float error = 0.0f; - + if (pid->errorLpfHz > 0.0f) { error = pt1FilterApply4(&pid->error_filter_state, setpoint - measurement, pid->errorLpfHz, dt); } else { @@ -79,7 +79,12 @@ float navPidApply3( } newDerivative = newDerivative * gainScaler * dTermScaler; - + // CR99 + // zero integral if proportional saturated + // if (newProportional > outMax || newProportional < outMin) { + // pidFlags |= PID_ZERO_INTEGRATOR; + // } + // CR99 if (pidFlags & PID_ZERO_INTEGRATOR) { pid->integrator = 0.0f; } @@ -107,7 +112,7 @@ float navPidApply3( /* Update I-term */ if ( !(pidFlags & PID_ZERO_INTEGRATOR) && - !(pidFlags & PID_FREEZE_INTEGRATOR) + !(pidFlags & PID_FREEZE_INTEGRATOR) ) { const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + (backCalc * pid->param.kT * dt); @@ -121,10 +126,10 @@ float navPidApply3( pid->integrator = newIntegrator; } } - + if (pidFlags & PID_LIMIT_INTEGRATOR) { pid->integrator = constrainf(pid->integrator, outMin, outMax); - } + } return outValConstrained; } diff --git a/src/main/common/fp_pid.h b/src/main/common/fp_pid.h index c45b2719dd5..67fed9dc381 100644 --- a/src/main/common/fp_pid.h +++ b/src/main/common/fp_pid.h @@ -70,7 +70,7 @@ float navPidApply3( const float dt, const float outMin, const float outMax, - const pidControllerFlags_e pidFlags, + pidControllerFlags_e pidFlags, // CR99 const float gainScaler, const float dTermScaler ); diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index 0ed37ddb4e6..fdf0b1fbc0b 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -178,8 +178,8 @@ int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c) #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { - //some FCs do not power max7456 from USB power - //driver can not read font metadata + //some FCs do not power max7456 from USB power + //driver can not read font metadata //chip assumed to not support second bank of font //artifical horizon, variometer and home direction are not drawn ( display.c: displayUpdateMaxChar()) //return dummy metadata to let all OSD elements to work in simulator mode diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ea09c5776d6..51e38bc9c67 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -143,9 +143,8 @@ static void cliAssert(char *cmdline); #endif #ifdef USE_CLI_BATCH -static bool commandBatchActive = false; -static bool commandBatchError = false; -static uint8_t commandBatchErrorCount = 0; +static bool commandBatchActive = false; +static bool commandBatchError = false; #endif // sync this with features_e @@ -258,7 +257,6 @@ static void cliPrintError(const char *str) #ifdef USE_CLI_BATCH if (commandBatchActive) { commandBatchError = true; - commandBatchErrorCount++; } #endif } @@ -270,7 +268,6 @@ static void cliPrintErrorLine(const char *str) #ifdef USE_CLI_BATCH if (commandBatchActive) { commandBatchError = true; - commandBatchErrorCount++; } #endif } @@ -373,7 +370,6 @@ static void cliPrintErrorVa(const char *format, va_list va) #ifdef USE_CLI_BATCH if (commandBatchActive) { commandBatchError = true; - commandBatchErrorCount++; } #endif } @@ -665,7 +661,6 @@ static void cliAssert(char *cmdline) #ifdef USE_CLI_BATCH if (commandBatchActive) { commandBatchError = true; - commandBatchErrorCount++; } #endif } @@ -3424,10 +3419,7 @@ static void cliDumpMixerProfile(uint8_t profileIndex, uint8_t dumpMask) #ifdef USE_CLI_BATCH static void cliPrintCommandBatchWarning(const char *warning) { - char errorBuf[59]; - tfp_sprintf(errorBuf, "%d ERRORS WERE DETECTED - Please review and fix before continuing!", commandBatchErrorCount); - - cliPrintErrorLinef(errorBuf); + cliPrintErrorLinef("ERRORS WERE DETECTED - PLEASE REVIEW BEFORE CONTINUING"); if (warning) { cliPrintErrorLinef(warning); } @@ -3437,7 +3429,6 @@ static void resetCommandBatch(void) { commandBatchActive = false; commandBatchError = false; - commandBatchErrorCount = 0; } static void cliBatch(char *cmdline) @@ -3446,7 +3437,6 @@ static void cliBatch(char *cmdline) if (!commandBatchActive) { commandBatchActive = true; commandBatchError = false; - commandBatchErrorCount = 0; } cliPrintLine("Command batch started"); } else if (strncasecmp(cmdline, "end", 3) == 0) { @@ -3694,8 +3684,8 @@ static void cliStatus(char *cmdline) #if defined(AT32F43x) cliPrintLine("AT32 system clocks:"); crm_clocks_freq_type clocks; - crm_clocks_freq_get(&clocks); - + crm_clocks_freq_get(&clocks); + cliPrintLinef(" SYSCLK = %d MHz", clocks.sclk_freq / 1000000); cliPrintLinef(" ABH = %d MHz", clocks.ahb_freq / 1000000); cliPrintLinef(" ABP1 = %d MHz", clocks.apb1_freq / 1000000); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 0d0863760de..203ef33f591 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -551,7 +551,6 @@ void tryArm(void) return; } #endif - #ifdef USE_PROGRAMMING_FRAMEWORK if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled() || LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) { @@ -741,7 +740,7 @@ void processRx(timeUs_t currentTimeUs) // Handle passthrough mode if (STATE(FIXED_WING_LEGACY)) { if ((IS_RC_MODE_ACTIVE(BOXMANUAL) && !navigationRequiresAngleMode() && !failsafeRequiresAngleMode()) || // Normal activation of passthrough - (!ARMING_FLAG(ARMED) && areSensorsCalibrating())){ // Backup - if we are not armed - enforce passthrough while calibrating + (!ARMING_FLAG(ARMED) && areSensorsCalibrating())){ // Backup - if we are not armed - enforce passthrough while calibrating ENABLE_FLIGHT_MODE(MANUAL_MODE); } else { DISABLE_FLIGHT_MODE(MANUAL_MODE); @@ -980,6 +979,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) blackboxUpdate(micros()); } #endif + } // This function is called in a busy-loop, everything called from here should do it's own diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index c66a0050ba2..ec05c254090 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -34,7 +34,6 @@ typedef enum disarmReason_e { DISARM_REASON_COUNT } disarmReason_t; - void handleInflightCalibrationStickPosition(void); void disarm(disarmReason_t disarmReason); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 2c3f40dfe45..846b567b31f 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -20,6 +20,10 @@ #include #include #include +// CR97 hitl test +// #include +// #include +// CR97 #include "common/log.h" //for MSP_SIMULATOR #include "platform.h" @@ -1113,7 +1117,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF legacyLedConfig |= ledConfig->led_function << shiftCount; shiftCount += 4; legacyLedConfig |= (ledConfig->led_overlay & 0x3F) << (shiftCount); - shiftCount += 6; + shiftCount += 6; legacyLedConfig |= (ledConfig->led_color) << (shiftCount); shiftCount += 4; legacyLedConfig |= (ledConfig->led_direction) << (shiftCount); @@ -1340,9 +1344,10 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_NAV_POSHOLD: sbufWriteU8(dst, navConfig()->general.flags.user_control_mode); sbufWriteU16(dst, navConfig()->general.max_auto_speed); - sbufWriteU16(dst, navConfig()->mc.max_auto_climb_rate); + // sbufWriteU16(dst, navConfig()->mc.max_auto_climb_rate); // CR97A + sbufWriteU16(dst, mixerConfig()->platformType == PLATFORM_AIRPLANE ? navConfig()->fw.max_auto_climb_rate : navConfig()->mc.max_auto_climb_rate); sbufWriteU16(dst, navConfig()->general.max_manual_speed); - sbufWriteU16(dst, mixerConfig()->platformType != PLATFORM_AIRPLANE ? navConfig()->mc.max_manual_climb_rate:navConfig()->fw.max_manual_climb_rate); + sbufWriteU16(dst, mixerConfig()->platformType == PLATFORM_AIRPLANE ? navConfig()->fw.max_manual_climb_rate : navConfig()->mc.max_manual_climb_rate); sbufWriteU8(dst, navConfig()->mc.max_bank_angle); sbufWriteU8(dst, navConfig()->mc.althold_throttle_type); sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle); @@ -1590,7 +1595,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, timerHardware[i].usageFlags); } break; - + case MSP2_INAV_MC_BRAKING: #ifdef USE_MR_BRAKING_MODE sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold); @@ -2394,12 +2399,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (dataSize == 13) { navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src); navConfigMutable()->general.max_auto_speed = sbufReadU16(src); - navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src); + // navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src); + // CR97A + if (mixerConfig()->platformType == PLATFORM_AIRPLANE) { + navConfigMutable()->fw.max_auto_climb_rate = sbufReadU16(src); + } else { + navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src); + } + // CR97A navConfigMutable()->general.max_manual_speed = sbufReadU16(src); - if (mixerConfig()->platformType != PLATFORM_AIRPLANE) { - navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src); - }else{ + if (mixerConfig()->platformType == PLATFORM_AIRPLANE) { navConfigMutable()->fw.max_manual_climb_rate = sbufReadU16(src); + } else { + navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src); } navConfigMutable()->mc.max_bank_angle = sbufReadU8(src); navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src); @@ -2699,9 +2711,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #ifdef USE_GPS case MSP_SET_RAW_GPS: if (dataSize == 14) { - gpsSol.fixType = sbufReadU8(src); - if (gpsSol.fixType) { - ENABLE_STATE(GPS_FIX); + gpsSol.fixType = sbufReadU8(src); + if (gpsSol.fixType) { + ENABLE_STATE(GPS_FIX); } else { DISABLE_STATE(GPS_FIX); } @@ -2729,7 +2741,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_WP: if (dataSize == 21) { - + const uint8_t msp_wp_no = sbufReadU8(src); // get the waypoint number navWaypoint_t msp_wp; msp_wp.action = sbufReadU8(src); // action @@ -2943,7 +2955,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) ledConfig->led_position = legacyConfig & 0xFF; ledConfig->led_function = (legacyConfig >> 8) & 0xF; ledConfig->led_overlay = (legacyConfig >> 12) & 0x3F; - ledConfig->led_color = (legacyConfig >> 18) & 0xF; + ledConfig->led_color = (legacyConfig >> 18) & 0xF; ledConfig->led_direction = (legacyConfig >> 22) & 0x3F; ledConfig->led_params = (legacyConfig >> 28) & 0xF; @@ -3039,16 +3051,16 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP2_INAV_SET_MIXER: if (dataSize == 9) { - mixerConfigMutable()->motorDirectionInverted = sbufReadU8(src); - sbufReadU8(src); // Was yaw_jump_prevention_limit + mixerConfigMutable()->motorDirectionInverted = sbufReadU8(src); + sbufReadU8(src); // Was yaw_jump_prevention_limit mixerConfigMutable()->motorstopOnLow = sbufReadU8(src); - mixerConfigMutable()->platformType = sbufReadU8(src); - mixerConfigMutable()->hasFlaps = sbufReadU8(src); - mixerConfigMutable()->appliedMixerPreset = sbufReadU16(src); - sbufReadU8(src); //Read and ignore MAX_SUPPORTED_MOTORS - sbufReadU8(src); //Read and ignore MAX_SUPPORTED_SERVOS - mixerUpdateStateFlags(); - } else + mixerConfigMutable()->platformType = sbufReadU8(src); + mixerConfigMutable()->hasFlaps = sbufReadU8(src); + mixerConfigMutable()->appliedMixerPreset = sbufReadU16(src); + sbufReadU8(src); //Read and ignore MAX_SUPPORTED_MOTORS + sbufReadU8(src); //Read and ignore MAX_SUPPORTED_SERVOS + mixerUpdateStateFlags(); + } else return MSP_RESULT_ERROR; break; @@ -3233,7 +3245,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) fwAutolandApproachConfigMutable(i)->approachAlt = sbufReadU32(src); fwAutolandApproachConfigMutable(i)->landAlt = sbufReadU32(src); fwAutolandApproachConfigMutable(i)->approachDirection = sbufReadU8(src); - + int16_t head1 = 0, head2 = 0; if (sbufReadI16Safe(&head1, src)) { fwAutolandApproachConfigMutable(i)->landApproachHeading1 = head1; @@ -3283,12 +3295,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionEnd = sbufReadU8(src); ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightCenter = sbufReadU8(src); ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightEnd = sbufReadU8(src); - + } else { return MSP_RESULT_ERROR; } - break; + break; #endif #ifdef USE_PROGRAMMING_FRAMEWORK @@ -3561,7 +3573,7 @@ bool isOSDTypeSupportedBySimulator(void) { #ifdef USE_OSD displayPort_t *osdDisplayPort = osdGetDisplayPort(); - return (!!osdDisplayPort && !!osdDisplayPort->vTable->readChar); + return (!!osdDisplayPort && !!osdDisplayPort->vTable->readChar); #else return false; #endif @@ -3569,144 +3581,146 @@ bool isOSDTypeSupportedBySimulator(void) void mspWriteSimulatorOSD(sbuf_t *dst) { - //RLE encoding - //scan displayBuffer iteratively - //no more than 80+3+2 bytes output in single run - //0 and 255 are special symbols - //255 [char] - font bank switch - //0 [flags,count] [char] - font bank switch, blink switch and character repeat + //RLE encoding + //scan displayBuffer iteratively + //no more than 80+3+2 bytes output in single run + //0 and 255 are special symbols + //255 [char] - font bank switch + //0 [flags,count] [char] - font bank switch, blink switch and character repeat //original 0 is sent as 32 //original 0xff, 0x100 and 0x1ff are forcibly sent inside command 0 - static uint8_t osdPos_y = 0; - static uint8_t osdPos_x = 0; + static uint8_t osdPos_y = 0; + static uint8_t osdPos_x = 0; //indicate new format hitl 1.4.0 - sbufWriteU8(dst, 255); - - if (isOSDTypeSupportedBySimulator()) - { - displayPort_t *osdDisplayPort = osdGetDisplayPort(); - - sbufWriteU8(dst, osdDisplayPort->rows); - sbufWriteU8(dst, osdDisplayPort->cols); - - sbufWriteU8(dst, osdPos_y); - sbufWriteU8(dst, osdPos_x); - - int bytesCount = 0; - - uint16_t c = 0; - textAttributes_t attr = 0; - bool highBank = false; - bool blink = false; - int count = 0; - - int processedRows = osdDisplayPort->rows; - - while (bytesCount < 80) //whole response should be less 155 bytes at worst. - { - bool blink1; - uint16_t lastChar; - - count = 0; - while ( true ) - { - displayReadCharWithAttr(osdDisplayPort, osdPos_x, osdPos_y, &c, &attr); - if (c == 0) c = 32; - - //REVIEW: displayReadCharWithAttr() should return mode with _TEXT_ATTRIBUTES_BLINK_BIT ! - //for max7456 it returns mode with MAX7456_MODE_BLINK instead (wrong) - //because max7456ReadChar() does not decode from MAX7456_MODE_BLINK to _TEXT_ATTRIBUTES_BLINK_BIT - //it should! - - //bool blink2 = TEXT_ATTRIBUTES_HAVE_BLINK(attr); - bool blink2 = attr & (1<<4); //MAX7456_MODE_BLINK - - if (count == 0) - { - lastChar = c; - blink1 = blink2; - } - else if ((lastChar != c) || (blink2 != blink1) || (count == 63)) - { - break; - } - - count++; - - osdPos_x++; - if (osdPos_x == osdDisplayPort->cols) - { - osdPos_x = 0; - osdPos_y++; - processedRows--; - if (osdPos_y == osdDisplayPort->rows) - { - osdPos_y = 0; - } - } - } - - uint8_t cmd = 0; + sbufWriteU8(dst, 255); + + if (isOSDTypeSupportedBySimulator()) + { + displayPort_t *osdDisplayPort = osdGetDisplayPort(); + + sbufWriteU8(dst, osdDisplayPort->rows); + sbufWriteU8(dst, osdDisplayPort->cols); + + sbufWriteU8(dst, osdPos_y); + sbufWriteU8(dst, osdPos_x); + + int bytesCount = 0; + + uint16_t c = 0; + textAttributes_t attr = 0; + bool highBank = false; + bool blink = false; + int count = 0; + + int processedRows = osdDisplayPort->rows; + + while (bytesCount < 80) //whole response should be less 155 bytes at worst. + { + bool blink1; + uint16_t lastChar; + + count = 0; + while ( true ) + { + displayReadCharWithAttr(osdDisplayPort, osdPos_x, osdPos_y, &c, &attr); + if (c == 0) c = 32; + + //REVIEW: displayReadCharWithAttr() should return mode with _TEXT_ATTRIBUTES_BLINK_BIT ! + //for max7456 it returns mode with MAX7456_MODE_BLINK instead (wrong) + //because max7456ReadChar() does not decode from MAX7456_MODE_BLINK to _TEXT_ATTRIBUTES_BLINK_BIT + //it should! + + //bool blink2 = TEXT_ATTRIBUTES_HAVE_BLINK(attr); + bool blink2 = attr & (1<<4); //MAX7456_MODE_BLINK + + if (count == 0) + { + lastChar = c; + blink1 = blink2; + } + else if ((lastChar != c) || (blink2 != blink1) || (count == 63)) + { + break; + } + + count++; + + osdPos_x++; + if (osdPos_x == osdDisplayPort->cols) + { + osdPos_x = 0; + osdPos_y++; + processedRows--; + if (osdPos_y == osdDisplayPort->rows) + { + osdPos_y = 0; + } + } + } + + uint8_t cmd = 0; uint8_t lastCharLow = (uint8_t)(lastChar & 0xff); - if (blink1 != blink) - { - cmd |= 128;//switch blink attr - blink = blink1; - } - - bool highBank1 = lastChar > 255; - if (highBank1 != highBank) - { - cmd |= 64;//switch bank attr - highBank = highBank1; - } - - if (count == 1 && cmd == 64) - { - sbufWriteU8(dst, 255); //short command for bank switch with char following - sbufWriteU8(dst, lastChar & 0xff); - bytesCount += 2; - } - else if ((count > 2) || (cmd !=0) || (lastChar == 255) || (lastChar == 0x100) || (lastChar == 0x1ff)) - { - cmd |= count; //long command for blink/bank switch and symbol repeat - sbufWriteU8(dst, 0); - sbufWriteU8(dst, cmd); - sbufWriteU8(dst, lastCharLow); - bytesCount += 3; - } - else if (count == 2) //cmd == 0 here - { - sbufWriteU8(dst, lastCharLow); - sbufWriteU8(dst, lastCharLow); - bytesCount+=2; - } - else - { - sbufWriteU8(dst, lastCharLow); - bytesCount++; - } - - if ( processedRows <= 0 ) - { - break; - } - } - sbufWriteU8(dst, 0); //command 0 with length=0 -> stop - sbufWriteU8(dst, 0); - } - else - { - sbufWriteU8(dst, 0); - } + if (blink1 != blink) + { + cmd |= 128;//switch blink attr + blink = blink1; + } + + bool highBank1 = lastChar > 255; + if (highBank1 != highBank) + { + cmd |= 64;//switch bank attr + highBank = highBank1; + } + + if (count == 1 && cmd == 64) + { + sbufWriteU8(dst, 255); //short command for bank switch with char following + sbufWriteU8(dst, lastChar & 0xff); + bytesCount += 2; + } + else if ((count > 2) || (cmd !=0) || (lastChar == 255) || (lastChar == 0x100) || (lastChar == 0x1ff)) + { + cmd |= count; //long command for blink/bank switch and symbol repeat + sbufWriteU8(dst, 0); + sbufWriteU8(dst, cmd); + sbufWriteU8(dst, lastCharLow); + bytesCount += 3; + } + else if (count == 2) //cmd == 0 here + { + sbufWriteU8(dst, lastCharLow); + sbufWriteU8(dst, lastCharLow); + bytesCount+=2; + } + else + { + sbufWriteU8(dst, lastCharLow); + bytesCount++; + } + + if ( processedRows <= 0 ) + { + break; + } + } + sbufWriteU8(dst, 0); //command 0 with length=0 -> stop + sbufWriteU8(dst, 0); + } + else + { + sbufWriteU8(dst, 0); + } } #endif bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResult_e *ret) { +#ifdef USE_SIMULATOR uint8_t tmp_u8; +#endif const unsigned int dataSize = sbufBytesRemaining(src); switch (cmdMSP) { @@ -3788,7 +3802,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu #ifdef USE_SIMULATOR case MSP_SIMULATOR: tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version - + // Check the MSP version of simulator if (tmp_u8 != SIMULATOR_MSP_VERSION) { break; @@ -3802,9 +3816,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); #ifdef USE_BARO - if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) { - baroStartCalibration(); - } + if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) { + baroStartCalibration(); + } #endif #ifdef USE_MAG DISABLE_STATE(COMPASS_CALIBRATED); @@ -3814,7 +3828,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu // Review: Many states were affected. Reboot? disarm(DISARM_SWITCH); // Disarm to prevent motor output!!! - } + } } else { if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once #ifdef USE_BARO @@ -3857,6 +3871,29 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu gpsSolDRV.llh.lat = sbufReadU32(src); gpsSolDRV.llh.lon = sbufReadU32(src); gpsSolDRV.llh.alt = sbufReadU32(src); + + // CR97 hitl test needs fixing for DRV 22/02/24 + // if (true) { + // uint8_t counter = 1 + ((millis() / 5000) % 10); // cyclic number between 0 and 10 changing every 5s + // static uint8_t randNumOld = 1; + // static uint8_t randNum = 1; + // static int8_t signChanger = 1; + // if (counter == 1 && randNumOld == randNum) { + // randNum = 1 + (rand() % 6); // random number between 1 and 6 + // signChanger = 1; + // if (randNum == 2 || randNum == 3 || randNum == 6) { + // signChanger = -1; + // } + // } else if (counter == 2) { + // randNumOld = randNum; + // } + + // DEBUG_SET(DEBUG_ALWAYS, 5, gpsSol.llh.alt); + // gpsSol.llh.alt += (20 * signChanger * randNum * counter); // in cm + // DEBUG_SET(DEBUG_ALWAYS, 6, gpsSol.llh.alt); + // } + // CR97 + gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src); gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src); @@ -3883,7 +3920,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu } else { sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); } - + // Get the acceleration in 1G units acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f; acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f; @@ -3891,7 +3928,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu acc.accVibeSq[X] = 0.0f; acc.accVibeSq[Y] = 0.0f; acc.accVibeSq[Z] = 0.0f; - + // Get the angular velocity in DPS gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f; gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f; @@ -3922,7 +3959,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu simulatorData.airSpeed = sbufReadU16(src); } else { if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { - sbufReadU16(src); + sbufReadU16(src); } } @@ -3997,8 +4034,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = MSP_RESULT_ERROR; } break; -#endif - +#endif + default: // Not handled return false; diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index c61c2aa19e9..6f8b5a986bb 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -280,6 +280,7 @@ void initActiveBoxIds(void) if (sensors(SENSOR_BARO)) { ADD_ACTIVE_BOX(BOXAUTOLEVEL); } + if (sensors(SENSOR_ACC)) { ADD_ACTIVE_BOX(BOXANGLEHOLD); } diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 246d65c0a7e..aee7a5ff90d 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -322,13 +322,13 @@ void taskUpdateAux(timeUs_t currentTimeUs) { updatePIDCoefficients(); dynamicLpfGyroTask(); -#ifdef USE_SIMULATOR - if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { - updateFixedWingLevelTrim(currentTimeUs); - } -#else +// #ifdef USE_SIMULATOR + // if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { + // updateFixedWingLevelTrim(currentTimeUs); + // } +// #else updateFixedWingLevelTrim(currentTimeUs); -#endif +// #endif } void fcTasksInit(void) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 9cce60a2a38..a0401177af5 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -229,7 +229,7 @@ void processRcStickPositions(bool isThrottleLow) // Disarming via ARM BOX // Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver // and can't afford to risk disarming in the air - if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) { + if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && rxAreFlightChannelsValid() && !failsafeIsActive()) { const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs; if (disarmDelay > armingConfig()->switchDisarmDelayMs) { if (armingConfig()->disarm_kill_switch || isThrottleLow) { @@ -302,7 +302,6 @@ void processRcStickPositions(bool isThrottleLow) beeper(BEEPER_ACTION_FAIL); // The above cannot fail, but traditionally, we play FAIL for not-loading } #endif - // Multiple configuration profiles if (feature(FEATURE_TX_PROF_SEL)) { diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 741308755ca..5448c528dd5 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -91,11 +91,7 @@ static void processAirmodeMultirotor(void) { * activation threshold */ ENABLE_STATE(AIRMODE_ACTIVE); - } else if ( - STATE(AIRMODE_ACTIVE) && - !feature(FEATURE_AIRMODE) && - !IS_RC_MODE_ACTIVE(BOXAIRMODE) - ) { + } else if (STATE(AIRMODE_ACTIVE) && !feature(FEATURE_AIRMODE) && !IS_RC_MODE_ACTIVE(BOXAIRMODE)) { /* * When user disables BOXAIRMODE, turn airmode off as well */ diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index fa1827a6ad0..ae3c145f543 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -25,63 +25,121 @@ #define BOXID_NONE 255 +// typedef enum { + // BOXARM = 0, + // BOXANGLE = 1, + // BOXHORIZON = 2, + // BOXNAVALTHOLD = 3, // old BOXBARO + // BOXHEADINGHOLD = 4, // old MAG + // BOXHEADFREE = 5, + // BOXHEADADJ = 6, + // BOXCAMSTAB = 7, + // BOXNAVRTH = 8, // old GPSHOME + // BOXNAVPOSHOLD = 9, // old GPSHOLD + // BOXMANUAL = 10, + // BOXBEEPERON = 11, + // BOXLEDLOW = 12, + // BOXLIGHTS = 13, + // BOXNAVLAUNCH = 14, + // BOXOSD = 15, + // BOXTELEMETRY = 16, + // BOXBLACKBOX = 17, + // BOXFAILSAFE = 18, + // BOXNAVWP = 19, + // BOXAIRMODE = 20, + // BOXHOMERESET = 21, + // BOXGCSNAV = 22, + // BOXKILLSWITCH = 23, // old HEADING LOCK + // BOXSURFACE = 24, + // BOXFLAPERON = 25, + // BOXTURNASSIST = 26, + // BOXAUTOTRIM = 27, + // BOXAUTOTUNE = 28, + // BOXCAMERA1 = 29, + // BOXCAMERA2 = 30, + // BOXCAMERA3 = 31, + // BOXOSDALT1 = 32, + // BOXOSDALT2 = 33, + // BOXOSDALT3 = 34, + // BOXNAVCOURSEHOLD = 35, + // BOXBRAKING = 36, + // BOXUSER1 = 37, + // BOXUSER2 = 38, + // BOXFPVANGLEMIX = 39, + // BOXLOITERDIRCHN = 40, + // BOXMSPRCOVERRIDE = 41, + // BOXPREARM = 42, + // BOXTURTLE = 43, + // BOXNAVCRUISE = 44, + // BOXAUTOLEVEL = 45, + // BOXPLANWPMISSION = 46, + // BOXSOARING = 47, + // BOXUSER3 = 48, + // BOXUSER4 = 49, + // BOXCHANGEMISSION = 50, + // BOXBEEPERMUTE = 51, + // BOXMULTIFUNCTION = 52, + // BOXANGLEHOLD = 53, + // CHECKBOX_ITEM_COUNT +// } boxId_e; + typedef enum { BOXARM = 0, BOXANGLE = 1, BOXHORIZON = 2, - BOXNAVALTHOLD = 3, // old BOXBARO - BOXHEADINGHOLD = 4, // old MAG - BOXHEADFREE = 5, - BOXHEADADJ = 6, - BOXCAMSTAB = 7, - BOXNAVRTH = 8, // old GPSHOME - BOXNAVPOSHOLD = 9, // old GPSHOLD - BOXMANUAL = 10, - BOXBEEPERON = 11, - BOXLEDLOW = 12, - BOXLIGHTS = 13, - BOXNAVLAUNCH = 14, - BOXOSD = 15, - BOXTELEMETRY = 16, - BOXBLACKBOX = 17, - BOXFAILSAFE = 18, - BOXNAVWP = 19, - BOXAIRMODE = 20, - BOXHOMERESET = 21, - BOXGCSNAV = 22, - BOXKILLSWITCH = 23, // old HEADING LOCK - BOXSURFACE = 24, - BOXFLAPERON = 25, - BOXTURNASSIST = 26, - BOXAUTOTRIM = 27, - BOXAUTOTUNE = 28, - BOXCAMERA1 = 29, - BOXCAMERA2 = 30, - BOXCAMERA3 = 31, - BOXOSDALT1 = 32, - BOXOSDALT2 = 33, - BOXOSDALT3 = 34, - BOXNAVCOURSEHOLD = 35, - BOXBRAKING = 36, - BOXUSER1 = 37, - BOXUSER2 = 38, - BOXFPVANGLEMIX = 39, - BOXLOITERDIRCHN = 40, - BOXMSPRCOVERRIDE = 41, - BOXPREARM = 42, - BOXTURTLE = 43, - BOXNAVCRUISE = 44, - BOXAUTOLEVEL = 45, - BOXPLANWPMISSION = 46, - BOXSOARING = 47, - BOXUSER3 = 48, - BOXUSER4 = 49, - BOXCHANGEMISSION = 50, - BOXBEEPERMUTE = 51, - BOXMULTIFUNCTION = 52, - BOXMIXERPROFILE = 53, - BOXMIXERTRANSITION = 54, - BOXANGLEHOLD = 55, + BOXANGLEHOLD = 3, + BOXMANUAL = 4, + BOXNAVALTHOLD = 5, // old BOXBARO 3 + BOXHEADINGHOLD = 6, // old MAG 4 + BOXNAVPOSHOLD = 7, // old GPSHOLD 9 + BOXNAVCOURSEHOLD = 8, + BOXNAVCRUISE = 9, + BOXNAVRTH = 10, // old GPSHOME 8 + BOXNAVWP = 11, + BOXNAVLAUNCH = 12, + BOXSOARING = 13, + BOXFAILSAFE = 14, + BOXAUTOLEVEL = 15, + BOXHEADFREE = 16, + BOXHEADADJ = 17, + BOXCAMSTAB = 18, + BOXBEEPERON = 19, + BOXAUTOTRIM = 20, + BOXAUTOTUNE = 21, + BOXOSD = 22, + BOXTELEMETRY = 23, + BOXBLACKBOX = 24, + BOXAIRMODE = 25, + BOXHOMERESET = 26, + BOXGCSNAV = 27, + BOXKILLSWITCH = 28, // old HEADING LOCK 23 + BOXSURFACE = 29, + BOXFLAPERON = 30, + BOXTURNASSIST = 31, + BOXLEDLOW = 32, + BOXLIGHTS = 33, + BOXCAMERA1 = 34, + BOXCAMERA2 = 35, + BOXCAMERA3 = 36, + BOXOSDALT1 = 37, + BOXOSDALT2 = 38, + BOXOSDALT3 = 39, + BOXBRAKING = 40, + BOXUSER1 = 41, + BOXUSER2 = 42, + BOXFPVANGLEMIX = 43, + BOXLOITERDIRCHN = 44, + BOXMSPRCOVERRIDE = 45, + BOXPREARM = 46, + BOXTURTLE = 47, + BOXPLANWPMISSION = 48, + BOXUSER3 = 49, + BOXUSER4 = 50, + BOXCHANGEMISSION = 51, + BOXBEEPERMUTE = 52, + BOXMULTIFUNCTION = 53, + BOXMIXERPROFILE = 54, + BOXMIXERTRANSITION = 55, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 5b6c6241397..3c60f6b3e24 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -105,6 +105,7 @@ typedef enum { TURTLE_MODE = (1 << 15), SOARING_MODE = (1 << 16), ANGLEHOLD_MODE = (1 << 17), + NAV_FW_AUTOLAND = (1 << 18) } flightModeFlags_e; extern uint32_t flightModeFlags; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0afd4c8baef..ea0eae533ac 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -345,7 +345,7 @@ groups: max: INT16_MAX - name: ins_gravity_cmss description: "Calculated 1G of Acc axis Z to use in INS" - default_value: 0.0 + default_value: 0 field: gravity_cmss_cal min: 0 max: 2000 @@ -758,7 +758,7 @@ groups: min: 0 max: PWM_RANGE_MAX - name: motor_pwm_rate - description: "Output frequency (in Hz) for motor pins. Applies only to brushed motors. " + description: "Output frequency (in Hz) for motor pins. Applies only to brushed motors." default_value: 16000 field: motorPwmRate min: 50 @@ -2074,18 +2074,32 @@ groups: min: 0 max: 100 default_value: 90 + # CR47 - name: nav_mc_vel_xy_dterm_attenuation_start - description: "A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins" - default_value: 10 + description: "Horizontal velocity where nav_mc_vel_xy_dterm_attenuation begins [m/s]" + default_value: 5 field: navVelXyDtermAttenuationStart min: 0 max: 100 - name: nav_mc_vel_xy_dterm_attenuation_end - description: "A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum" - default_value: 60 + description: "Horizontal velocity where nav_mc_vel_xy_dterm_attenuation reaches maximum [m/s]" + default_value: 10 field: navVelXyDtermAttenuationEnd min: 0 max: 100 + # - name: nav_mc_vel_xy_dterm_attenuation_start + # description: "A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins" + # default_value: 10 + # field: navVelXyDtermAttenuationStart + # min: 0 + # max: 100 + # - name: nav_mc_vel_xy_dterm_attenuation_end + # description: "A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum" + # default_value: 60 + # field: navVelXyDtermAttenuationEnd + # min: 0 + # max: 100 + # CR47 - name: nav_fw_pos_z_p description: "P gain of altitude PID controller (Fixedwing)" default_value: 40 @@ -2104,6 +2118,20 @@ groups: field: bank_fw.pid[PID_POS_Z].D min: 0 max: 255 +# CR97 + - name: nav_fw_alt_control_response + description: "Adjusts the deceleration response of fixed wing altitude control as the target altitude is approached. Decrease value to help avoid overshooting the target altitude." + default_value: 20 + field: fwAltControlResponseFactor + min: 5 + max: 100 + # - name: nav_fw_pos_z_ff + # description: "Feed forward gain of altitude PID controller (Fixedwing)" + # default_value: 10 + # field: bank_fw.pid[PID_POS_Z].FF + # min: 0 + # max: 255 +# CR97 - name: nav_fw_pos_xy_p description: "P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH" default_value: 75 @@ -2328,7 +2356,7 @@ groups: field: w_z_surface_p min: 0 max: 100 - default_value: 3.5 + default_value: 3.5 - name: inav_w_z_surface_v field: w_z_surface_v min: 0 @@ -2795,6 +2823,14 @@ groups: field: fw.max_bank_angle min: 5 max: 80 + # CR97A + - name: nav_fw_auto_climb_rate + description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]" + default_value: 500 + field: fw.max_auto_climb_rate + min: 10 + max: 2000 + # CR97A - name: nav_fw_manual_climb_rate description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]" default_value: 300 @@ -2931,6 +2967,13 @@ groups: field: fw.launch_climb_angle min: 5 max: 45 + # CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxx + - name: nav_fw_launch_allow_throttle_low + description: "Allow launch sequence with throttle maintained low throughout. When main launch sequence completes control is maintained with Nav cruise throttle until sticks moved/throttle raised or control switches to other Nav mode if preselected" + default_value: OFF + field: fw.launch_allow_throttle_low + type: bool + # CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxx - name: nav_fw_launch_manual_throttle description: "Allows launch with manually controlled throttle. INAV only levels wings and controls climb pitch during launch. Throttle is controlled directly by throttle stick movement. IF USED WITHOUT A GPS LOCK plane must be launched immediately after throttle is increased to avoid issues with climb out stabilisation and the launch ending sooner than expected (launch end timer starts as soon as the throttle stick is raised)." default_value: OFF @@ -3502,6 +3545,7 @@ groups: default_value: 11 min: 10 max: 13 + - name: osd_plus_code_short description: "Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates." field: plus_code_short @@ -3571,7 +3615,6 @@ groups: max: 5 default_value: 3 description: Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD) - - name: osd_ahi_pitch_interval field: ahi_pitch_interval min: 0 @@ -4137,7 +4180,7 @@ groups: type: navFwAutolandConfig_t headers: ["navigation/navigation.h"] condition: USE_FW_AUTOLAND - members: + members: - name: nav_fw_land_approach_length description: "Length of the final approach" default_value: 35000 diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index d3f9d90d7db..0910bfba1e0 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -46,6 +46,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" #include "fc/settings.h" +#include "fc/rc_controls.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -57,6 +58,8 @@ #include "io/gps.h" +#include "navigation/navigation_private.h" // CR27 + #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/pitotmeter.h" @@ -113,6 +116,7 @@ STATIC_FASTRAM pt1Filter_t GPS3DspeedFilter; FASTRAM bool gpsHeadingInitialized; +FASTRAM uint16_t compassGpsCogError; // heading difference between compass and GPS (degrees) // CR27 FASTRAM bool imuUpdated = false; static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF); @@ -674,7 +678,7 @@ static void imuCalculateGPSacceleration(fpVector3_t *vEstcentrifugalAccelBF, flo } static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF, float dT, float *acc_ignore_slope_multipiler) -{ +{ //fixed wing only static float lastspeed = -1.0f; float currentspeed = 0; @@ -703,7 +707,42 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF vEstcentrifugalAccelBF->z = currentspeed*imuMeasuredRotationBFFiltered.y; lastspeed = currentspeed; } +// CR27 +#if defined(USE_MAG) && defined(USE_GPS) +bool compassHeadingGPSCogErrorCheck(void) +{ + static timeMs_t timerStartMs = 0; + + compassGpsCogError = 270; + if (!isGPSHeadingValid()) { + timerStartMs = 0; + return false; + } + + compassGpsCogError = 260; + bool rcCommandCondition = ABS(rcCommand[PITCH]) > 25 || ABS(rcCommand[ROLL]) > 25 || navigationIsFlyingAutonomousMode(); + + if (sensors(SENSOR_MAG) && compassIsHealthy() && rcCommandCondition) { + static uint16_t compassGpsCogErrorPrev = 10; + int16_t commandCorrection = RADIANS_TO_DECIDEGREES(atan2_approx(rcCommand[ROLL], rcCommand[PITCH])); + compassGpsCogError = ABS(gpsSol.groundCourse - (wrap_36000(10 * (attitude.values.yaw + commandCorrection))) / 10); + // compassGpsCogError = ABS(900 - (wrap_36000(10 * (attitude.values.yaw + commandCorrection))) / 10); + compassGpsCogError = compassGpsCogError > 1800 ? ABS(compassGpsCogError - 3600) : compassGpsCogError; + compassGpsCogError = 0.8 * compassGpsCogErrorPrev + 0.2 * compassGpsCogError; + compassGpsCogErrorPrev = compassGpsCogError; + compassGpsCogError = compassGpsCogError / 10; + + if (compassGpsCogError > 90) { // 90 for test, change for better value + timerStartMs = timerStartMs == 0 ? millis(): timerStartMs; + return millis() - timerStartMs > 10000; // 10s for test, use shorter time + } + } + timerStartMs = 0; + return false; +} +#endif +// CR27 fpQuaternion_t* getTailSitterQuaternion(bool normal2tail){ static bool firstRun = true; static fpQuaternion_t qNormal2Tail; @@ -871,7 +910,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) acc.accADCf[Z] = 0.0f; } } - + bool isImuReady(void) { @@ -880,6 +919,7 @@ bool isImuReady(void) bool isImuHeadingValid(void) { + // CR27 add block if compass heading mismatch with GPS heading in compassHeadingGPSCheck() return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || gpsHeadingInitialized; } diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 8afcc50e579..5f2501e6b11 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -44,6 +44,8 @@ extern fpQuaternion_t orientation; extern attitudeEulerAngles_t attitude; extern float rMat[3][3]; +extern uint16_t compassGpsCogError; // heading difference between compass and GPS (degrees) // CR27 + typedef struct imuConfig_s { uint16_t dcm_kp_acc; // DCM filter proportional gain ( x 10000) for accelerometer uint16_t dcm_ki_acc; // DCM filter integral gain ( x 10000) for accelerometer @@ -82,6 +84,7 @@ void imuUpdateAccelerometer(void); float calculateCosTiltAngle(void); bool isImuReady(void); bool isImuHeadingValid(void); +bool compassHeadingGPSCogErrorCheck(void); // CR27 void imuTransformVectorBodyToEarth(fpVector3_t * v); void imuTransformVectorEarthToBody(fpVector3_t * v); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 834841e6580..7fba415100b 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -291,6 +291,7 @@ static uint16_t handleOutputScaling( } return value; } + static void applyTurtleModeToMotors(void) { if (ARMING_FLAG(ARMED)) { @@ -634,7 +635,7 @@ void FAST_CODE mixTable(void) } else { motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); } - + //stop motors if (currentMixer[i].throttle <= 0.0f) { motor[i] = motorZeroCommand; @@ -684,10 +685,12 @@ motorStatus_e getMotorStatus(void) // and either on a plane or on a quad with inactive airmode - stop motor return MOTOR_STOPPED_USER; - } else if (!failsafeIsActive()) { +// CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxx + } else if (!failsafeIsActive() && !launchAllowedWithThrottleLow()) { +// CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxx // 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 - + // Launch with throttle low overrides MOTOR STOP always switch (navConfig()->general.flags.nav_overrides_motor_stop) { case NOMS_ALL_NAV: return navigationInAutomaticThrottleMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d913802d559..387583fee03 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -170,7 +170,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false; static EXTENDED_FASTRAM float fixedWingLevelTrim; static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 6); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 6); // CR97 PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -231,9 +231,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, }, [PID_HEADING] = { SETTING_NAV_FW_HEADING_P_DEFAULT, 0, 0, 0 }, [PID_POS_Z] = { - .P = SETTING_NAV_FW_POS_Z_P_DEFAULT, // FW_POS_Z_P * 10 - .I = SETTING_NAV_FW_POS_Z_I_DEFAULT, // FW_POS_Z_I * 10 - .D = SETTING_NAV_FW_POS_Z_D_DEFAULT, // FW_POS_Z_D * 10 + .P = SETTING_NAV_FW_POS_Z_P_DEFAULT, // FW_POS_Z_P * 100 // CR97 all changed from 10 to 100 + .I = SETTING_NAV_FW_POS_Z_I_DEFAULT, // FW_POS_Z_I * 100 + .D = SETTING_NAV_FW_POS_Z_D_DEFAULT, // FW_POS_Z_D * 100 .FF = 0, }, [PID_POS_XY] = { @@ -301,6 +301,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT, .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT, + .fwAltControlResponseFactor = SETTING_NAV_FW_ALT_CONTROL_RESPONSE_DEFAULT, // CR97 + #ifdef USE_SMITH_PREDICTOR .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT, .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT, @@ -594,7 +596,7 @@ static float computePidLevelTarget(flight_dynamics_index_t axis) { // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle #ifdef USE_FW_AUTOLAND - if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !isFwLandInProgess()) { + if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && ! FLIGHT_MODE(NAV_FW_AUTOLAND)) { #else if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) { #endif @@ -1057,21 +1059,22 @@ void checkItermLimitingActive(pidState_t *pidState) } void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) -{ +{// CR ? + pidState->itermFreezeActive = false; if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) { // Do not allow yaw I-term to grow when bank angle is too large - float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); - if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ + uint8_t bankAngle = ABS(DECIDEGREES_TO_DEGREES(attitude.values.roll)); + if (bankAngle > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ pidState->itermFreezeActive = true; - } else - { - pidState->itermFreezeActive = false; - } - } else - { - pidState->itermFreezeActive = false; - } - + } // else + // { + // pidState->itermFreezeActive = false; + // } + // } else + // { + // pidState->itermFreezeActive = false; + } + // CR ? } bool isAngleHoldLevel(void) @@ -1095,6 +1098,11 @@ void updateAngleHold(float *angleTarget, uint8_t axis) static int16_t angleHoldTarget[2]; + DEBUG_SET(DEBUG_ALWAYS, 0, angleHoldTarget[FD_ROLL]); + DEBUG_SET(DEBUG_ALWAYS, 2, attitude.raw[FD_ROLL]); + DEBUG_SET(DEBUG_ALWAYS, 1, angleHoldTarget[FD_PITCH]); + DEBUG_SET(DEBUG_ALWAYS, 3, attitude.raw[FD_PITCH]); + if (restartAngleHoldMode) { // set target attitude to current attitude on activation angleHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL]; angleHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH] + DEGREES_TO_DECIDEGREES(fixedWingLevelTrim); @@ -1378,15 +1386,14 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs) const float dT = US2S(currentTimeUs - previousUpdateTimeUs); previousUpdateTimeUs = currentTimeUs; - /* - * Prepare flags for the PID controller - */ + // Prepare flags for the PID controller pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR; // Iterm should freeze when conditions for setting level trim aren't met or time since last expected update too long ago if (!isFixedWingLevelTrimActive() || (dT > 5.0f * US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ)))) { flags |= PID_FREEZE_INTEGRATOR; } + DEBUG_SET(DEBUG_AUTOLEVEL, 3, flags); const float output = navPidApply3( &fixedWingLevelTrimController, @@ -1399,7 +1406,6 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs) 1.0f, 1.0f ); - DEBUG_SET(DEBUG_AUTOLEVEL, 4, output); fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim + (output * FIXED_WING_LEVEL_TRIM_MULTIPLIER); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e0a8b9d310b..8abf25c1837 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -151,6 +151,8 @@ typedef struct pidProfile_s { float fixedWingLevelTrim; float fixedWingLevelTrimGain; + + uint8_t fwAltControlResponseFactor; // CR97 #ifdef USE_SMITH_PREDICTOR float smithPredictorStrength; float smithPredictorDelay; diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index 708c71bd1cf..86ceeec921f 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -145,7 +145,6 @@ static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float spee // returns Wh static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { - const float RTH_initial_altitude_change = MAX(0, (getFinalRTHAltitude() - getEstimatedActualPosition(Z)) / 100); float RTH_heading; // degrees @@ -219,7 +218,6 @@ float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) { return -1; } #endif - // check requirements const bool areBatterySettingsOK = feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && batteryWasFullWhenPluggedIn(); const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH &¤tBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 8375774e044..49314c4b77f 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -956,18 +956,13 @@ STATIC_PROTOTHREAD(gpsConfigure) // Configure GNSS for M8N and later if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - bool use_VALSET = 0; - if ( (gpsState.swVersionMajor > 23) || (gpsState.swVersionMajor == 23 && gpsState.swVersionMinor > 1) ) { - use_VALSET = 1; - } - - if ( use_VALSET && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10) ) { + if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor >= 23 && gpsState.swVersionMinor >= 1)) { configureGNSS10(); } else { configureGNSS(); } - ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); if(_ack_state == UBX_ACK_GOT_NAK) { gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e792d87b6d0..a052d3e26ef 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -191,6 +191,7 @@ static bool fullRedraw = false; static uint8_t armState; +// Multifunction display static textAttributes_t osdGetMultiFunctionMessage(char *buff); static uint8_t osdWarningsFlags = 0; @@ -986,7 +987,7 @@ static const char * osdFailsafePhaseMessage(void) static const char * osdFailsafeInfoMessage(void) { - if (failsafeIsReceivingRxData()) { + if (failsafeIsReceivingRxData() && !FLIGHT_MODE(NAV_FW_AUTOLAND)) { // User must move sticks to exit FS mode return OSD_MESSAGE_STR(OSD_MSG_MOVE_EXIT_FS); } @@ -2362,7 +2363,7 @@ static bool osdDrawSingleElement(uint8_t item) { char *p = "ACRO"; #ifdef USE_FW_AUTOLAND - if (isFwLandInProgess()) + if (FLIGHT_MODE(NAV_FW_AUTOLAND)) p = "LAND"; else #endif @@ -2383,7 +2384,8 @@ static bool osdDrawSingleElement(uint8_t item) else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) p = "CRSH"; else if (FLIGHT_MODE(NAV_WP_MODE)) - p = " WP "; + p = FLIGHT_MODE(SOARING_MODE) ? "WP-S" : " WP "; // CR36 not used in release + // p = " WP "; else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) { // If navigationRequiresAngleMode() returns false when ALTHOLD is active, // it means it can be combined with ANGLE, HORIZON, ANGLEHOLD, ACRO, etc... @@ -2425,7 +2427,6 @@ static bool osdDrawSingleElement(uint8_t item) { vtxDeviceOsdInfo_t osdInfo; vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo); - tfp_sprintf(buff, "CH:%c%s:", osdInfo.bandLetter, osdInfo.channelName); displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); @@ -2603,6 +2604,7 @@ static bool osdDrawSingleElement(uint8_t item) wp2.lat = posControl.waypointList[j].lat; wp2.lon = posControl.waypointList[j].lon; wp2.alt = posControl.waypointList[j].alt; + fpVector3_t poi; geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, waypointMissionAltConvMode(posControl.waypointList[j].p3)); int32_t altConvModeAltitude = waypointMissionAltConvMode(posControl.waypointList[j].p3) == GEO_ALT_ABSOLUTE ? osdGetAltitudeMsl() : osdGetAltitude(); @@ -2625,13 +2627,23 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_ATTITUDE_PITCH: - if (ABS(attitude.values.pitch) < 1) + ;// CR42 + int16_t levelDatumPitch = attitude.values.pitch + DEGREES_TO_DECIDEGREES(getFixedWingLevelTrim()); + // if (ABS(attitude.values.pitch) < 1) + // buff[0] = 'P'; + // else if (attitude.values.pitch > 0) + // buff[0] = SYM_PITCH_DOWN; + // else if (attitude.values.pitch < 0) + // buff[0] = SYM_PITCH_UP; + // osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.pitch)), 0, 1, 0, 3, false); + if (ABS(levelDatumPitch) < 1) buff[0] = 'P'; - else if (attitude.values.pitch > 0) + else if (levelDatumPitch > 0) buff[0] = SYM_PITCH_DOWN; - else if (attitude.values.pitch < 0) + else if (levelDatumPitch < 0) buff[0] = SYM_PITCH_UP; - osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.pitch)), 0, 1, 0, 3, false); + osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(levelDatumPitch)), 0, 1, 0, 3, false); + // CR42 break; case OSD_ARTIFICIAL_HORIZON: @@ -3326,15 +3338,16 @@ static bool osdDrawSingleElement(uint8_t item) * Only 7 digits for negative and 8 digits for positive values allowed */ for (uint8_t bufferIndex = 0; bufferIndex < DEBUG32_VALUE_COUNT; ++elemPosY, bufferIndex += 2) { + char buffx[32] = {0}; // CR48 tfp_sprintf( - buff, + buffx, // CR48 "[%u]=%8ld [%u]=%8ld", bufferIndex, (long)constrain(debug[bufferIndex], -9999999, 99999999), bufferIndex+1, (long)constrain(debug[bufferIndex+1], -9999999, 99999999) ); - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buffx); // CR48 } break; } @@ -4440,7 +4453,21 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) if (isSinglePageStatsCompatible) { displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---"); } else if (page == 0) { - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); + //CR4 xxxxxxxxxxxxxxxxxxxxxxx + dateTime_t dt; + char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; + char *date; + char *time; + if (rtcGetDateTime(&dt)) { + dateTimeFormatLocal(buf, &dt); + dateTimeSplitFormatted(buf, &date, &time); + tfp_sprintf(buff, "-STATS-(%s) 1/2 ->", date); + displayWrite(osdDisplayPort, statNameX, top++, buff); + } else { + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); + } + //CR4 xxxxxxxxxxxxxxxxxxxxxxx + // displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); } else if (page == 1) { displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- <- 2/2"); } @@ -4688,13 +4715,25 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) // HD arming screen. based on the minimum HD OSD grid size of 50 x 18 static void osdShowHDArmScreen(void) { +// <<<<<<< HEAD + // dateTime_t dt; + // char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; + // char craftNameBuf[MAX_NAME_LENGTH]; + // char versionBuf[30]; + // char *date; + // char *time; + // // We need 12 visible rows + // uint8_t y = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1; // rows = 13 NTSC, 16 PAL MAX7456 +// ======= dateTime_t dt; + char buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; char craftNameBuf[MAX_NAME_LENGTH]; char versionBuf[osdDisplayPort->cols]; uint8_t safehomeRow = 0; uint8_t armScreenRow = 2; // Start at row 2 +// >>>>>>> upstream/master armScreenRow = drawLogos(false, armScreenRow); armScreenRow++; @@ -5227,7 +5266,8 @@ displayCanvas_t *osdGetDisplayPortCanvas(void) return NULL; } -timeMs_t systemMessageCycleTime(unsigned messageCount, const char **messages){ +timeMs_t systemMessageCycleTime(unsigned messageCount, const char **messages) +{ uint8_t i = 0; float factor = 1.0f; while (i < messageCount) { @@ -5254,7 +5294,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (ARMING_FLAG(ARMED)) { #ifdef USE_FW_AUTOLAND - if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || isFwLandInProgess()) { + if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || FLIGHT_MODE(NAV_FW_AUTOLAND)) { if (isWaypointMissionRTHActive() && !posControl.fwLandState.landWp) { #else if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { @@ -5295,7 +5335,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter #ifdef USE_FW_AUTOLAND if (canFwLandCanceld()) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS); - } else if (!isFwLandInProgess()) { + } else if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) { #endif const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { @@ -5373,6 +5413,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (STATE(LANDING_DETECTED)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED); } + if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) { int8_t navAngleHoldAxis = navCheckActiveAngleHoldAxis(); if (isAngleHoldLevel()) { @@ -5383,40 +5424,44 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); } } + // CR44 + // if (posControl.navState == NAV_STATE_IDLE && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP))) { + // messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_NAV_SENSOR_LOSS); + // } + // CR44 } } + // CR27 + // if (posControl.flags.compassGpsCogMismatchError) { + // messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_COMPASS_ERROR); + // } + // CR27 } 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)) { + const setting_t *setting = settingGet(invalidIndex); + settingGetName(setting, messageBuf); + for (int ii = 0; messageBuf[ii]; ii++) { + messageBuf[ii] = sl_toupper(messageBuf[ii]); + } + invertedInfoMessage = messageBuf; + messages[messageCount++] = invertedInfoMessage; - const setting_t *setting = settingGet(invalidIndex); - settingGetName(setting, messageBuf); - for (int ii = 0; messageBuf[ii]; ii++) { - messageBuf[ii] = sl_toupper(messageBuf[ii]); - } - invertedInfoMessage = messageBuf; - messages[messageCount++] = invertedInfoMessage; - - invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING); - messages[messageCount++] = invertedInfoMessage; - + invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING); + messages[messageCount++] = invertedInfoMessage; } else { - - invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM); - messages[messageCount++] = invertedInfoMessage; - - // Show the reason for not arming - messages[messageCount++] = osdArmingDisabledReasonMessage(); - + invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM); + messages[messageCount++] = invertedInfoMessage; + // Show the reason for not arming + messages[messageCount++] = osdArmingDisabledReasonMessage(); } } else if (!ARMING_FLAG(ARMED)) { if (isWaypointListValid()) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_MISSION_LOADED); } } - /* Messages that are shown regardless of Arming state */ if (savingSettings == true) { @@ -5439,7 +5484,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } else if (message == invertedInfoMessage) { TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); } - // We're shoing either failsafePhaseMessage or + // We're showing either failsafePhaseMessage or // navStateMessage. Don't BLINK here since // having this text available might be crucial // during a lost aircraft recovery and blinking @@ -5570,19 +5615,27 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) #endif // MULTIFUNCTION - functions only, warnings always defined /* --- WARNINGS --- */ - const char *messages[7]; + const char *messages[8]; uint8_t messageCount = 0; bool warningCondition = false; warningsCount = 0; uint8_t warningFlagID = 1; - // Low Battery - const batteryState_e batteryState = getBatteryState(); - warningCondition = batteryState == BATTERY_CRITICAL || batteryState == BATTERY_WARNING; + // Low Battery Voltage + const batteryState_e batteryVoltageState = checkBatteryVoltageState(); + warningCondition = batteryVoltageState == BATTERY_CRITICAL || batteryVoltageState == BATTERY_WARNING; if (osdCheckWarning(warningCondition, warningFlagID, &warningsCount)) { - messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT LOW !"; + messages[messageCount++] = batteryVoltageState == BATTERY_CRITICAL ? "VBATT CRIT" : "VBATT LOW "; } + // Low Battery Capacity + if (batteryUsesCapacityThresholds()) { + const batteryState_e batteryState = getBatteryState(); + warningCondition = batteryState == BATTERY_CRITICAL || batteryVoltageState == BATTERY_WARNING; + if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) { + messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT LOW "; + } + } #if defined(USE_GPS) // GPS Fix and Failure if (feature(FEATURE_GPS)) { @@ -5604,7 +5657,6 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) messages[messageCount++] = "ALT SANITY"; } #endif - #if defined(USE_MAG) // Magnetometer failure if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { @@ -5615,11 +5667,12 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) } #endif // Vibration levels TODO - needs better vibration measurement to be useful - // const float vibrationLevel = accGetVibrationLevel(); - // warningCondition = vibrationLevel > 1.5f; - // if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) { - // messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!"; - // } + const float vibrationLevel = accGetVibrationLevel(); + // DEBUG_SET(DEBUG_ALWAYS, 0, vibrationLevel * 100); + warningCondition = vibrationLevel > 1.5f; + if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) { + messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!"; + } #ifdef USE_DEV_TOOLS if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1, &warningsCount)) { @@ -5627,6 +5680,12 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) } #endif +// #if defined(USE_BARO) + // if (osdCheckWarning(getHwBarometerStatus() == HW_SENSOR_UNAVAILABLE, warningFlagID <<= 1, &warningsCount)) { + // messages[messageCount++] = "BARO FAIL"; + // } +// #endif + if (messageCount) { message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; // display each warning on 1s cycle strcpy(buff, message); @@ -5635,6 +5694,19 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) buff[0] = SYM_ALERT; tfp_sprintf(buff + 1, "%u ", warningsCount); } + return elemAttr; + + // CR27 + if (STATE(MULTIROTOR)) { + if (compassGpsCogError == 270) { + strcpy(buff, "NO GPS COG"); + } else if (compassGpsCogError == 260) { + strcpy(buff, "MOVE DRONE"); + } else { + tfp_sprintf(buff, "MAG ERR%3u", compassGpsCogError); + } + } + // CR27 return elemAttr; } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 503f7dd0e09..2ae7e793487 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -105,8 +105,9 @@ #define OSD_MSG_LANDING "LANDING" #define OSD_MSG_LOITERING_HOME "LOITERING AROUND HOME" #define OSD_MSG_HOVERING "HOVERING" -#define OSD_MSG_LANDED "LANDED" +#define OSD_MSG_LANDED "! LANDED !" #define OSD_MSG_PREPARING_LAND "PREPARING TO LAND" +// #define OSD_MSG_NAV_SENSOR_LOSS "NAV FAIL -> SENSOR LOSS" // CR44 #define OSD_MSG_AUTOLAUNCH "AUTOLAUNCH" #define OSD_MSG_AUTOLAUNCH_MANUAL "AUTOLAUNCH (MANUAL)" #define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)" @@ -117,6 +118,7 @@ #define OSD_MSG_HEADFREE "(HEADFREE)" #define OSD_MSG_NAV_SOARING "(SOARING)" #define OSD_MSG_UNABLE_ARM "UNABLE TO ARM" +#define OSD_MSG_COMPASS_ERROR "COMPASS ERROR !" // CR27 #define OSD_MSG_SAVING_SETTNGS "** SAVING SETTINGS **" #define OSD_MSG_SETTINGS_SAVED "** SETTINGS SAVED **" #define OSD_MSG_ANGLEHOLD_ROLL "(ANGLEHOLD ROLL)" @@ -257,7 +259,7 @@ typedef enum { OSD_RANGEFINDER, OSD_PLIMIT_REMAINING_BURST_TIME, OSD_PLIMIT_ACTIVE_CURRENT_LIMIT, - OSD_PLIMIT_ACTIVE_POWER_LIMIT, + OSD_PLIMIT_ACTIVE_POWER_LIMIT, // limit of CMS entries OSD_GLIDESLOPE, OSD_GPS_MAX_SPEED, OSD_3D_MAX_SPEED, @@ -274,7 +276,7 @@ typedef enum { OSD_GLIDE_RANGE, OSD_CLIMB_EFFICIENCY, OSD_NAV_WP_MULTI_MISSION_INDEX, - OSD_GROUND_COURSE, // 140 + OSD_GROUND_COURSE, // 140 OSD_CROSS_TRACK_ERROR, OSD_PILOT_NAME, OSD_PAN_SERVO_CENTRED, diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c index 47dd4ee47bc..0f24515f390 100644 --- a/src/main/io/osd_canvas.c +++ b/src/main/io/osd_canvas.c @@ -682,7 +682,6 @@ static bool osdCanvasDrawSidebar(uint32_t *configured, displayWidgets_t *widgets osdCrosshairPosition(&ex, &ey); const int height = 2 * OSD_AH_SIDEBAR_HEIGHT_POS * OSD_CHAR_HEIGHT; const int y = (ey - OSD_AH_SIDEBAR_HEIGHT_POS) * OSD_CHAR_HEIGHT; - widgetSidebarConfiguration_t config = { .rect.y = y, .rect.w = width, diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 38db724c8e1..f09ff2d9a17 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -605,7 +605,7 @@ static char * navigationStateMessage(void) switch (NAV_Status.state) { case MW_NAV_STATE_NONE: break; - case MW_NAV_STATE_RTH_START: + case MW_NAV_STATE_RTH_START: return OSD_MESSAGE_STR("STARTING RTH"); case MW_NAV_STATE_RTH_CLIMB: return OSD_MESSAGE_STR("ADJUSTING RTH ALTITUDE"); @@ -1063,6 +1063,11 @@ static bool djiFormatMessages(char *buff) if (FLIGHT_MODE(MANUAL_MODE)) { messages[messageCount++] = "(MANUAL)"; } + + if (FLIGHT_MODE(NAV_FW_AUTOLAND)) { + messages[messageCount++] = "(LAND)"; + } + } } // Pick one of the available messages. Each message lasts diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index f9ddc1f2554..b676918132e 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -19,7 +19,10 @@ #include #include #include - +// CR97 hitl test +// #include +// #include +// CR97 #include "platform.h" #include "build/debug.h" @@ -118,7 +121,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -180,8 +183,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, // MC-specific .mc = { - .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees - .max_auto_climb_rate = SETTING_NAV_MC_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s + .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees + .max_auto_climb_rate = SETTING_NAV_MC_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s .max_manual_climb_rate = SETTING_NAV_MC_MANUAL_CLIMB_RATE_DEFAULT, #ifdef USE_MR_BRAKING_MODE @@ -192,7 +195,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .braking_boost_timeout = SETTING_NAV_MC_BRAKING_BOOST_TIMEOUT_DEFAULT, // Timout boost after 750ms .braking_boost_speed_threshold = SETTING_NAV_MC_BRAKING_BOOST_SPEED_THRESHOLD_DEFAULT, // Boost can happen only above 1.5m/s .braking_boost_disengage_speed = SETTING_NAV_MC_BRAKING_BOOST_DISENGAGE_SPEED_DEFAULT, // Disable boost at 1m/s - .braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT, // Max braking angle + .braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT, // Max braking angle #endif .posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT, // posDecelerationTime * 100 @@ -204,6 +207,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, // Fixed wing .fw = { .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees + .max_auto_climb_rate = SETTING_NAV_FW_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s CR97A .max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT, // 3 m/s .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees @@ -231,6 +235,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended .launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees .launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg + .launch_allow_throttle_low = SETTING_NAV_FW_LAUNCH_ALLOW_THROTTLE_LOW_DEFAULT, // allow launch with throttle low CR6 .launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF .launch_land_abort_deadband = SETTING_NAV_FW_LAUNCH_LAND_ABORT_DEADBAND_DEFAULT, // 100 us @@ -248,7 +253,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, static navWapointHeading_t wpHeadingControl; navigationPosControl_t posControl; navSystemStatus_t NAV_Status; -static bool landingDetectorIsActive; +static bool landingDetectorIsActive = false; EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients; @@ -296,13 +301,11 @@ void missionPlannerSetWaypoint(void); void initializeRTHSanityChecker(void); bool validateRTHSanityChecker(void); + +static navigationFSMEvent_t selectNavEventFromBoxModeInput(bool launchBypass); //CR6 void updateHomePosition(void); bool abortLaunchAllowed(void); -// static bool rthAltControlStickOverrideCheck(unsigned axis); -// static void updateRthTrackback(bool forceSaveTrackPoint); -// static fpVector3_t * rthGetTrackbackPos(void); - #ifdef USE_FW_AUTOLAND static float getLandAltitude(void); static int32_t calcWindDiff(int32_t heading, int32_t windHeading); @@ -806,18 +809,18 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -991,10 +994,18 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + // CR6 xxxxxxxxxxxxxxxxxx + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + // CR6 xxxxxxxxxxxxxxxxxx } }, @@ -1044,13 +1055,14 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, +/** Advanced Fixed Wing Autoland **/ #ifdef USE_FW_AUTOLAND [NAV_STATE_FW_LANDING_CLIMB_TO_LOITER] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, - .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1070,8 +1082,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_LOITER, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_LOITER, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, - .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1091,8 +1103,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_APPROACH, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, - .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_WP, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1113,7 +1125,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE, .timeoutMs = 10, .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW, - .mapToFlightModes = NAV_COURSE_HOLD_MODE, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1134,7 +1146,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_FLARE, .timeoutMs = 10, .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW, - .mapToFlightModes = NAV_COURSE_HOLD_MODE, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1147,8 +1159,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_ABORT, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_ABORT, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, - .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, + .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -1211,7 +1223,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navi setupAltitudeController(); setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset } - return NAV_FSM_EVENT_SUCCESS; } @@ -1282,7 +1293,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( } DEBUG_SET(DEBUG_CRUISE, 0, 1); - // Switch to IDLE if we do not have an healty position. Try the next iteration. + // Switch to IDLE if we do not have an healthy position. Try the next iteration. if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } @@ -1336,7 +1347,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS float rateTarget = scaleRangef((float)headingAdjustCommand, -500.0f, 500.0f, -cruiseYawRate, cruiseYawRate); float centidegsPerIteration = rateTarget * MS2S(timeDifference); posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration); - DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course)); posControl.cruise.lastCourseAdjustmentTime = currentTimeMs; } else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) { posControl.cruise.previousCourse = posControl.cruise.course; @@ -1350,7 +1360,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(navigationFSMState_t previousState) { UNUSED(previousState); - DEBUG_SET(DEBUG_CRUISE, 0, 3); // User is rolling, changing manually direction. Wait until it is done and then restore CRUISE if (posControl.flags.isAdjustingPosition) { @@ -1412,7 +1421,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati } } - // If we have valid position sensor or configured to ignore it's loss at initial stage - continue + // If we have valid position sensor or configured to ignore its loss at initial stage - continue if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) { // Prepare controllers resetPositionController(); @@ -1606,7 +1615,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState) { UNUSED(previousState); - //On ROVER and BOAT we immediately switch to the next event if (!STATE(ALTITUDE_CONTROL)) { return NAV_FSM_EVENT_SUCCESS; @@ -1636,7 +1644,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing if (!pauseLanding && ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY))) { resetLandingDetector(); // force reset landing detector just in case - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); // CR97 return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land } else { fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); @@ -1677,7 +1685,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF /* If position sensors unavailable - land immediately (wait for timeout on GPS) * Continue to check for RTH sanity during landing */ - if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) { + if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || (previousState != NAV_STATE_WAYPOINT_REACHED && !validateRTHSanityChecker())) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1687,7 +1695,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF #ifdef USE_FW_AUTOLAND if (STATE(AIRPLANE)) { - int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8; + int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8, approachSettingIdx = -1; + #ifdef USE_MULTI_MISSION missionIdx = posControl.loadedMultiMissionIndex - 1; #endif @@ -1696,20 +1705,27 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF shIdx = posControl.safehomeState.index; missionFwLandConfigStartIdx = MAX_SAFE_HOMES; #endif - if (!posControl.fwLandState.landAborted && (shIdx >= 0 || missionIdx >= 0) && (fwAutolandApproachConfig(shIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(shIdx)->landApproachHeading2 != 0)) { + if (previousState == NAV_STATE_WAYPOINT_REACHED && missionIdx >= 0) { + approachSettingIdx = missionFwLandConfigStartIdx + missionIdx; + } else if (shIdx >= 0) { + approachSettingIdx = shIdx; + } + if (!posControl.fwLandState.landAborted && approachSettingIdx >= 0 && (fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading2 != 0)) { if (previousState == NAV_STATE_WAYPOINT_REACHED) { posControl.fwLandState.landPos = posControl.activeWaypoint.pos; - posControl.fwLandState.approachSettingIdx = missionFwLandConfigStartIdx + missionIdx; posControl.fwLandState.landWp = true; } else { posControl.fwLandState.landPos = posControl.safehomeState.nearestSafeHome; - posControl.fwLandState.approachSettingIdx = shIdx; posControl.fwLandState.landWp = false; } + posControl.fwLandState.approachSettingIdx = approachSettingIdx; + posControl.fwLandState.approachSettingIdx = approachSettingIdx; posControl.fwLandState.landAltAgl = fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt; + posControl.fwLandState.landAproachAltAgl = fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachAlt; + return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING; } } @@ -1779,6 +1795,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav resetPositionController(); resetAltitudeController(false); // Make sure surface tracking is not enabled - WP uses global altitude, not AGL +#ifdef USE_FW_AUTOLAND + if (previousState != NAV_STATE_FW_LANDING_ABORT) { + posControl.fwLandState.landAborted = false; + } +#endif + if (posControl.activeWaypointIndex == posControl.startWpIndex || posControl.wpMissionRestart) { /* Use p3 as the volatile jump counter, allowing embedded, rearmed jumps Using p3 minimises the risk of saving an invalid counter if a mission is aborted */ @@ -1818,7 +1840,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav case NAV_WP_ACTION_LAND: calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]); posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); - posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; + // posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; // CR97 posControl.wpAltitudeReached = false; return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS @@ -1882,10 +1904,25 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na fpVector3_t tmpWaypoint; tmpWaypoint.x = posControl.activeWaypoint.pos.x; tmpWaypoint.y = posControl.activeWaypoint.pos.y; - tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance), - posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f, - posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); - setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + // CR97 + setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); + + static float climbRate = 0; + if (posControl.wpDistance > 0.25f * posControl.wpInitialDistance) { + climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) / + (posControl.wpDistance - 0.1f * posControl.wpInitialDistance); + } + + if (fabsf(climbRate) >= (STATE(AIRPLANE) ? navConfig()->fw.max_auto_climb_rate : navConfig()->mc.max_auto_climb_rate)) { // CR97A + climbRate = 0; + } + updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET); + + // tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance), + // posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f, + // posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); + // setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + // CR97 if(STATE(MULTIROTOR)) { switch (wpHeadingControl.mode) { case NAV_WP_HEAD_MODE_NONE: @@ -1954,7 +1991,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState) { UNUSED(previousState); - /* If position sensors unavailable - land immediately (wait for timeout on GPS) */ if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; @@ -1988,11 +2024,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig { UNUSED(previousState); +#ifdef USE_FW_AUTOLAND + if (posControl.fwLandState.landAborted) { + return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED; + } +#endif + const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState); +#ifdef USE_FW_AUTOLAND if (landEvent == NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING) { return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING; - } else if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) { + } else +#endif + if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) { return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED; } else if (landEvent == NAV_FSM_EVENT_SUCCESS) { // Landing controller returned success - invoke RTH finishing state and finish the waypoint @@ -2002,6 +2047,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig else { return NAV_FSM_EVENT_NONE; } + + // if (landEvent == NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING) { + // return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING; + // } else if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) { + // return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED; + // } else if (landEvent == NAV_FSM_EVENT_SUCCESS) { + // // Landing controller returned success - invoke RTH finishing state and finish the waypoint + // navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState); + // return NAV_FSM_EVENT_SUCCESS; + // } + // else { + // return NAV_FSM_EVENT_NONE; + // } } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState) @@ -2121,6 +2179,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi return NAV_FSM_EVENT_SUCCESS; } +// CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx + if (isFixedWingLaunchFinishedThrottleLow()) { + // check if launch can switch to other preselected Nav mode + navigationFSMEvent_t isNavModeSelected = selectNavEventFromBoxModeInput(true); + + if (isNavModeSelected != NAV_FSM_EVENT_SWITCH_TO_IDLE) { + return isNavModeSelected; + } + } +// CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx + return NAV_FSM_EVENT_NONE; } @@ -2202,7 +2271,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOI } if (ABS(getEstimatedActualPosition(Z) - posControl.fwLandState.landAproachAltAgl) < (navConfig()->general.waypoint_enforce_altitude > 0 ? navConfig()->general.waypoint_enforce_altitude : FW_LAND_LOITER_ALT_TOLERANCE)) { - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); // CR97 posControl.fwLandState.landState = FW_AUTOLAND_STATE_LOITER; return NAV_FSM_EVENT_SUCCESS; } @@ -2227,7 +2296,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig } if (micros() - posControl.fwLandState.loiterStartTime > FW_LAND_LOITER_MIN_TIME) { - if (isEstimatedWindSpeedValid()) { + if (isEstimatedWindSpeedValid()) { uint16_t windAngle = 0; int32_t approachHeading = -1; @@ -2351,8 +2420,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav tmpWaypoint.x = posControl.activeWaypoint.pos.x; tmpWaypoint.y = posControl.activeWaypoint.pos.y; tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance), - posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f, - posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); + posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f, + posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); return NAV_FSM_EVENT_NONE; @@ -2366,7 +2435,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; } - if (getLandAltitude() <= posControl.fwLandState.landAltAgl + navFwAutolandConfig()->flareAltitude) { + if (posControl.flags.estAglStatus >= EST_USABLE && getLandAltitude() <= posControl.fwLandState.landAltAgl + navFwAutolandConfig()->flareAltitude) { // CR116 posControl.fwLandState.landState = FW_AUTOLAND_STATE_FLARE; return NAV_FSM_EVENT_SUCCESS; } @@ -2581,14 +2650,14 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali posControl.actualState.velXY = calc_length_pythagorean_2D(newVelX, newVelY); // CASE 1: POS & VEL valid - if (estPosValid && estVelValid) { + if (estPosValid && estVelValid) { // && !IS_RC_MODE_ACTIVE(BOXBEEPERON)) { // CR52 posControl.flags.estPosStatus = EST_TRUSTED; posControl.flags.estVelStatus = EST_TRUSTED; posControl.flags.horizontalPositionDataNew = true; posControl.lastValidPositionTimeMs = millis(); } // CASE 1: POS invalid, VEL valid - else if (!estPosValid && estVelValid) { + else if (!estPosValid && estVelValid) { // && !IS_RC_MODE_ACTIVE(BOXBEEPERON)) { // CR52 posControl.flags.estPosStatus = EST_USABLE; // Pos usable, but not trusted posControl.flags.estVelStatus = EST_TRUSTED; posControl.flags.horizontalPositionDataNew = true; @@ -2600,7 +2669,6 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali posControl.flags.estVelStatus = EST_NONE; posControl.flags.horizontalPositionDataNew = false; } - //Update blackbox data navLatestActualPosition[X] = newX; navLatestActualPosition[Y] = newY; @@ -2642,6 +2710,7 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo posControl.flags.estAglStatus = EST_NONE; posControl.flags.verticalPositionDataNew = false; posControl.flags.gpsCfEstimatedAltitudeMismatch = false; + } if (ARMING_FLAG(ARMED)) { @@ -2671,7 +2740,17 @@ void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroun /* Update heading. Check if we're acquiring a valid heading for the * first time and update home heading accordingly. */ - + // CR27 + /* Check compass heading matches GPS COG if available + * latch mismatch error if exists. Reset on disarm ? ONLY FOR TEST !! */ + if (STATE(MULTIROTOR) && ARMING_FLAG(ARMED)) { + if (!posControl.flags.compassGpsCogMismatchError) { + posControl.flags.compassGpsCogMismatchError = compassHeadingGPSCogErrorCheck(); + } else if (!ARMING_FLAG(ARMED)) { // TEST ONLY REMOVE AFTER !! + posControl.flags.compassGpsCogMismatchError = false; + } + } + // CR27 navigationEstimateStatus_e newEstHeading = headingValid ? EST_TRUSTED : EST_NONE; #ifdef USE_DEV_TOOLS @@ -2749,22 +2828,23 @@ int32_t calculateBearingBetweenLocalPositions(const fpVector3_t * startPos, cons return calculateBearingFromDelta(deltaX, deltaY); } -bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos) // NOT USED ANYWHERE -{ - if (posControl.flags.estPosStatus == EST_NONE || - posControl.flags.estHeadingStatus == EST_NONE) { +// NOT USED ANYWHERE !!! +// bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos) +// { + // if (posControl.flags.estPosStatus == EST_NONE || + // posControl.flags.estHeadingStatus == EST_NONE) { - return false; - } + // return false; + // } - const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity(); - const float deltaX = destinationPos->x - posvel->pos.x; - const float deltaY = destinationPos->y - posvel->pos.y; + // const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity(); + // const float deltaX = destinationPos->x - posvel->pos.x; + // const float deltaY = destinationPos->y - posvel->pos.y; - result->distance = calculateDistanceFromDelta(deltaX, deltaY); - result->bearing = calculateBearingFromDelta(deltaX, deltaY); - return true; -} + // result->distance = calculateDistanceFromDelta(deltaX, deltaY); + // result->bearing = calculateBearingFromDelta(deltaX, deltaY); + // return true; +// } static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos) { @@ -2790,6 +2870,7 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos) return true; } } +// tblastpointindex = posControl.rthTBWrapAroundCounter = -1 ? 0 : posControl.rthTBLastSavedIndex == NAV_RTH_TRACKBACK_POINTS - 1 ? 0 : posControl.rthTBLastSavedIndex + 1; return false; // no position available } @@ -2993,7 +3074,7 @@ void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFla updateDesiredRTHAltitude(); // Reset RTH sanity checker for new home position if RTH active - if (FLIGHT_MODE(NAV_RTH_MODE)) { + if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_FW_AUTOLAND)) { initializeRTHSanityChecker(); } @@ -3033,19 +3114,19 @@ void checkSafeHomeState(bool shouldBeEnabled) shouldBeEnabled = posControl.flags.forcedRTHActivated; } // no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything - if (posControl.safehomeState.distance == 0 || posControl.safehomeState.isApplied == shouldBeEnabled) { - return; - } + if (posControl.safehomeState.distance == 0 || posControl.safehomeState.isApplied == shouldBeEnabled) { + return; + } if (shouldBeEnabled) { - // set home to safehome + // set home to safehome setHomePosition(&posControl.safehomeState.nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); - posControl.safehomeState.isApplied = true; - } else { - // set home to original arming point + posControl.safehomeState.isApplied = true; + } else { + // set home to original arming point setHomePosition(&posControl.rthState.originalHomePosition, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); - posControl.safehomeState.isApplied = false; - } - // if we've changed the home position, update the distance and direction + posControl.safehomeState.isApplied = false; + } + // if we've changed the home position, update the distance and direction updateHomePosition(); } @@ -3077,7 +3158,7 @@ bool findNearestSafeHome(void) } } if (posControl.safehomeState.index >= 0) { - posControl.safehomeState.distance = nearest_safehome_distance; + posControl.safehomeState.distance = nearest_safehome_distance; } else { posControl.safehomeState.distance = 0; } @@ -3088,6 +3169,7 @@ bool findNearestSafeHome(void) /*----------------------------------------------------------- * Update home position, calculate distance and bearing to home *-----------------------------------------------------------*/ + void updateHomePosition(void) { // Disarmed and have a valid position, constantly update home before first arm (depending on setting) @@ -3115,7 +3197,7 @@ void updateHomePosition(void) static bool isHomeResetAllowed = false; // If pilot so desires he may reset home position to current position if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) { - if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) { + if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) { homeUpdateFlags = 0; homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setHome = true; @@ -3164,7 +3246,7 @@ bool rthAltControlStickOverrideCheck(uint8_t axis) (axis == ROLL && STATE(MULTIROTOR) && !posControl.flags.rthTrackbackActive)) { return false; } - + static timeMs_t rthOverrideStickHoldStartTime[2]; if (rxGetChannelValue(axis) > rxConfig()->maxcheck) { @@ -3248,12 +3330,13 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag if ((useMask & NAV_POS_UPDATE_XY) != 0 && !STATE(NAV_CRUISE_BRAKING)) { posControl.desiredState.pos.x = pos->x; posControl.desiredState.pos.y = pos->y; - } + } // CR99 - add check if target changed significantly -> reset PIDS if so. Same for Altitude // Z-position if ((useMask & NAV_POS_UPDATE_Z) != 0) { - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller - posControl.desiredState.pos.z = pos->z; + // updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller // CR97 + // posControl.desiredState.pos.z = pos->z; // CR97 delete + updateClimbRateToAltitudeController(0, pos->z, ROC_TO_ALT_TARGET); // CR97 } // Heading @@ -3286,9 +3369,9 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t d *-----------------------------------------------------------*/ void updateLandingStatus(timeMs_t currentTimeMs) { - if (STATE(AIRPLANE) && !navConfig()->general.flags.disarm_on_landing) { - return; // no point using this with a fixed wing if not set to disarm - } + // if (STATE(AIRPLANE) && !navConfig()->general.flags.disarm_on_landing) { + // return; // no point using this with a fixed wing if not set to disarm + // } static timeMs_t lastUpdateTimeMs = 0; if ((currentTimeMs - lastUpdateTimeMs) <= HZ2MS(100)) { // limit update to 100Hz @@ -3366,68 +3449,135 @@ bool isProbablyStillFlying(void) /*----------------------------------------------------------- * Z-position controller *-----------------------------------------------------------*/ -void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode) -{ -#define MIN_TARGET_CLIMB_RATE 100.0f // cm/s +// void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode) +// { +// #define MIN_TARGET_CLIMB_RATE 100.0f // cm/s + + // static timeUs_t lastUpdateTimeUs; + // timeUs_t currentTimeUs = micros(); + + // // Terrain following uses different altitude measurement + // const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z; + + // if (mode != ROC_TO_ALT_RESET && desiredClimbRate) { + // /* ROC_TO_ALT_CONSTANT - constant climb rate + // * ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached + // * Rate reduction starts at distance from target altitude of 5 x climb rate for FW, 1 x climb rate for MC */ + + // if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) { + // const int8_t direction = desiredClimbRate > 0 ? 1 : -1; + // const float absClimbRate = fabsf(desiredClimbRate); + // const uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate; + // const float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude, + // 0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate); + + // desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate); + // } + + // /* + // * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0 + // * In other words, when altitude is reached, allow it only to shrink + // */ + // if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0) { + // desiredClimbRate = 0; + // } + // // CR97M + // if (STATE(FIXED_WING_LEGACY)) { + // // Fixed wing climb rate controller is open-loop. We simply move the known altitude target + // float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs); + // static bool targetHoldActive = false; + + // if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) { + // // Update target altitude only if actual altitude moving in same direction and lagging by < 5 m, otherwise hold target + // if (navGetCurrentActualPositionAndVelocity()->vel.z * desiredClimbRate >= 0 && fabsf(posControl.desiredState.pos.z - altitudeToUse) < 500) { + // posControl.desiredState.pos.z += desiredClimbRate * timeDelta; + // targetHoldActive = false; + // } else if (!targetHoldActive) { // Reset and hold target to actual + climb rate boost until actual catches up + // posControl.desiredState.pos.z = altitudeToUse + desiredClimbRate; + // targetHoldActive = true; + // } + // } else { + // targetHoldActive = false; + // } + // } + // else { + // // Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD + // posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP); + // } + // } else { // ROC_TO_ALT_RESET or zero desired climbrate + // posControl.desiredState.pos.z = altitudeToUse; + // } + + // lastUpdateTimeUs = currentTimeUs; +// } + +// CR97 +float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) +{ + float targetVel = 0.0f; + const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); + + float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate; // CR97A + if (posControl.desiredState.climbRateDemand) { + maxClimbRate = ABS(posControl.desiredState.climbRateDemand); + } else if (emergLandingIsActive) { + maxClimbRate = navConfig()->general.emerg_descent_rate; + } else if (posControl.flags.isAdjustingAltitude) { + maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_manual_climb_rate : navConfig()->fw.max_manual_climb_rate; // CR97A + } + const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z; - static timeUs_t lastUpdateTimeUs; - timeUs_t currentTimeUs = micros(); + if (STATE(MULTIROTOR)) { + targetVel = getSqrtControllerVelocity(targetAltitude, deltaMicros); + } else { + targetVel = pidProfile()->fwAltControlResponseFactor * targetAltitudeError / 100.0f; + } - // Terrain following uses different altitude measurement - const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z; + if (emergLandingIsActive && targetAltitudeError > -50) { + return -100; + } else { + return constrainf(targetVel, -maxClimbRate, maxClimbRate); + } +} - if (mode != ROC_TO_ALT_RESET && desiredClimbRate) { - /* ROC_TO_ALT_CONSTANT - constant climb rate - * ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached - * Rate reduction starts at distance from target altitude of 5 x climb rate */ +void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode) +{ + /* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to 0 when reached. + * Rate reduction starts at distance from target altitude of 5 x climb rate for Fixed wing. + * Climb rate = max allowed climb rate unless set to 0 in which case default max limits are used. + * + * ROC_TO_ALT_CURRENT - similar to ROC_TO_ALT_TARGET except target altitude set to current altitude. + * No climb rate or altitude target required. + * + * ROC_TO_ALT_CONSTANT - constant climb rate. Climb rate and direction required. Target alt not required. */ - if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) { - const int8_t direction = desiredClimbRate > 0 ? 1 : -1; - const float absClimbRate = fabsf(desiredClimbRate); - const uint16_t maxRateCutoffAlt = absClimbRate * 5; - const float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude, - 0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate); + if (mode == ROC_TO_ALT_CURRENT) { + posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z; + } else if (mode == ROC_TO_ALT_TARGET) { + posControl.desiredState.pos.z = targetAltitude; + } - desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate); - } + posControl.desiredState.climbRateDemand = desiredClimbRate; + posControl.flags.rocToAltMode = mode; - /* - * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0 - * In other words, when altitude is reached, allow it only to shrink - */ - if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0) { - desiredClimbRate = 0; - } + /* + * If max altitude is set and altitude limit reached reset altitude target to max altitude unless desired climb rate is -ve. + * Inhibit during RTH mode and also WP mode with altitude enforce active to prevent climbs getting stuck at max alt limit. + */ + if (navConfig()->general.max_altitude && !FLIGHT_MODE(NAV_RTH_MODE) && !(FLIGHT_MODE(NAV_WP_MODE) && navConfig()->general.waypoint_enforce_altitude)) { + posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude); - if (STATE(FIXED_WING_LEGACY)) { - // Fixed wing climb rate controller is open-loop. We simply move the known altitude target - float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs); - static bool targetHoldActive = false; - - if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) { - // Update target altitude only if actual altitude moving in same direction and lagging by < 5 m, otherwise hold target - if (navGetCurrentActualPositionAndVelocity()->vel.z * desiredClimbRate >= 0 && fabsf(posControl.desiredState.pos.z - altitudeToUse) < 500) { - posControl.desiredState.pos.z += desiredClimbRate * timeDelta; - targetHoldActive = false; - } else if (!targetHoldActive) { // Reset and hold target to actual + climb rate boost until actual catches up - posControl.desiredState.pos.z = altitudeToUse + desiredClimbRate; - targetHoldActive = true; - } - } else { - targetHoldActive = false; - } + if (mode != ROC_TO_ALT_CONSTANT || (mode == ROC_TO_ALT_CONSTANT && desiredClimbRate < 0.0f)) { + return; } - else { - // Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD - posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP); + + if (posControl.flags.isAdjustingAltitude) { + posControl.desiredState.pos.z = navConfig()->general.max_altitude; + posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET; } - } else { // ROC_TO_ALT_RESET or zero desired climbrate - posControl.desiredState.pos.z = altitudeToUse; } - - lastUpdateTimeUs = currentTimeUs; } - +// CR97 static void resetAltitudeController(bool useTerrainFollowing) { // Set terrain following flag @@ -3488,8 +3638,6 @@ static void clearJumpCounters(void) } } - - /*----------------------------------------------------------- * Heading controller (pass-through to MAG mode) *-----------------------------------------------------------*/ @@ -3535,7 +3683,6 @@ static bool adjustPositionFromRCInput(void) retValue = adjustFixedWingPositionFromRCInput(); } else { - const int16_t rcPitchAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband, -500, 500); const int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500); @@ -3648,7 +3795,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) } // WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) { - if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND || wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD ) { + if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND || wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD) { // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission) static int8_t nonGeoWaypointCount = 0; @@ -3853,7 +4000,7 @@ bool saveNonVolatileWaypointList(void) return true; } -#endif +#endif // NAV_NON_VOLATILE_WAYPOINT_STORAGE #if defined(USE_SAFE_HOME) @@ -3935,6 +4082,7 @@ bool isNavHoldPositionActive(void) // Also hold position during emergency landing if position valid return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) || FLIGHT_MODE(NAV_POSHOLD_MODE) || + (posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) || navigationIsExecutingAnEmergencyLanding(); } @@ -3974,7 +4122,8 @@ bool isWaypointNavTrackingActive(void) // is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP. // (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called) - return FLIGHT_MODE(NAV_WP_MODE) || (posControl.flags.rthTrackbackActive && rth_trackback.activePointIndex != rth_trackback.lastSavedIndex); + return FLIGHT_MODE(NAV_WP_MODE) || posControl.navState == NAV_STATE_FW_LANDING_APPROACH || + (posControl.flags.rthTrackbackActive && rth_trackback.activePointIndex != rth_trackback.lastSavedIndex); // CR116 } /*----------------------------------------------------------- @@ -4014,7 +4163,9 @@ void applyWaypointNavigationAndAltitudeHold(void) if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1); if (posControl.flags.estPosStatus == EST_TRUSTED) navFlags |= (1 << 2); if (posControl.flags.isTerrainFollowEnabled) navFlags |= (1 << 3); - // naFlags |= (1 << 4); // Old NAV GPS Glitch Detection flag +#if defined(NAV_GPS_GLITCH_DETECTION) + if (isGPSGlitchDetected()) navFlags |= (1 << 4); +#endif if (posControl.flags.estHeadingStatus == EST_TRUSTED) navFlags |= (1 << 5); // Reset all navigation requests - NAV controllers will set them if necessary @@ -4039,7 +4190,7 @@ void applyWaypointNavigationAndAltitudeHold(void) posControl.flags.verticalPositionDataConsumed = false; #ifdef USE_FW_AUTOLAND - if (!isFwLandInProgess()) { + if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) { posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE; } #endif @@ -4080,7 +4231,7 @@ void applyWaypointNavigationAndAltitudeHold(void) void switchNavigationFlightModes(void) { const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState); - const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes); + const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes); DISABLE_FLIGHT_MODE(disabledFlightModes); ENABLE_FLIGHT_MODE(enabledNavFlightModes); } @@ -4141,10 +4292,27 @@ void checkManualEmergencyLandingControl(bool forcedActivation) navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE); } } + + // ***** frig to trigger using beeper mode + if (!IS_RC_MODE_ACTIVE(BOXBEEPERON) && posControl.flags.manualEmergLandActive) { + navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE); + } + posControl.flags.manualEmergLandActive = IS_RC_MODE_ACTIVE(BOXBEEPERON); + // ***** } -static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) +static navigationFSMEvent_t selectNavEventFromBoxModeInput(bool launchBypass) // CR6 { + // General use debugs + // DEBUG_SET(DEBUG_ALWAYS, 0, posControl.rthTBPointsList[0].x); + // DEBUG_SET(DEBUG_ALWAYS, 1, posControl.rthTBPointsList[1].x); + // DEBUG_SET(DEBUG_ALWAYS, 2, isFixedWingFlying()); + // DEBUG_SET(DEBUG_ALWAYS, 3, posControl.rthTBPointsList[3].x); + // DEBUG_SET(DEBUG_ALWAYS, 4, posControl.rthTBPointsList[4].x); + // DEBUG_SET(DEBUG_ALWAYS, 5, posControl.rthTBPointsList[5].x); + // DEBUG_SET(DEBUG_ALWAYS, 6, posControl.rthTBPointsList[5].x); + // DEBUG_SET(DEBUG_ALWAYS, 7, posControl.activeRthTBPointIndex); + static bool canActivateLaunchMode = false; //We can switch modes only when ARMED @@ -4172,7 +4340,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) checkManualEmergencyLandingControl(false); /* Emergency landing triggered by failsafe Landing or manually initiated */ - if (posControl.flags.forcedEmergLandingActivated || posControl.flags.manualEmergLandActive) { + if (posControl.flags.forcedEmergLandingActivated || posControl.flags.manualEmergLandActive) { // || IS_RC_MODE_ACTIVE(BOXBEEPERON) return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -4192,22 +4360,6 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } posControl.rthSanityChecker.rthSanityOK = true; - /* WP mission activation control: - * canActivateWaypoint & waypointWasActivated are used to prevent WP mission - * auto restarting after interruption by Manual or RTH modes. - * WP mode must be deselected before it can be reactivated again. */ - static bool waypointWasActivated = false; - const bool isWpMissionLoaded = isWaypointMissionValid(); - bool canActivateWaypoint = isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; // Block activation if using WP Mission Planner - - if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) { - canActivateWaypoint = false; - if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) { - canActivateWaypoint = true; - waypointWasActivated = false; - } - } - /* Airplane specific modes */ if (STATE(AIRPLANE)) { // LAUNCH mode has priority over any other NAV mode @@ -4216,7 +4368,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) canActivateLaunchMode = false; return NAV_FSM_EVENT_SWITCH_TO_LAUNCH; } - else if FLIGHT_MODE(NAV_LAUNCH_MODE) { + // CR6 xxxxxxxxxxxxxxxxxxxxx + // allow launch priority bypass at launch finish with throttle low + // to check if possible to switch to other preselected Nav mode + else if (FLIGHT_MODE(NAV_LAUNCH_MODE) && !launchBypass) { + // CR6 xxxxxxxxxxxxxxxxxxxxx // Make sure we don't bail out to IDLE return NAV_FSM_EVENT_NONE; } @@ -4234,7 +4390,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) /* Soaring mode, disables altitude control in Position hold and Course hold modes. * Pitch allowed to freefloat within defined Angle mode deadband */ - if (IS_RC_MODE_ACTIVE(BOXSOARING) && (FLIGHT_MODE(NAV_POSHOLD_MODE) || FLIGHT_MODE(NAV_COURSE_HOLD_MODE))) { + if (IS_RC_MODE_ACTIVE(BOXSOARING) && (FLIGHT_MODE(NAV_POSHOLD_MODE) || FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || IS_RC_MODE_ACTIVE(BOXNAVWP))) { ENABLE_FLIGHT_MODE(SOARING_MODE); } else { DISABLE_FLIGHT_MODE(SOARING_MODE); @@ -4247,14 +4403,36 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_RTH; } - /* Pilot-triggered RTH, also fall-back for WP if there is no mission loaded. - * WP prevented from falling back to RTH if WP mission planner is active */ - const bool wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; + /* WP mission activation control: + * canActivateWaypoint & waypointWasActivated are used to prevent WP mission + * auto restarting after interruption by Manual or RTH modes. + * WP mode must be deselected before it can be reactivated again + * WP Mode also inhibited when Mission Planner is active */ + static bool waypointWasActivated = false; + bool canActivateWaypoint = isWaypointMissionValid(); + bool wpRthFallbackIsActive = false; + + if (IS_RC_MODE_ACTIVE(BOXMANUAL) || posControl.flags.wpMissionPlannerActive) { + canActivateWaypoint = false; + } else { + if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) { + canActivateWaypoint = false; + + if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) { + canActivateWaypoint = true; + waypointWasActivated = false; + } + } + + wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint; + } + + /* Pilot-triggered RTH, also fall-back for WP if no mission is loaded. + * Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss + * Without this loss of any of the canActivateNavigation && canActivateAltHold + * will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back + * logic kicking in (waiting for GPS on airplanes, switch to emergency landing etc) */ if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || wpRthFallbackIsActive) { - // Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss - // Without this loss of any of the canActivateNavigation && canActivateAltHold - // will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back - // logic kicking in (waiting for GPS on airplanes, switch to emergency landing etc) if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { return NAV_FSM_EVENT_SWITCH_TO_RTH; } @@ -4268,11 +4446,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) // Pilot-activated waypoint mission. Fall-back to RTH if no mission loaded. // Also check multimission mission change status before activating WP mode. #ifdef USE_MULTI_MISSION - if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) { + if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP)) { #else - if (IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) { + if (IS_RC_MODE_ACTIVE(BOXNAVWP)) { #endif - if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { + if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { waypointWasActivated = true; return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT; } @@ -4555,7 +4733,7 @@ void updateWaypointsAndNavigationMode(void) updateFlightBehaviorModifiers(); // Process switch to a different navigation mode (if needed) - navProcessFSMEvents(selectNavEventFromBoxModeInput()); + navProcessFSMEvents(selectNavEventFromBoxModeInput(false)); // CR6 // Process pilot's RC input to adjust behaviour processNavigationRCAdjustments(); @@ -4587,14 +4765,12 @@ void navigationUsePIDs(void) // Initialize position hold P-controller for (int axis = 0; axis < 2; axis++) { - navPidInit( - &posControl.pids.pos[axis], - (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f, - 0.0f, - 0.0f, - 0.0f, - NAV_DTERM_CUT_HZ, - 0.0f + navPidInit(&posControl.pids.pos[axis], (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f, + 0.0f, + 0.0f, + 0.0f, + NAV_DTERM_CUT_HZ, + 0.0f ); navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f, @@ -4609,23 +4785,25 @@ void navigationUsePIDs(void) /* * Set coefficients used in MC VEL_XY */ - multicopterPosXyCoefficients.dTermAttenuation = pidProfile()->navVelXyDtermAttenuation / 100.0f; - multicopterPosXyCoefficients.dTermAttenuationStart = pidProfile()->navVelXyDtermAttenuationStart / 100.0f; - multicopterPosXyCoefficients.dTermAttenuationEnd = pidProfile()->navVelXyDtermAttenuationEnd / 100.0f; + multicopterPosXyCoefficients.dTermAttenuation = pidProfile()->navVelXyDtermAttenuation / 100.0f; + // CR47 + multicopterPosXyCoefficients.dTermAttenuationStart = pidProfile()->navVelXyDtermAttenuationStart * 100.0f; + multicopterPosXyCoefficients.dTermAttenuationEnd = pidProfile()->navVelXyDtermAttenuationEnd * 100.0f; + // multicopterPosXyCoefficients.dTermAttenuationStart = pidProfile()->navVelXyDtermAttenuationStart / 100.0f; + // multicopterPosXyCoefficients.dTermAttenuationEnd = pidProfile()->navVelXyDtermAttenuationEnd / 100.0f; + // CR47 #ifdef USE_MR_BRAKING_MODE multicopterPosXyCoefficients.breakingBoostFactor = (float) navConfig()->mc.braking_boost_factor / 100.0f; #endif // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z - navPidInit( - &posControl.pids.pos[Z], - (float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f, - 0.0f, - 0.0f, - 0.0f, - NAV_DTERM_CUT_HZ, - 0.0f + navPidInit(&posControl.pids.pos[Z], (float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f, + 0.0f, + 0.0f, + 0.0f, + NAV_DTERM_CUT_HZ, + 0.0f ); navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->bank_mc.pid[PID_VEL_Z].P / 66.7f, @@ -4655,9 +4833,10 @@ void navigationUsePIDs(void) 0.0f ); - navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f, + navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, // CR97 uses 100, original was 10 + (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f, // CR97 uses 100, original was 10 + (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f, // CR97 uses 100, original was 10 + // (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f, // CR97 new FF, original was 0.0f 0.0f, NAV_DTERM_CUT_HZ, 0.0f @@ -4685,6 +4864,7 @@ void navigationInit(void) posControl.flags.estVelStatus = EST_NONE; posControl.flags.estHeadingStatus = EST_NONE; posControl.flags.estAglStatus = EST_NONE; + posControl.flags.compassGpsCogMismatchError = false; // CR27 posControl.flags.forcedRTHActivated = false; posControl.flags.forcedEmergLandingActivated = false; @@ -4725,6 +4905,8 @@ void navigationInit(void) loadNonVolatileWaypointList(false); } #endif + +// srand(time(NULL)); // CR97 hitl test } /*----------------------------------------------------------- @@ -4750,7 +4932,7 @@ void activateForcedRTH(void) #ifdef USE_SAFE_HOME checkSafeHomeState(true); #endif - navProcessFSMEvents(selectNavEventFromBoxModeInput()); + navProcessFSMEvents(selectNavEventFromBoxModeInput(false)); // CR6 } void abortForcedRTH(void) @@ -4767,7 +4949,7 @@ void abortForcedRTH(void) rthState_e getStateOfForcedRTH(void) { /* If forced RTH activated and in AUTO_RTH or EMERG state */ - if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT))) { + if (posControl.flags.forcedRTHActivated && ((navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT)) || FLIGHT_MODE(NAV_FW_AUTOLAND))) { if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) { return RTH_HAS_LANDED; } @@ -4779,7 +4961,6 @@ rthState_e getStateOfForcedRTH(void) return RTH_IDLE; } } - /*----------------------------------------------------------- * Ability to execute Emergency Landing on external event *-----------------------------------------------------------*/ @@ -4787,12 +4968,12 @@ void activateForcedEmergLanding(void) { abortFixedWingLaunch(); posControl.flags.forcedEmergLandingActivated = true; - navProcessFSMEvents(selectNavEventFromBoxModeInput()); + navProcessFSMEvents(selectNavEventFromBoxModeInput(false)); // CR6 } void abortForcedEmergLanding(void) { - // Disable emergency landing and make sure we back out of navigation mode to IDLE + // Disable forced emergency landing and make sure we back out of navigation mode to IDLE // If any navigation mode was active prior to emergency landing it will be re-enabled with next RX update posControl.flags.forcedEmergLandingActivated = false; navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE); @@ -4830,6 +5011,14 @@ bool navigationInAutomaticThrottleMode(void) ((stateFlags & NAV_CTL_LAUNCH) && !navConfig()->fw.launch_manual_throttle); } +// CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx +bool launchAllowedWithThrottleLow(void) +{ + navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); + return (stateFlags & NAV_CTL_LAUNCH) && navConfig()->fw.launch_allow_throttle_low; +} +// CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx + bool navigationIsControllingThrottle(void) { // Note that this makes a detour into mixer code to evaluate actual motor status @@ -4849,6 +5038,12 @@ bool navigationIsFlyingAutonomousMode(void) bool navigationRTHAllowsLanding(void) { +#ifdef USE_FW_AUTOLAND + if (posControl.fwLandState.landAborted) { + return false; + } +#endif + // WP mission RTH landing setting if (isWaypointMissionRTHActive() && isWaypointMissionValid()) { return posControl.waypointList[posControl.startWpIndex + posControl.waypointCount - 1].p1 > 0; @@ -4856,8 +5051,7 @@ bool navigationRTHAllowsLanding(void) // normal RTH landing setting navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing; - return allow == NAV_RTH_ALLOW_LANDING_ALWAYS || - (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE)); + return allow == NAV_RTH_ALLOW_LANDING_ALWAYS || (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE)); } bool isNavLaunchEnabled(void) @@ -4896,7 +5090,8 @@ bool isAdjustingHeading(void) { } int32_t getCruiseHeadingAdjustment(void) { - return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse); + return posControl.cruise.course; + // return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse); } int32_t navigationGetHeadingError(void) @@ -5006,15 +5201,6 @@ void resetFwAutolandApproach(int8_t idx) } } -bool isFwLandInProgess(void) -{ - return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER - || posControl.navState == NAV_STATE_FW_LANDING_LOITER - || posControl.navState == NAV_STATE_FW_LANDING_APPROACH - || posControl.navState == NAV_STATE_FW_LANDING_GLIDE - || posControl.navState == NAV_STATE_FW_LANDING_FLARE; -} - bool canFwLandCanceld(void) { return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 84e5390d43e..5cdf1f02c36 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -120,7 +120,7 @@ void resetFwAutolandApproach(int8_t idx); #define NAV_MAX_WAYPOINTS 15 #endif -#define NAV_ACCEL_CUTOFF_FREQUENCY_HZ 2 // low-pass filter on XY-acceleration target +#define NAV_ACCEL_CUTOFF_FREQUENCY_HZ 2 // low-pass filter on XY-acceleration target enum { NAV_GPS_ATTI = 0, // Pitch/roll stick controls attitude (pitch/roll lean angles) @@ -192,12 +192,6 @@ typedef enum { RTH_CLIMB_ON_FW_SPIRAL, } navRTHClimbFirst_e; -typedef enum { // keep aligned with fixedWingLaunchState_t - FW_LAUNCH_DETECTED = 4, - FW_LAUNCH_ABORTED = 9, - FW_LAUNCH_FLYING = 10, -} navFwLaunchStatus_e; - typedef enum { WP_PLAN_WAIT, WP_PLAN_SAVE, @@ -205,6 +199,12 @@ typedef enum { WP_PLAN_FULL, } wpMissionPlannerStatus_e; +typedef enum { // keep aligned with fixedWingLaunchState_t + FW_LAUNCH_DETECTED = 4, + FW_LAUNCH_ABORTED = 10, + FW_LAUNCH_FLYING = 11, +} navFwLaunchStatus_e; + typedef enum { WP_MISSION_START, WP_MISSION_RESUME, @@ -265,7 +265,7 @@ typedef struct positionEstimationConfig_s { #ifdef USE_GPS_FIX_ESTIMATION uint8_t allow_gps_fix_estimation; -#endif +#endif } positionEstimationConfig_t; PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig); @@ -351,6 +351,7 @@ typedef struct navConfig_s { struct { uint8_t max_bank_angle; // Fixed wing max banking angle (deg) + uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec // CR97A uint16_t max_manual_climb_rate; // manual velocity control max vertical speed uint8_t max_climb_angle; // Fixed wing max banking angle (deg) uint8_t max_dive_angle; // Fixed wing max banking angle (deg) @@ -369,11 +370,14 @@ typedef struct navConfig_s { uint16_t launch_idle_motor_timer; // Time to wait before motor starts at_idle 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) uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] + // CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx + bool launch_allow_throttle_low; // Allow launch with throttle low + // xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx bool launch_manual_throttle; // Allows launch with manual throttle control uint8_t launch_land_abort_deadband; // roll/pitch stick movement deadband for launch abort uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled @@ -654,6 +658,9 @@ emergLandState_e getStateOfForcedEmergLanding(void); /* Getter functions which return data about the state of the navigation system */ bool navigationInAutomaticThrottleMode(void); +// CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx +bool launchAllowedWithThrottleLow(void); +// CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx bool navigationIsControllingThrottle(void); bool isFixedWingAutoThrottleManuallyIncreased(void); bool navigationIsFlyingAutonomousMode(void); @@ -668,6 +675,7 @@ bool isWaypointMissionRTHActive(void); bool rthClimbStageActiveAndComplete(void); bool isNavLaunchEnabled(void); +bool isFixedWingLaunchFinishedThrottleLow(void); // CR6 uint8_t fixedWingLaunchStatus(void); const char * fixedWingLaunchStateMessage(void); @@ -700,9 +708,9 @@ uint8_t getActiveWpNumber(void); * are in the [0, 360 * 100) interval. */ int32_t navigationGetHomeHeading(void); +uint8_t getActiveWpNumber(void); // CR112 #ifdef USE_FW_AUTOLAND -bool isFwLandInProgess(void); bool canFwLandCanceld(void); #endif diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index db4b3229d58..9c207018dce 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -122,7 +122,7 @@ bool adjustFixedWingAltitudeFromRCInput(void) else { // Adjusting finished - reset desired position to stay exactly where pilot released the stick if (posControl.flags.isAdjustingAltitude) { - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); // CR97 } return false; } @@ -135,46 +135,86 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) // On a fixed wing we might not have a reliable climb rate source (if no BARO available), so we can't apply PID controller to // velocity error. We use PID controller on altitude error and calculate desired pitch angle + // CR97 + float desiredClimbRate = posControl.desiredState.climbRateDemand; - // Update energies - const float demSPE = (posControl.desiredState.pos.z * 0.01f) * GRAVITY_MSS; - const float demSKE = 0.0f; - - const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z * 0.01f) * GRAVITY_MSS; - const float estSKE = 0.0f; - - // speedWeight controls balance between potential and kinetic energy used for pitch controller - // speedWeight = 1.0 : pitch will only control airspeed and won't control altitude - // speedWeight = 0.5 : pitch will be used to control both airspeed and altitude - // speedWeight = 0.0 : pitch will only control altitude - const float speedWeight = 0.0f; // no speed sensing for now - - const float demSEB = demSPE * (1.0f - speedWeight) - demSKE * speedWeight; - const float estSEB = estSPE * (1.0f - speedWeight) - estSKE * speedWeight; + if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) { + desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); + } - // SEB to pitch angle gain to account for airspeed (with respect to specified reference (tuning) speed - const float pitchGainInv = 1.0f / 1.0f; + // Reduce max allowed climb if performing loiter + if (needToCalculateCircularLoiter && desiredClimbRate > 0.0f) { + desiredClimbRate *= 0.67f; + } +// DEBUG_SET(DEBUG_ALWAYS, 0, desiredClimbRate); // Here we use negative values for dive for better clarity - float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); + const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle); - // Reduce max allowed climb pitch if performing loiter (stall prevention) - if (needToCalculateCircularLoiter) { - maxClimbDeciDeg = maxClimbDeciDeg * 0.67f; - } - // PID controller to translate energy balance error [J] into pitch angle [decideg] - float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv, 1.0f); - + float currentClimbRate = navGetCurrentActualPositionAndVelocity()->vel.z; + float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR); + // CR97 +// DEBUG_SET(DEBUG_ALWAYS, 1, targetPitchAngle); +// DEBUG_SET(DEBUG_ALWAYS, 2, desiredClimbRate - currentClimbRate); // Apply low-pass filter to prevent rapid correction targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros)); - +// DEBUG_SET(DEBUG_ALWAYS, 3, targetPitchAngle); +// DEBUG_SET(DEBUG_ALWAYS, 4, posControl.desiredState.pos.z); // Reconstrain pitch angle ( >0 - climb, <0 - dive) targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg); posControl.rcAdjustment[PITCH] = targetPitchAngle; + + posControl.desiredState.vel.z = desiredClimbRate; + navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.z), -32678, 32767); } +// static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) +// { + // static pt1Filter_t velzFilterState; + + // // On a fixed wing we might not have a reliable climb rate source (if no BARO available), so we can't apply PID controller to + // // velocity error. We use PID controller on altitude error and calculate desired pitch angle + // // Update energies + // const float demSPE = (posControl.desiredState.pos.z * 0.01f) * GRAVITY_MSS; + // const float demSKE = 0.0f; + + // const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z * 0.01f) * GRAVITY_MSS; + // const float estSKE = 0.0f; + + // // speedWeight controls balance between potential and kinetic energy used for pitch controller + // // speedWeight = 1.0 : pitch will only control airspeed and won't control altitude + // // speedWeight = 0.5 : pitch will be used to control both airspeed and altitude + // // speedWeight = 0.0 : pitch will only control altitude + // const float speedWeight = 0.0f; // no speed sensing for now + + // const float demSEB = demSPE * (1.0f - speedWeight) - demSKE * speedWeight; + // const float estSEB = estSPE * (1.0f - speedWeight) - estSKE * speedWeight; + + // // SEB to pitch angle gain to account for airspeed (with respect to specified reference (tuning) speed + // const float pitchGainInv = 1.0f / 1.0f; + + // // Here we use negative values for dive for better clarity + // float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); + // const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle); + + // // Reduce max allowed climb pitch if performing loiter (stall prevention) + // if (needToCalculateCircularLoiter) { + // maxClimbDeciDeg = maxClimbDeciDeg * 0.67f; + // } + + // // PID controller to translate energy balance error [J] into pitch angle [decideg] + // float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv, 1.0f); + + // // Apply low-pass filter to prevent rapid correction + // targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros)); + + // // Reconstrain pitch angle ( >0 - climb, <0 - dive) + // targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg); + // posControl.rcAdjustment[PITCH] = targetPitchAngle; +// } + void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs) { static timeUs_t previousTimePositionUpdate = 0; // Occurs @ altitude sensor update rate (max MAX_ALTITUDE_UPDATE_RATE_HZ) @@ -271,7 +311,7 @@ static int8_t loiterDirection(void) { static void calculateVirtualPositionTarget_FW(float trackingPeriod) { - if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || posControl.fwLandState.landState >= FW_AUTOLAND_STATE_GLIDE) { // CR116 return; } @@ -311,7 +351,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) float turnStartFactor; if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) { // passes through WP turnStartFactor = waypointTurnAngle / 6000.0f; - } else { // // cut inside turn missing WP + } else { // cut inside turn missing WP turnStartFactor = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 1.0f, 2.0f); } // velXY provides additional turn initiation distance based on an assumed 1 second delayed turn response time @@ -345,10 +385,13 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY); } - // Calculate virtual waypoint - virtualDesiredPosition.x = navGetCurrentActualPositionAndVelocity()->pos.x + posErrorX * (trackingDistance / distanceToActualTarget); - virtualDesiredPosition.y = navGetCurrentActualPositionAndVelocity()->pos.y + posErrorY * (trackingDistance / distanceToActualTarget); + + // Calculate virtual waypoint + // CR72 + virtualDesiredPosition.x = navGetCurrentActualPositionAndVelocity()->pos.x + posErrorX; + virtualDesiredPosition.y = navGetCurrentActualPositionAndVelocity()->pos.y + posErrorY; + // CR72 // Shift position according to pilot's ROLL input (up to max_manual_speed velocity) if (posControl.flags.isAdjustingPosition) { int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500); @@ -357,6 +400,10 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) float rcShiftY = rcRollAdjustment * navConfig()->general.max_manual_speed / 500.0f * trackingPeriod; // Rotate this target shift from body frame to to earth frame and apply to position target + // CR72 + virtualDesiredPosition.x += posErrorX * ((trackingDistance / distanceToActualTarget) - 1); + virtualDesiredPosition.y += posErrorY * ((trackingDistance / distanceToActualTarget) - 1); + // CR72 virtualDesiredPosition.x += -rcShiftY * posControl.actualState.sinYaw; virtualDesiredPosition.y += rcShiftY * posControl.actualState.cosYaw; } @@ -405,14 +452,14 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta static bool forceTurnDirection = false; int32_t virtualTargetBearing; - if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || posControl.fwLandState.landState >= FW_AUTOLAND_STATE_GLIDE) { // CR116 virtualTargetBearing = posControl.desiredState.yaw; } else { // We have virtual position target, calculate heading error virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); } - - /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */ + // DEBUG_SET(DEBUG_ALWAYS, 0, virtualTargetBearing); + /* If waypoint tracking enabled force craft toward waypoint course line and hold on course line */ if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { // courseVirtualCorrection initially used to determine current position relative to course line for later use int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); @@ -421,11 +468,12 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta // tracking only active when certain distance and heading conditions are met if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) { int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog); - + // DEBUG_SET(DEBUG_ALWAYS, 2, courseHeadingError); + // DEBUG_SET(DEBUG_ALWAYS, 3, posControl.activeWaypoint.bearing); // captureFactor adjusts distance/heading sensitivity balance when closing in on course line. // Closing distance threashold based on speed and an assumed 1 second response time. float captureFactor = navCrossTrackError < posControl.actualState.velXY ? constrainf(2.0f - ABS(courseHeadingError) / 500.0f, 0.0f, 2.0f) : 1.0f; - + // DEBUG_SET(DEBUG_ALWAYS, 5, captureFactor * 100); // bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy // initial courseCorrectionFactor based on distance to course line float courseCorrectionFactor = constrainf(captureFactor * navCrossTrackError / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f); @@ -434,16 +482,18 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta // course heading alignment factor float courseHeadingFactor = constrainf(courseHeadingError / 18000.0f, 0.0f, 1.0f); courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor; - - // final courseCorrectionFactor combining distance and heading factors + // DEBUG_SET(DEBUG_ALWAYS, 7, courseHeadingFactor * 100); courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f); + // DEBUG_SET(DEBUG_ALWAYS, 6, courseCorrectionFactor * 100); // final courseVirtualCorrection value courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor; + virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - courseVirtualCorrection); } } - + // DEBUG_SET(DEBUG_ALWAYS, 1, virtualTargetBearing); + // DEBUG_SET(DEBUG_ALWAYS, 4, navCrossTrackError); /* * Calculate NAV heading error * Units are centidegrees @@ -644,7 +694,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat // Manual throttle increase #ifdef USE_FW_AUTOLAND - if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !isFwLandInProgess()) { + if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND)) { #else if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) { #endif @@ -663,9 +713,9 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat #ifdef USE_FW_AUTOLAND // Advanced autoland - if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE || STATE(LANDING_DETECTED)) { + if (posControl.fwLandState.landState >= FW_AUTOLAND_STATE_GLIDE || STATE(LANDING_DETECTED)) { // CR116 // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled - rcCommand[THROTTLE] = getThrottleIdleValue(); + // rcCommand[THROTTLE] = getThrottleIdleValue(); // CR116 ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE) { @@ -683,7 +733,6 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat if ((posControl.flags.estAltStatus >= EST_USABLE && navGetCurrentActualPositionAndVelocity()->pos.z <= finalAltitude) || (posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) { - // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); @@ -726,7 +775,7 @@ bool isFixedWingLandingDetected(void) // Basic condition to start looking for landing bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) - || FLIGHT_MODE(FAILSAFE_MODE) + || FLIGHT_MODE(FAILSAFE_MODE) || posControl.fwLandState.landState >= FW_AUTOLAND_STATE_GLIDE // CR116 || (!navigationIsControllingThrottle() && throttleStickIsLow()); if (!startCondition || posControl.flags.resetLandingDetector) { @@ -775,6 +824,7 @@ bool isFixedWingLandingDetected(void) } } } + return false; } @@ -787,7 +837,8 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs) if (posControl.flags.estAltStatus >= EST_USABLE) { // target min descent rate 10m above takeoff altitude - updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000.0f, ROC_TO_ALT_TARGET); + // updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000.0f, ROC_TO_ALT_TARGET); + updateClimbRateToAltitudeController(0, 1000.0f, ROC_TO_ALT_TARGET); // CR97 applyFixedWingAltitudeAndThrottleController(currentTimeUs); int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle)); @@ -862,7 +913,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, } //if (navStateFlags & NAV_CTL_YAW) - if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) { + if (navStateFlags & NAV_CTL_ALT || navStateFlags & NAV_CTL_POS) { applyFixedWingPitchRollThrottleController(navStateFlags, currentTimeUs); } diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 5708e5dcaac..4a7912ca927 100644 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -60,14 +60,15 @@ #define FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION "READY TO LAUNCH" #define FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS "MOVE THE STICKS TO ABORT" #define FW_LAUNCH_MESSAGE_TEXT_FINISHING "FINISHING" - +#define FW_LAUNCH_MESSAGE_TEXT_FINISHING_THR_LOW "RAISE THROTTLE/MOVE STICKS" // CR6 typedef enum { FW_LAUNCH_MESSAGE_TYPE_NONE = 0, FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE, FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE, FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION, FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS, - FW_LAUNCH_MESSAGE_TYPE_FINISHING + FW_LAUNCH_MESSAGE_TYPE_FINISHING, + FW_LAUNCH_MESSAGE_TYPE_FINISHING_THR_LOW // CR6 } fixedWingLaunchMessage_t; typedef enum { @@ -76,6 +77,7 @@ typedef enum { FW_LAUNCH_EVENT_GOTO_DETECTION, FW_LAUNCH_EVENT_ABORT, FW_LAUNCH_EVENT_THROTTLE_LOW, + FW_LAUNCH_EVENT_FINISH_THR_LOW, // CR6 FW_LAUNCH_EVENT_COUNT } fixedWingLaunchEvent_t; @@ -89,8 +91,9 @@ typedef enum { // if changed update navFwLaunchStatus_e FW_LAUNCH_STATE_MOTOR_SPINUP, FW_LAUNCH_STATE_IN_PROGRESS, FW_LAUNCH_STATE_FINISH, - FW_LAUNCH_STATE_ABORTED, // 9 - FW_LAUNCH_STATE_FLYING, // 10 + FW_LAUNCH_STATE_FINISH_THR_LOW, // CR6 + FW_LAUNCH_STATE_ABORTED, // 10 + FW_LAUNCH_STATE_FLYING, // 11 FW_LAUNCH_STATE_COUNT } fixedWingLaunchState_t; @@ -103,6 +106,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t 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); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH_THR_LOW(timeUs_t currentTimeUs); // CR6 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_ABORTED(timeUs_t currentTimeUs); static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FLYING(timeUs_t currentTimeUs); @@ -197,12 +201,23 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE [FW_LAUNCH_STATE_FINISH] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FLYING + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FLYING, + // CR6 + [FW_LAUNCH_EVENT_FINISH_THR_LOW] = FW_LAUNCH_STATE_FINISH_THR_LOW + // CR6 }, .messageType = FW_LAUNCH_MESSAGE_TYPE_FINISHING }, - - [FW_LAUNCH_STATE_ABORTED] = { + // CR6 + [FW_LAUNCH_STATE_FINISH_THR_LOW] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH_THR_LOW, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FLYING + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_FINISHING_THR_LOW + }, + // CR6 + [FW_LAUNCH_STATE_ABORTED] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_ABORTED, .onEvent = { @@ -210,7 +225,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE .messageType = FW_LAUNCH_MESSAGE_TYPE_NONE }, - [FW_LAUNCH_STATE_FLYING] = { + [FW_LAUNCH_STATE_FLYING] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_FLYING, .onEvent = { @@ -282,7 +297,10 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs { UNUSED(currentTimeUs); - if (!throttleStickIsLow()) { + // CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx + if (!throttleStickIsLow() || navConfig()->fw.launch_allow_throttle_low) { + applyThrottleIdleLogic(true); // Stick not low, continue launch + if (currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue()) { return FW_LAUNCH_EVENT_SUCCESS; } @@ -290,10 +308,11 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle; return FW_LAUNCH_EVENT_GOTO_DETECTION; } - } - else { - applyThrottleIdleLogic(true); // Stick low, force mixer idle (motor stop or low rpm) - } + } + else { + applyThrottleIdleLogic(true); // Stick low, force mixer idle (motor stop or low rpm) + } + // CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx fwLaunch.pitchAngle = 0; @@ -302,7 +321,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs) { - if (throttleStickIsLow()) { + if (throttleStickIsLow() && !navConfig()->fw.launch_allow_throttle_low) { return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE } @@ -320,9 +339,11 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(tim static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) { - if (throttleStickIsLow()) { + // CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx + if (throttleStickIsLow() && !navConfig()->fw.launch_allow_throttle_low) { return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE } + // CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) { @@ -339,9 +360,11 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) { - if (throttleStickIsLow()) { + // CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx + if (throttleStickIsLow() && !navConfig()->fw.launch_allow_throttle_low) { return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE } + // CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 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); @@ -454,8 +477,24 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t endTimeMs = navConfig()->fw.launch_end_time; - if (elapsedTimeMs > endTimeMs || isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) { - return FW_LAUNCH_EVENT_SUCCESS; // End launch go to FW_LAUNCH_STATE_FLYING state + if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) { + return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FLYING state + } + // CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx + if (navConfig()->fw.launch_allow_throttle_low && throttleStickIsLow()) { + // default to cruise throttle until switch to preselected Nav mode or pilot takes control + rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.cruise_throttle; + } + // CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx + + if (elapsedTimeMs > endTimeMs) { + //CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx + if (navConfig()->fw.launch_allow_throttle_low && throttleStickIsLow()) { + return FW_LAUNCH_EVENT_FINISH_THR_LOW; + } else { + return FW_LAUNCH_EVENT_SUCCESS; + } + //CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx } else { // Make a smooth transition from the launch state to the current state for pitch angle @@ -470,6 +509,31 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr return FW_LAUNCH_EVENT_NONE; } +// CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH_THR_LOW(timeUs_t currentTimeUs) +{ + static timeMs_t throttleRaisedStartTimeMs; + const timeMs_t elapsedTimeMs = US2MS(currentTimeUs) - throttleRaisedStartTimeMs; + const timeMs_t endTimeMs = 1000; + + if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) { + return FW_LAUNCH_EVENT_SUCCESS; // end the launch and go to FW_LAUNCH_STATE_FLYING + } + + if (throttleStickIsLow()) { + throttleRaisedStartTimeMs = US2MS(currentTimeUs); + rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.cruise_throttle; + } else { + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, currentBatteryProfile->nav.fw.cruise_throttle, rcCommand[THROTTLE]); + if (elapsedTimeMs > endTimeMs) { + return FW_LAUNCH_EVENT_SUCCESS; + } + } + + return FW_LAUNCH_EVENT_NONE; +} +// CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx + static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_ABORTED(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); @@ -489,7 +553,6 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FLYING(timeUs_t curr 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); @@ -504,7 +567,7 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs) // Control beeper if (fwLaunch.currentState == FW_LAUNCH_STATE_WAIT_THROTTLE) { - beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); + beeper(BEEPER_HARDWARE_FAILURE); } else { if (idleMotorAboutToStart) { @@ -536,6 +599,13 @@ uint8_t fixedWingLaunchStatus(void) return fwLaunch.currentState; } +// CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx +bool isFixedWingLaunchFinishedThrottleLow(void) +{ + return fwLaunch.currentState == FW_LAUNCH_STATE_FINISH_THR_LOW; +} +// CR6 xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx + void abortFixedWingLaunch(void) { setCurrentState(FW_LAUNCH_STATE_ABORTED, 0); @@ -558,7 +628,10 @@ const char * fixedWingLaunchStateMessage(void) case FW_LAUNCH_MESSAGE_TYPE_FINISHING: return FW_LAUNCH_MESSAGE_TEXT_FINISHING; - + // CR6 + case FW_LAUNCH_MESSAGE_TYPE_FINISHING_THR_LOW: + return FW_LAUNCH_MESSAGE_TEXT_FINISHING_THR_LOW; + // CR6 default: return NULL; } diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index a9a6997a9f0..6fbfb7634c2 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -62,29 +62,45 @@ static int16_t altHoldThrottleRCZero = 1500; static pt1Filter_t altholdThrottleFilterState; static bool prepareForTakeoffOnReset = false; static sqrt_controller_t alt_hold_sqrt_controller; - -// Position to velocity controller for Z axis -static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) +//CR97 +float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros) { - float targetVel = sqrtControllerApply( - &alt_hold_sqrt_controller, - posControl.desiredState.pos.z, - navGetCurrentActualPositionAndVelocity()->pos.z, - US2S(deltaMicros) + return sqrtControllerApply( + &alt_hold_sqrt_controller, + targetAltitude, + navGetCurrentActualPositionAndVelocity()->pos.z, + US2S(deltaMicros) ); +} +// CR97 +// Position to velocity controller for Z axis +static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) +{// CR97 + float targetVel = posControl.desiredState.climbRateDemand; - // hard limit desired target velocity to max_climb_rate - float vel_max_z = 0.0f; - - if (posControl.flags.isAdjustingAltitude) { - vel_max_z = navConfig()->mc.max_manual_climb_rate; - } else { - vel_max_z = navConfig()->mc.max_auto_climb_rate; + if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) { + targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); } - targetVel = constrainf(targetVel, -vel_max_z, vel_max_z); + // float targetVel = sqrtControllerApply( + // &alt_hold_sqrt_controller, + // posControl.desiredState.pos.z, + // navGetCurrentActualPositionAndVelocity()->pos.z, + // US2S(deltaMicros) + // ); + + // // hard limit desired target velocity to max_climb_rate + // float vel_max_z = 0.0f; + + // if (posControl.flags.isAdjustingAltitude) { + // vel_max_z = navConfig()->general.max_manual_climb_rate; + // } else { + // vel_max_z = navConfig()->general.max_auto_climb_rate; + // } - posControl.pids.pos[Z].output_constrained = targetVel; + // targetVel = constrainf(targetVel, -vel_max_z, vel_max_z); + // CR97 + posControl.pids.pos[Z].output_constrained = targetVel; // only used for Blackbox and OSD info CR97 // Limit max up/down acceleration target const float smallVelChange = US2S(deltaMicros) * (GRAVITY_CMSS * 0.1f); @@ -106,7 +122,8 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) // Small - desired acceleration is less than 0.1G. We should be safe setting velocity target directly - any platform should be able to satisfy this posControl.desiredState.vel.z = targetVel; } - +DEBUG_SET(DEBUG_ALWAYS, 0, posControl.desiredState.vel.z); +DEBUG_SET(DEBUG_ALWAYS, 1, posControl.desiredState.pos.z); navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.z), -32678, 32767); } @@ -132,8 +149,9 @@ bool adjustMulticopterAltitudeFromRCInput(void) // In terrain follow mode we apply different logic for terrain control if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) { // We have solid terrain sensor signal - directly map throttle to altitude - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); - posControl.desiredState.pos.z = altTarget; + // updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // CR97 + // posControl.desiredState.pos.z = altTarget; + updateClimbRateToAltitudeController(0, altTarget, ROC_TO_ALT_TARGET); // CR97 } else { updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT); @@ -165,7 +183,7 @@ bool adjustMulticopterAltitudeFromRCInput(void) else { // Adjusting finished - reset desired position to stay exactly where pilot released the stick if (posControl.flags.isAdjustingAltitude) { - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); // CR97 } return false; @@ -218,12 +236,13 @@ void resetMulticopterAltitudeController(void) pt1FilterReset(&posControl.pids.vel[Z].dterm_filter_state, 0.0f); if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { - nav_speed_up = navConfig()->mc.max_auto_climb_rate; - nav_accel_z = navConfig()->mc.max_auto_climb_rate; + const float maxSpeed = getActiveSpeed(); + nav_speed_up = maxSpeed; + nav_accel_z = maxSpeed; nav_speed_down = navConfig()->mc.max_auto_climb_rate; } else { - nav_speed_up = navConfig()->mc.max_manual_climb_rate; - nav_accel_z = navConfig()->mc.max_manual_climb_rate; + nav_speed_up = navConfig()->general.max_manual_speed; + nav_accel_z = navConfig()->general.max_manual_speed; nav_speed_down = navConfig()->mc.max_manual_climb_rate; } @@ -251,7 +270,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs) if (prepareForTakeoffOnReset) { const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity(); - posControl.desiredState.vel.z = -navConfig()->mc.max_manual_climb_rate; + posControl.desiredState.vel.z = -navConfig()->mc.max_manual_climb_rate; // CR97 posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->mc.max_manual_climb_rate / posControl.pids.pos[Z].param.kP); posControl.pids.vel[Z].integrator = -500.0f; pt1FilterReset(&altholdThrottleFilterState, -500.0f); @@ -551,20 +570,29 @@ static float computeNormalizedVelocity(const float value, const float maxValue) } static float computeVelocityScale( - const float value, - const float maxValue, + float value, + // const float maxValue, // CR47 const float attenuationFactor, - const float attenuationStart, - const float attenuationEnd + const float attenuationStartVel, // CR47 + const float attenuationEndVel // CR47 ) { - const float normalized = computeNormalizedVelocity(value, maxValue); - - float scale = scaleRangef(normalized, attenuationStart, attenuationEnd, 0, attenuationFactor); - return constrainf(scale, 0, attenuationFactor); + // CR47 + value -= attenuationStartVel; + if (value <= 0.0f) { + return 0.0f; + } + const float normalized = computeNormalizedVelocity(value, attenuationEndVel); + float scale = scaleRangef(normalized, 0.0f, 1.0f, 0.0f, attenuationFactor); + return constrainf(scale, 0.0f, attenuationFactor); + + // const float normalized = computeNormalizedVelocity(value, maxValue); + // float scale = scaleRangef(normalized, attenuationStart, attenuationEnd, 0, attenuationFactor); + // return constrainf(scale, 0, attenuationFactor); + // CR47 } -static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed) +static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit) // , const float maxSpeed) CR47 { const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x; const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y; @@ -582,8 +610,8 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA const float velErrorMagnitude = calc_length_pythagorean_2D(velErrorX, velErrorY); if (velErrorMagnitude > 0.1f) { - accelLimitX = maxAccelLimit / velErrorMagnitude * fabsf(velErrorX); - accelLimitY = maxAccelLimit / velErrorMagnitude * fabsf(velErrorY); + accelLimitX = maxAccelLimit * (fabsf(velErrorX) / velErrorMagnitude); + accelLimitY = maxAccelLimit * (fabsf(velErrorY) / velErrorMagnitude); } else { accelLimitX = maxAccelLimit / 1.414213f; accelLimitY = accelLimitX; @@ -615,14 +643,15 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA */ const float setpointScale = computeVelocityScale( setpointXY, - maxSpeed, + // maxSpeed, // CR47 multicopterPosXyCoefficients.dTermAttenuation, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd ); + const float measurementScale = computeVelocityScale( posControl.actualState.velXY, - maxSpeed, + // maxSpeed, // CR47 multicopterPosXyCoefficients.dTermAttenuation, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd @@ -630,6 +659,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA //Choose smaller attenuation factor and convert from attenuation to scale const float dtermScale = 1.0f - MIN(setpointScale, measurementScale); + DEBUG_SET(DEBUG_ALWAYS, 4, dtermScale * 100); // Apply PID with output limiting and I-term anti-windup // Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit @@ -656,7 +686,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA 1.0f, // Total gain scale dtermScale // Additional dTerm scale ); - +// DEBUG_SET(DEBUG_ALWAYS, 3, newAccelX * 1000); int32_t maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.max_bank_angle); #ifdef USE_MR_BRAKING_MODE @@ -683,7 +713,6 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.braking_bank_angle); } #endif - // Save last acceleration target lastAccelTargetX = newAccelX; lastAccelTargetY = newAccelY; @@ -698,6 +727,9 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle); posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle); + + DEBUG_SET(DEBUG_ALWAYS, 2, accelForward); + DEBUG_SET(DEBUG_ALWAYS, 3, posControl.rcAdjustment[PITCH]); } static void applyMulticopterPositionController(timeUs_t currentTimeUs) @@ -734,7 +766,7 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs) // Get max speed for current NAV mode float maxSpeed = getActiveSpeed(); updatePositionVelocityController_MC(maxSpeed); - updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed); + updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX); // , maxSpeed); CR47 navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767); navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767); @@ -760,9 +792,9 @@ bool isMulticopterFlying(void) } /*----------------------------------------------------------- - * Multicopter land detector + * Multicopter landing detector *-----------------------------------------------------------*/ - #if defined(USE_BARO) +#if defined(USE_BARO) float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue) { static float baroAltRate; @@ -812,7 +844,6 @@ bool isMulticopterLandingDetected(void) return true; // Landing flagged immediately if landing bump detected } #endif - bool throttleIsBelowMidHover = rcCommand[THROTTLE] < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue())); /* Basic condition to start looking for landing @@ -827,7 +858,7 @@ bool isMulticopterLandingDetected(void) if (!startCondition || posControl.flags.resetLandingDetector) { landingDetectorStartedAt = 0; - return posControl.flags.resetLandingDetector = false; + return posControl.flags.resetLandingDetector = false; } const float sensitivity = navConfig()->general.land_detect_sensitivity / 5.0f; @@ -837,8 +868,8 @@ bool isMulticopterLandingDetected(void) posControl.actualState.velXY < (MC_LAND_CHECK_VEL_XY_MOVING * sensitivity); // check gyro rates are low (degs/s) bool gyroCondition = averageAbsGyroRates() < (4.0f * sensitivity); - DEBUG_SET(DEBUG_LANDING, 2, velCondition); - DEBUG_SET(DEBUG_LANDING, 3, gyroCondition); + // DEBUG_SET(DEBUG_LANDING, 2, velCondition); + // DEBUG_SET(DEBUG_LANDING, 3, gyroCondition); bool possibleLandingDetected = false; @@ -869,6 +900,7 @@ bool isMulticopterLandingDetected(void) possibleLandingDetected = isAtMinimalThrust && velCondition; + DEBUG_SET(DEBUG_LANDING, 4, 3); DEBUG_SET(DEBUG_LANDING, 6, rcCommandAdjustedThrottle); DEBUG_SET(DEBUG_LANDING, 7, landingThrSum / landingThrSamples - MC_LAND_DESCEND_THROTTLE); } else { // non autonomous and emergency landing @@ -920,6 +952,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) rcCommand[THROTTLE] = getThrottleIdleValue(); return; } + rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true); return; } @@ -932,7 +965,8 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) // Check if last correction was not too long ago if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { // target min descent rate 5m above takeoff altitude - updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500.0f, ROC_TO_ALT_TARGET); + // updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500.0f, ROC_TO_ALT_TARGET); + updateClimbRateToAltitudeController(0, 500.0f, ROC_TO_ALT_TARGET); // CR97 updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate); updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate); } diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 3453c75c92b..f0c2ff63b3f 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -162,6 +162,56 @@ static timeUs_t getGPSDeltaTimeFilter(timeUs_t dTus) return dTus; // Filter failed. Set GPS Hz by measurement } +#if defined(NAV_GPS_GLITCH_DETECTION) +static bool detectGPSGlitch(timeUs_t currentTimeUs) +{ + static timeUs_t previousTime = 0; + static fpVector3_t lastKnownGoodPosition; + static fpVector3_t lastKnownGoodVelocity; + + bool isGlitching = false; + +#ifdef USE_GPS_FIX_ESTIMATION + if (STATE(GPS_ESTIMATED_FIX)) { + //disable sanity checks in GPS estimation mode + //when estimated GPS fix is replaced with real fix, coordinates may jump + previousTime = 0; + return true; + } +#endif + + if (previousTime == 0) { + isGlitching = false; + } + else { + fpVector3_t predictedGpsPosition; + float gpsDistance; + float dT = US2S(currentTimeUs - previousTime); + + /* We predict new position based on previous GPS velocity and position */ + predictedGpsPosition.x = lastKnownGoodPosition.x + lastKnownGoodVelocity.x * dT; + predictedGpsPosition.y = lastKnownGoodPosition.y + lastKnownGoodVelocity.y * dT; + + /* New pos is within predefined radius of predicted pos, radius is expanded exponentially */ + gpsDistance = calc_length_pythagorean_2D(predictedGpsPosition.x - lastKnownGoodPosition.x, predictedGpsPosition.y - lastKnownGoodPosition.y); + if (gpsDistance <= (INAV_GPS_GLITCH_RADIUS + 0.5f * INAV_GPS_GLITCH_ACCEL * dT * dT)) { + isGlitching = false; + } + else { + isGlitching = true; + } + } + + if (!isGlitching) { + previousTime = currentTimeUs; + lastKnownGoodPosition = posEstimator.gps.pos; + lastKnownGoodVelocity = posEstimator.gps.vel; + } + + return isGlitching; +} +#endif + /** * Update GPS topic * Function is called on each GPS update @@ -181,12 +231,12 @@ void onNewGPSData(void) newLLH.lon = gpsSol.llh.lon; newLLH.alt = gpsSol.llh.alt; - if (sensors(SENSOR_GPS) + if (sensors(SENSOR_GPS) #ifdef USE_GPS_FIX_ESTIMATION || STATE(GPS_ESTIMATED_FIX) #endif ) { - if (!(STATE(GPS_FIX) + if (!(STATE(GPS_FIX) #ifdef USE_GPS_FIX_ESTIMATION || STATE(GPS_ESTIMATED_FIX) #endif @@ -245,6 +295,19 @@ void onNewGPSData(void) posEstimator.gps.vel.z = (posEstimator.gps.vel.z + (gpsSol.llh.alt - previousAlt) / dT) / 2.0f; } +#if defined(NAV_GPS_GLITCH_DETECTION) + /* GPS glitch protection. We have local coordinates and local velocity for current GPS update. Check if they are sane */ + if (detectGPSGlitch(currentTimeUs)) { + posEstimator.gps.glitchRecovery = false; + posEstimator.gps.glitchDetected = true; + } + else { + /* Store previous glitch flag in glitchRecovery to indicate a valid reading after a glitch */ + posEstimator.gps.glitchRecovery = posEstimator.gps.glitchDetected; + posEstimator.gps.glitchDetected = false; + } +#endif + /* FIXME: use HDOP/VDOP */ if (gpsSol.flags.validEPE) { posEstimator.gps.eph = gpsSol.eph; @@ -492,7 +555,7 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) newFlags |= EST_GPS_XY_VALID; } } - + // !IS_RC_MODE_ACTIVE(BOXBEEPERON) CR97 to disable Baro, only use GPS alt if (sensors(SENSOR_BARO) && ((currentTimeUs - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS))) { newFlags |= EST_BARO_VALID; } @@ -544,6 +607,10 @@ static void estimationPredict(estimationContext_t * ctx) static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) { + // DEBUG_SET(DEBUG_ALWAYS, 2, posEstimator.est.pos.z); // Position estimate CR97 + // DEBUG_SET(DEBUG_ALWAYS, 3, posEstimator.baro.alt); // Baro altitude + // DEBUG_SET(DEBUG_ALWAYS, 4, posEstimator.gps.pos.z); // GPS altitude + DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z); // Position estimate DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt); // Baro altitude DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z); // GPS altitude @@ -554,7 +621,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count bool correctOK = false; - + //ignore baro if difference is too big, baro is probably wrong const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f; //fade out the baro to prevent sudden jump @@ -723,14 +790,11 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) estimationPredict(&ctx); /* Correction stage: Z */ - const bool estZCorrectOk = - estimationCalculateCorrection_Z(&ctx); + const bool estZCorrectOk = estimationCalculateCorrection_Z(&ctx); /* Correction stage: XY: GPS, FLOW */ // FIXME: Handle transition from FLOW to GPS and back - seamlessly fly indoor/outdoor - const bool estXYCorrectOk = - estimationCalculateCorrection_XY_GPS(&ctx) || - estimationCalculateCorrection_XY_FLOW(&ctx); + const bool estXYCorrectOk = estimationCalculateCorrection_XY_GPS(&ctx) || estimationCalculateCorrection_XY_FLOW(&ctx); // If we can't apply correction or accuracy is off the charts - decay velocity to zero if (!estXYCorrectOk || ctx.newEPH > positionEstimationConfig()->max_eph_epv) { @@ -832,6 +896,13 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs) } } +#if defined(NAV_GPS_GLITCH_DETECTION) +bool isGPSGlitchDetected(void) +{ + return posEstimator.gps.glitchDetected; +} +#endif + float getEstimatedAglPosition(void) { return posEstimator.est.aglAlt; } diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 44f0333d149..fb7dd9f159e 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -65,6 +65,10 @@ typedef struct { typedef struct { timeUs_t lastUpdateTime; // Last update time (us) +#if defined(NAV_GPS_GLITCH_DETECTION) + bool glitchDetected; + bool glitchRecovery; +#endif fpVector3_t pos; // GPS position in NEU coordinate system (cm) fpVector3_t vel; // GPS velocity (cms) float eph; @@ -128,7 +132,7 @@ typedef struct { } navPositionEstimatorESTIMATE_t; typedef struct { - timeUs_t lastUpdateTime; + timeUs_t lastUpdateTime; fpVector3_t accelNEU; fpVector3_t accelBias; float calibratedGravityCMSS; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index ca0cea8a3d1..dd97c42c9c0 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -58,7 +58,7 @@ typedef enum { } navSetWaypointFlags_t; typedef enum { - ROC_TO_ALT_RESET, + ROC_TO_ALT_CURRENT, // CR97 ROC_TO_ALT_CONSTANT, ROC_TO_ALT_TARGET } climbRateToAltitudeControllerMode_e; @@ -89,8 +89,11 @@ typedef struct navigationFlags_s { navigationEstimateStatus_e estVelStatus; // Indicates that GPS is working (or not) navigationEstimateStatus_e estAglStatus; navigationEstimateStatus_e estHeadingStatus; // Indicate valid heading - wither mag or GPS at certain speed on airplane + bool compassGpsCogMismatchError; // mismatch between compass heading and valid GPS heading // CR27 bool gpsCfEstimatedAltitudeMismatch; // Indicates a mismatch between GPS altitude and estimated altitude + climbRateToAltitudeControllerMode_e rocToAltMode; // CR97 + bool isAdjustingPosition; bool isAdjustingAltitude; bool isAdjustingHeading; @@ -136,6 +139,7 @@ typedef struct { fpVector3_t pos; fpVector3_t vel; int32_t yaw; + int16_t climbRateDemand; // CR97 } navigationDesiredState_t; typedef enum { @@ -285,7 +289,7 @@ typedef enum { NAV_STATE_CRUISE_INITIALIZE, NAV_STATE_CRUISE_IN_PROGRESS, NAV_STATE_CRUISE_ADJUSTING, - + NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, NAV_STATE_FW_LANDING_LOITER, NAV_STATE_FW_LANDING_APPROACH, @@ -424,7 +428,7 @@ typedef struct { uint32_t lastValidPositionTimeMs; uint32_t lastValidAltitudeTimeMs; - /* INAV GPS origin (position where GPS fix was first acquired) */ + /* INAV GPS origin (position where GPS fix first acquired) */ gpsOrigin_t gpsOrigin; /* Home/RTH parameters - NEU coordinates (geodetic position of home (LLH) is stored in GPS_home variable) */ @@ -459,7 +463,7 @@ typedef struct { #endif navWaypointPosition_t activeWaypoint; // Local position, current bearing and turn angle to next WP, filled on waypoint activation int8_t activeWaypointIndex; - float wpInitialAltitude; // Altitude at start of WP + float wpInitialAltitude; // Altitude at start of WP CR97 check for auto landing changes float wpInitialDistance; // Distance when starting flight to WP float wpDistance; // Distance to active WP timeMs_t wpReachedTime; // Time the waypoint was reached @@ -508,7 +512,7 @@ flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state); void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags); void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask); void setDesiredSurfaceOffset(float surfaceOffset); -void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask); // NOT USED +// void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask); // NOT USED void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode); bool isNavHoldPositionActive(void); @@ -519,6 +523,7 @@ bool isWaypointNavTrackingActive(void); void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse); void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY); void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError); +float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros); // CR97 bool checkForPositionSensorTimeout(void); @@ -537,10 +542,9 @@ bool adjustMulticopterHeadingFromRCInput(void); bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment); void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs); - bool isMulticopterLandingDetected(void); - void calculateMulticopterInitialHoldPosition(fpVector3_t * pos); +float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros); // CR97 /* Fixed-wing specific functions */ void setupFixedWingAltitudeController(void); @@ -563,6 +567,7 @@ void calculateFixedWingInitialHoldPosition(fpVector3_t * pos); /* Fixed-wing launch controller */ void resetFixedWingLaunchController(timeUs_t currentTimeUs); +// bool isFixedWingLaunchFinishedThrottleLow(void); // CR6 void enableFixedWingLaunchController(timeUs_t currentTimeUs); void abortFixedWingLaunch(void); void applyFixedWingLaunchController(timeUs_t currentTimeUs); diff --git a/src/main/navigation/sqrt_controller.c b/src/main/navigation/sqrt_controller.c index 569df14d70f..ef327f16ede 100644 --- a/src/main/navigation/sqrt_controller.c +++ b/src/main/navigation/sqrt_controller.c @@ -26,7 +26,7 @@ #include "navigation/sqrt_controller.h" /* - Square Root Controller calculates the correction based on a proportional controller (kP). + Square Root Controller calculates the correction based on a proportional controller (kP). Used only in AutoPilot System for Vertical (Z) control. */ @@ -99,15 +99,14 @@ float sqrtControllerApply(sqrt_controller_t *sqrt_controller_pointer, float targ if (deltaTime != 0.0f) { // This ensures we do not get small oscillations by over shooting the error correction in the last time step. return constrainf(correction_rate, -fabsf(sqrt_controller_pointer->error) / deltaTime, fabsf(sqrt_controller_pointer->error) / deltaTime); - } + } - return correction_rate; + return correction_rate; } // Sets the maximum error to limit output and derivative of output void sqrtControllerInit(sqrt_controller_t *sqrt_controller_pointer,const float kp,const float output_min,const float output_max,const float derivative_out_max) { - // Reset the variables sqrt_controller_pointer->kp = kp; sqrt_controller_pointer->derivative_max = 0.0f; diff --git a/src/main/navigation/sqrt_controller.h b/src/main/navigation/sqrt_controller.h index fa6f1d4f70c..ed2cdee869e 100644 --- a/src/main/navigation/sqrt_controller.h +++ b/src/main/navigation/sqrt_controller.h @@ -17,7 +17,7 @@ #pragma once -typedef struct sqrt_controller_s { +typedef struct sqrt_controller_s { float kp; // proportional gain float error; // error calced float error_min; // error limit in negative direction @@ -26,4 +26,4 @@ typedef struct sqrt_controller_s { } sqrt_controller_t; float sqrtControllerApply(sqrt_controller_t *sqrt_controller_pointer, float target, float measurement, float deltaTime); -void sqrtControllerInit(sqrt_controller_t *sqrt_controller_pointer, const float kp, const float output_min, const float output_max, const float derivative_out_max); \ No newline at end of file +void sqrtControllerInit(sqrt_controller_t *sqrt_controller_pointer, const float kp, const float output_min, const float output_max, const float derivative_out_max); diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index ad1cb21c6cf..aefcf3da53f 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -97,7 +97,7 @@ static int logicConditionCompute( int temporaryValue; #if defined(USE_VTX_CONTROL) vtxDeviceCapability_t vtxDeviceCapability; -#endif +#endif switch (operation) { @@ -154,7 +154,7 @@ static int logicConditionCompute( case LOGIC_CONDITION_NOR: return !(operandA || operandB); - break; + break; case LOGIC_CONDITION_NOT: return !operandA; @@ -170,7 +170,7 @@ static int logicConditionCompute( return false; } - //When both operands are not met, keep current value + //When both operands are not met, keep current value return currentValue; break; @@ -238,7 +238,7 @@ static int logicConditionCompute( gvSet(operandA, operandB); return operandB; break; - + case LOGIC_CONDITION_GVAR_INC: temporaryValue = gvGet(operandA) + operandB; gvSet(operandA, temporaryValue); @@ -270,7 +270,7 @@ static int logicConditionCompute( return operandA; } break; - + case LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY: LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY); return true; @@ -293,12 +293,12 @@ static int logicConditionCompute( ENABLE_STATE(CALIBRATE_MAG); return true; break; -#endif +#endif case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: -#if defined(USE_VTX_CONTROL) +#if defined(USE_VTX_CONTROL) #if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)) if ( - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA && + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability) ) { logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount); @@ -346,18 +346,18 @@ static int logicConditionCompute( LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH); return true; break; - + case LOGIC_CONDITION_INVERT_YAW: LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW); return true; break; - + case LOGIC_CONDITION_OVERRIDE_THROTTLE: logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE] = operandA; LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE); return operandA; break; - + case LOGIC_CONDITION_SET_OSD_LAYOUT: logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT] = operandA; LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT); @@ -373,18 +373,18 @@ static int logicConditionCompute( case LOGIC_CONDITION_SIN: temporaryValue = (operandB == 0) ? 500 : operandB; - return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; + return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; break; - + case LOGIC_CONDITION_COS: temporaryValue = (operandB == 0) ? 500 : operandB; - return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; + return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; break; break; - + case LOGIC_CONDITION_TAN: temporaryValue = (operandB == 0) ? 500 : operandB; - return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; + return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; break; case LOGIC_CONDITION_MIN: @@ -394,11 +394,11 @@ static int logicConditionCompute( case LOGIC_CONDITION_MAX: return (operandA > operandB) ? operandA : operandB; break; - + case LOGIC_CONDITION_MAP_INPUT: return scaleRange(constrain(operandA, 0, operandB), 0, operandB, 0, 1000); break; - + case LOGIC_CONDITION_MAP_OUTPUT: return scaleRange(constrain(operandA, 0, 1000), 0, 1000, 0, operandB); break; @@ -483,8 +483,9 @@ static int logicConditionCompute( } break; -#ifdef LED_PIN +#ifdef USE_LED_STRIP case LOGIC_CONDITION_LED_PIN_PWM: + if (operandA >=0 && operandA <= 100) { ledPinStartPWM((uint8_t)operandA); } else { @@ -506,7 +507,7 @@ static int logicConditionCompute( default: return false; - break; + break; } } @@ -515,7 +516,7 @@ void logicConditionProcess(uint8_t i) { const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId); if (logicConditions(i)->enabled && activatorValue && !cliMode) { - + /* * Process condition only when latch flag is not set * Latched LCs can only go from OFF to ON, not the other way @@ -524,13 +525,13 @@ void logicConditionProcess(uint8_t i) { const int operandAValue = logicConditionGetOperandValue(logicConditions(i)->operandA.type, logicConditions(i)->operandA.value); const int operandBValue = logicConditionGetOperandValue(logicConditions(i)->operandB.type, logicConditions(i)->operandB.value); const int newValue = logicConditionCompute( - logicConditionStates[i].value, - logicConditions(i)->operation, - operandAValue, + logicConditionStates[i].value, + logicConditions(i)->operation, + operandAValue, operandBValue, i ); - + logicConditionStates[i].value = newValue; /* @@ -605,7 +606,7 @@ static int logicConditionGetWaypointOperandValue(int operand) { return distance; } break; - + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION: return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0; break; @@ -651,11 +652,11 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s return constrain((uint32_t)getFlightTime(), 0, INT16_MAX); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m return constrain(GPS_distanceToHome, 0, INT16_MAX); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m return constrain(getTotalTravelDistance() / 100, 0, INT16_MAX); break; @@ -663,7 +664,7 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI: return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100 return getBatteryVoltage(); break; @@ -679,7 +680,7 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100 return getAmperage(); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN: // mAh return getMAhDrawn(); break; @@ -696,7 +697,7 @@ static int logicConditionGetFlightOperandValue(int operand) { return gpsSol.numSat; } break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1 return STATE(GPS_FIX) ? 1 : 0; break; @@ -748,15 +749,15 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) ? 1 : 0; - break; - + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_ALT) ? 1 : 0; - break; + break; case LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_POS) ? 1 : 0; - break; + break; case LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_EMERG) ? 1 : 0; @@ -764,30 +765,30 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH: // 0/1 return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0; - break; + break; case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1 #ifdef USE_FW_AUTOLAND - return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || isFwLandInProgess()) ? 1 : 0; + return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || FLIGHT_MODE(NAV_FW_AUTOLAND)) ? 1 : 0; #else return ((navGetCurrentStateFlags() & NAV_CTL_LAND)) ? 1 : 0; #endif - + break; case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1 return (failsafePhase() != FAILSAFE_IDLE) ? 1 : 0; break; - - case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: // + + case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: // return axisPID[YAW]; break; - - case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: // + + case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: // return axisPID[ROLL]; break; - - case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: // + + case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: // return axisPID[PITCH]; break; @@ -814,7 +815,7 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int return getConfigProfile() + 1; break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int return currentMixerProfileIndex + 1; break; @@ -829,11 +830,11 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS: return isEstimatedAglTrusted(); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_AGL: return getEstimatedAglPosition(); - break; - + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW: return rangefinderGetLatestRawAltitude(); break; @@ -941,7 +942,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) { //Extract RC channel raw value if (operand >= 1 && operand <= MAX_SUPPORTED_RC_CHANNEL_COUNT) { retVal = rxGetChannelValue(operand - 1); - } + } break; case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT: @@ -969,7 +970,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) { retVal = programmingPidGetOutput(operand); } break; - + case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS: retVal = logicConditionGetWaypointOperandValue(operand); break; @@ -983,7 +984,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) { /* * conditionId == -1 is always evaluated as true - */ + */ int logicConditionGetValue(int8_t conditionId) { if (conditionId >= 0) { return logicConditionStates[conditionId].value; @@ -1065,7 +1066,7 @@ int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) { uint32_t getLoiterRadius(uint32_t loiterRadius) { #ifdef USE_PROGRAMMING_FRAMEWORK - if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) && + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) && !(FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding())) { return constrain(logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE], loiterRadius, 100000); } else { diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 3745ed7a8ea..fd29ea15fcb 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -284,7 +284,7 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected) } break; #endif - + #if defined(USE_FAKE_BATT_SENSOR) case VOLTAGE_SENSOR_FAKE: vbat = fakeBattSensorGetVBat(); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 02a9df69c44..468545a4064 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -373,9 +373,9 @@ void compassUpdate(timeUs_t currentTimeUs) // Check magZero if ( compassConfig()->magZero.raw[X] == 0 && compassConfig()->magZero.raw[Y] == 0 && compassConfig()->magZero.raw[Z] == 0 && - compassConfig()->magGain[X] == 1024 && compassConfig()->magGain[Y] == 1024 && compassConfig()->magGain[Z] == 1024 + compassConfig()->magGain[X] == 1024 && compassConfig()->magGain[Y] == 1024 && compassConfig()->magGain[Z] == 1024 ) { - DISABLE_STATE(COMPASS_CALIBRATED); + DISABLE_STATE(COMPASS_CALIBRATED); // CR27 disable if mismatch with GPS heading in compassHeadingGPSCheck() } else { ENABLE_STATE(COMPASS_CALIBRATED); diff --git a/src/main/target/FLYWOOF411/target.h b/src/main/target/FLYWOOF411/target.h index ed263d5ee3d..b700c898d76 100644 --- a/src/main/target/FLYWOOF411/target.h +++ b/src/main/target/FLYWOOF411/target.h @@ -100,7 +100,7 @@ #ifdef FLYWOOF411_V2 #define UART2_TX_PIN PA2 #else -#define UART2_TX_PIN NONE //PA2 +#define UART2_TX_PIN PA2 //PA2 Changed from NONE AWH #endif #define UART2_RX_PIN PA3 @@ -109,8 +109,8 @@ #define SOFTSERIAL_1_TX_PIN PB6 // Clash with TX2, possible to use as S.Port or VTX control #define SOFTSERIAL_1_RX_PIN PB7 #else -#define SOFTSERIAL_1_TX_PIN PA2 // Clash with TX2, possible to use as S.Port or VTX control -#define SOFTSERIAL_1_RX_PIN PA2 +#define SOFTSERIAL_1_TX_PIN PA15 // LED Strip repurposed AWH +#define SOFTSERIAL_1_RX_PIN NONE #endif #define SERIAL_PORT_COUNT 4 // VCP, USART1, USART2, SS1 @@ -143,13 +143,15 @@ #ifdef FLYWOOF411_V2 #define WS2811_PIN PA0 #else -#define WS2811_PIN PA15 +#define WS2811_PIN NONE // PA15 repurosed as Softserial AWH #endif + // *************** OTHERS ************************* #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL) -#define USE_DSHOT -#define USE_ESC_SENSOR +// #define USE_DSHOT removed to prevent Flash1 overflow with Custom build CR46 +// #define USE_ESC_SENSOR removed to prevent Flash1 overflow with Custom build CR46 +#define USE_SERIALSHOT #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/common.h b/src/main/target/common.h index 8bbaadef718..189dd13b364 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -42,7 +42,7 @@ #define USE_SERIALRX_FPORT #define USE_SERIALRX_FPORT2 -//#define USE_DEV_TOOLS // tools for dev use only. Undefine for release builds. +#define USE_DEV_TOOLS // functions for dev use only. Undefine for release builds. #define COMMON_DEFAULT_FEATURES (FEATURE_TX_PROF_SEL) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index af9f278659f..9562cbe3690 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -214,7 +214,11 @@ static void crsfFrameGps(sbuf_t *dst) crsfSerialize32(dst, gpsSol.llh.lon); crsfSerialize16(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // gpsSol.groundSpeed is in cm/s crsfSerialize16(dst, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse)); // gpsSol.groundCourse is 0.1 degrees, need 0.01 deg - const uint16_t altitude = (getEstimatedActualPosition(Z) / 100) + 1000; + // CR63 + // const uint16_t altitude = (getEstimatedActualPosition(Z) / 100) + 1000; + // const uint16_t altitude = 1000 + (STATE(GPS_FIX) ? gpsSol.llh.alt / 100 : 0); + const uint16_t altitude = 1000 + gpsSol.llh.alt / 100; + // CR63 crsfSerialize16(dst, altitude); crsfSerialize8(dst, gpsSol.numSat); } @@ -370,13 +374,17 @@ static void crsfFrameFlightMode(sbuf_t *dst) flightMode = "HOR"; } else if (FLIGHT_MODE(ANGLEHOLD_MODE)) { flightMode = "ANGH"; +#ifdef USE_FW_AUTOLAND + } else if (FLIGHT_MODE(NAV_FW_AUTOLAND)) { + flightMode = "LAND"; +#endif } #ifdef USE_GPS } else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) { flightMode = "WAIT"; // Waiting for GPS lock #endif } else if (isArmingDisabled()) { - flightMode = "!ERR"; + flightMode = "DARM"; // CR63 } crsfSerializeData(dst, (const uint8_t*)flightMode, strlen(flightMode)); @@ -622,7 +630,7 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType) crsfFrameBaroVarioSensor(sbuf); break; } - + const int frameSize = crsfFinalizeBuf(sbuf, frame); return frameSize; } diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 84dccddb869..b0b9d0c8d85 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -182,6 +182,10 @@ void ltm_sframe(sbuf_t *dst) lt_flightmode = LTM_MODE_ANGLE; else if (FLIGHT_MODE(HORIZON_MODE)) lt_flightmode = LTM_MODE_HORIZON; +#ifdef USE_FW_AUTOLAND + else if (FLIGHT_MODE(NAV_FW_AUTOLAND)) + lt_flightmode = LTM_MODE_LAND; +#endif else lt_flightmode = LTM_MODE_RATE; // Rate mode