diff --git a/docs/Profiles.md b/docs/Profiles.md index 80d432d7d4..aac3438aad 100644 --- a/docs/Profiles.md +++ b/docs/Profiles.md @@ -5,7 +5,7 @@ A profile is a set of configuration settings. Currently three profiles are supported. The default profile is profile `1`. ## Changing profiles - +### Stick Commands Profiles can be selected using a GUI or the following stick combinations: | Profile | Throttle | Yaw | Pitch | Roll | @@ -14,12 +14,29 @@ Profiles can be selected using a GUI or the following stick combinations: | 2 | Down | Left | Up | Middle | | 3 | Down | Left | Middle | Right | -The CLI `profile` command can also be used: +### CLI +The CLI `profile` command can also be used to change profiles: ``` profile ``` +### Programming (4.0.0 onwards) +You can change profiles using the programming frame work. This allows a lot of flexability in how you change profiles. + +For example, using a simple switch on channel 15. + +[![For example, using a simple switch](https://i.imgur.com/SS9CaaOl.png)](https://i.imgur.com/SS9CaaO.png) + +Or using the speed to change profiles. In this example: +- when lower than 25 cm/s (basically not flying), profiles are not effected. +- Below 2682 cm/s (60 mph | 97 Km/h) use Profile 1 +- Above 5364 cm/s (120 mph | 193 Km/h) use Profile 3 +- Between 2683 and 5364 cm/s, use Profile 2 + +[![Using speed to change profiles](https://i.imgur.com/WjkuhhWl.png)](https://i.imgur.com/WjkuhhW.png) + +## Profile Contents The values contained within a profile can be seen by using the CLI `dump profile` command. e.g diff --git a/docs/Settings.md b/docs/Settings.md index d0b26b0a68..c4d004816d 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1612,26 +1612,6 @@ Defines the type of the main gyro LPF filter. Possible values: `PT1`, `BIQUAD`. --- -### gyro_notch_cutoff - -_// TODO_ - -| Default | Min | Max | -| --- | --- | --- | -| 1 | 1 | 500 | - ---- - -### gyro_notch_hz - -_// TODO_ - -| Default | Min | Max | -| --- | --- | --- | -| 0 | | 500 | - ---- - ### gyro_to_use _// TODO_ diff --git a/docs/boards/Omnibus F4.md b/docs/boards/Omnibus F4.md index 67e9a23c59..018a0d17f8 100644 --- a/docs/boards/Omnibus F4.md +++ b/docs/boards/Omnibus F4.md @@ -166,9 +166,19 @@ Please note that this is *not* the motor PWM5/PWM6 pins, but small surface mount ### Omnibus F4 v3/v4/v5 SoftwareSerial Connections -The SOFTSERIAL1 is an uni-directional port mapped to UART6-TX pin. +Softserial mappings can be selected by choosing between different build targets. +Some of these can be used for Smartport, FPort, or other inverted protocols. + +OMNIBUSF4V3: Softserial1 on UART6-TX pin. +OMNIBUSF4V3_S6_SS: Softserial1 RX or TX on S6 (motor 6) +OMNIBUSF4V3_S5_S6_2SS: Softserial1 on S5 RX or TX (motor 5) and Softserial2 on S6 (motor 6) +OMNIBUSF4V3_S5S6_SS: Softserial1 on S5/RX and S6/TX + +With the OMNIBUSF4V3 target, SOFTSERIAL1 is an uni-directional port mapped to UART6-TX pin. When enabled, the UART6 is still available as hardware port but it's then RX-only port (good for e.g. receiving S.BUS input). TX instead is controlled in software and can be used for transmitting one-way telemetry (e.g. LTM). Please note that UART6-TX line passes programmable inverter on those boards, so it is a pure output-only port. SmartPort/FPort telemetry requires bi-directional communication, so it is not possible on this port without hardware modification (bypassing the inverter). +SmartPort / FPort is possible without a hardware inverter by using one of the OMNIBUSF4V3___SS builds and connecting SmartPort to the motor 5 or 6 pad. + ## Where to buy: * [Omnibus F4 v5](https://inavflight.com/shop/p/OMNIBUSF4V5) diff --git a/docs/development/wp_mission_schema/README.md b/docs/development/wp_mission_schema/README.md new file mode 100644 index 0000000000..850c6fe055 --- /dev/null +++ b/docs/development/wp_mission_schema/README.md @@ -0,0 +1,59 @@ +# XML Mission File Definition + +## Overview + +Historically, mission planners interoperating with inav (and multiwii) missions have used the XML mission file format defined by EOSBandi for MultiWii 2.3 (2013). + +The format is defined the XSD schema here. + +* Lower case tags are preferred by inav. Older tools may prefer uppercase (the original MW usage). +* For inav 4.0 and later, the `missionitem/flags` attribute is required for "fly-by home" waypoints and multi-mission files. +* For inav 4.0 and later, multi-mission files; mission segments are delimited by a flag value of `165` (the MSP protocol value). +* For multi-mission files, the waypoints may be numbered either sequentially across the whole file, or "reset-numbering" within each mission segment. The latter may (or not) be considered to be more "human readable". +* The `mwp` tag was introduced by the eponymous mission planner. Other mission planners may consider that reusing some of the tags (`cx`, `cy` - centre location, `zoom` TMS zoom level) is useful. +* The `version` tag may be intepreted by mission planners as they see fit. For example, the (obsolete) Android 'ez-gui' application requires '2.3-pre8'. For multi-mission files it is recommended to use another `version`. + +## Examples + +### Multi-mission file with sequential numbering + +``` + + + + + + + + + + + + + + + +``` + +### Multi-mission file with "reset" numbering and per-segment metadata + +``` + + + + +
+ + + +
+ + + + + + +
+ + +``` diff --git a/docs/development/wp_mission_schema/mw-mission.xsd b/docs/development/wp_mission_schema/mw-mission.xsd new file mode 100644 index 0000000000..667cc1027f --- /dev/null +++ b/docs/development/wp_mission_schema/mw-mission.xsd @@ -0,0 +1,132 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 68f22eadef..3054f0a441 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1812,10 +1812,6 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz); #endif - BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_notch_hz, - 0); - BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_notch_cutoff, - 1); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", accelerometerConfig()->acc_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware); #ifdef USE_BARO diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index e420d1cc95..f3afc32270 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -481,7 +481,12 @@ static void dumpPgValue(const setting_t *value, uint8_t dumpMask) settingGetName(value, name); if (dumpMask & SHOW_DEFAULTS && !equalsDefault) { cliPrintf(defaultFormat, name); - printValuePointer(value, defaultValuePointer, 0); + // if the craftname has a leading space, then enclose the name in quotes + if (strcmp(name, "name") == 0 && ((const char *)valuePointer)[0] == ' ') { + cliPrintf("\"%s\"", (const char *)valuePointer); + } else { + printValuePointer(value, valuePointer, 0); + } cliPrintLinefeed(); } cliPrintf(format, name); @@ -3109,7 +3114,13 @@ static void cliGet(char *cmdline) val = settingGet(i); if (settingNameContains(val, name, cmdline)) { cliPrintf("%s = ", name); - cliPrintVar(val, 0); + if (strcmp(name, "name") == 0) { + // if the craftname has a leading space, then enclose the name in quotes + const char * v = (const char *)settingGetValuePointer(val); + cliPrintf(v[0] == ' ' ? "\"%s\"" : "%s", v); + } else { + cliPrintVar(val, 0); + } cliPrintLinefeed(); cliPrintVarRange(val); cliPrintLinefeed(); @@ -3167,7 +3178,12 @@ static void cliSet(char *cmdline) if (settingNameIsExactMatch(val, name, cmdline, variableNameLength)) { const setting_type_e type = SETTING_TYPE(val); if (type == VAR_STRING) { - settingSetString(val, eqptr, strlen(eqptr)); + // if setting the craftname, remove any quotes around the name. This allows leading spaces in the name + if (strcmp(name, "name") == 0 && eqptr[0] == '"' && eqptr[strlen(eqptr)-1] == '"') { + settingSetString(val, eqptr + 1, strlen(eqptr)-2); + } else { + settingSetString(val, eqptr, strlen(eqptr)); + } return; } const setting_mode_e mode = SETTING_MODE(val); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index e4610cddf7..02c8f1c8e6 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -183,9 +183,6 @@ uint32_t getGyroLooptime(void) { void validateAndFixConfig(void) { - if (gyroConfig()->gyro_notch_cutoff >= gyroConfig()->gyro_notch_hz) { - gyroConfigMutable()->gyro_notch_hz = 0; - } if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) { accelerometerConfigMutable()->acc_notch_hz = 0; } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index bd9d793fb8..c65b5734d5 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1238,8 +1238,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz); sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); - sbufWriteU16(dst, gyroConfig()->gyro_notch_hz); - sbufWriteU16(dst, gyroConfig()->gyro_notch_cutoff); + sbufWriteU16(dst, 0); //Was gyroConfig()->gyro_notch_hz + sbufWriteU16(dst, 1); //Was gyroConfig()->gyro_notch_cutoff sbufWriteU16(dst, 0); //BF: pidProfile()->dterm_notch_hz sbufWriteU16(dst, 1); //pidProfile()->dterm_notch_cutoff @@ -2176,8 +2176,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 500); pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255); if (dataSize >= 9) { - gyroConfigMutable()->gyro_notch_hz = constrain(sbufReadU16(src), 0, 500); - gyroConfigMutable()->gyro_notch_cutoff = constrain(sbufReadU16(src), 1, 500); + sbufReadU16(src); //Was gyroConfigMutable()->gyro_notch_hz + sbufReadU16(src); //Was gyroConfigMutable()->gyro_notch_cutoff } else { return MSP_RESULT_ERROR; } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 3066d2a9c2..2af03754f5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -235,15 +235,6 @@ groups: default_value: 32 field: gyroMovementCalibrationThreshold max: 128 - - name: gyro_notch_hz - field: gyro_notch_hz - max: 500 - default_value: 0 - - name: gyro_notch_cutoff - field: gyro_notch_cutoff - min: 1 - max: 500 - default_value: 1 - name: gyro_main_lpf_hz description: "Software based gyro main lowpass filter. Value is cutoff frequency (Hz)" default_value: 60 diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 2b2f348638..e0d29beb0a 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -61,12 +61,7 @@ FILE_COMPILE_FOR_SPEED #include "sensors/sensors.h" -/** - * In Cleanflight accelerometer is aligned in the following way: - * X-axis = Forward - * Y-axis = Left - * Z-axis = Up - * Our INAV uses different convention +/* * X-axis = North/Forward * Y-axis = East/Right * Z-axis = Up diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 840d5495a9..dd317cdaea 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -1428,8 +1428,8 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); // BF: currentPidProfile->dterm_lowpass_hz sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); // BF: currentPidProfile->yaw_lowpass_hz - sbufWriteU16(dst, gyroConfig()->gyro_notch_hz); // BF: gyroConfig()->gyro_soft_notch_hz_1 - sbufWriteU16(dst, gyroConfig()->gyro_notch_cutoff); // BF: gyroConfig()->gyro_soft_notch_cutoff_1 + sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_1 + sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_1 sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_notch_hz sbufWriteU16(dst, 1); // BF: currentPidProfile->dterm_notch_cutoff sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_2 diff --git a/src/main/msc/emfat.c b/src/main/msc/emfat.c index bcf87ee5fe..44f7ee7d12 100644 --- a/src/main/msc/emfat.c +++ b/src/main/msc/emfat.c @@ -388,8 +388,8 @@ void read_boot_sector(const emfat_t *emfat, uint8_t *sect) bs->volume_id[1] = 14; bs->volume_id[2] = 13; bs->volume_id[3] = 8; - memcpy(bs->volume_label, "NO NAME ", 12); - memcpy(bs->file_system_type, "FAT32 ", 8); + memcpy(bs->volume_label, "NO NAME ", VOL_LABEL_LEN); + memcpy(bs->file_system_type, "FAT32 ", FILE_SYS_TYPE_LENGTH); sect[SECT - 2] = 0x55; sect[SECT - 1] = 0xAA; } @@ -525,8 +525,9 @@ void fill_entry(dir_entry *entry, const char *name, uint8_t attr, uint32_t clust l2 = l2 > FILE_NAME_EXTN_LEN ? FILE_NAME_EXTN_LEN : l2; } - memset(entry->name, ' ', FILE_NAME_SHRT_LEN + FILE_NAME_EXTN_LEN); + memset(entry->name, ' ', FILE_NAME_SHRT_LEN); memcpy(entry->name, name, l1); + memset(entry->extn, ' ', FILE_NAME_EXTN_LEN); memcpy(entry->extn, name + dot_pos + 1, l2); for (i = 0; i < FILE_NAME_SHRT_LEN; i++) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 3c9fa88adf..224b344116 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -782,7 +782,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_WP_ENROUTE, .mwError = MW_NAV_ERROR_FINISH, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_FINISHED, // re-process state [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, @@ -1011,7 +1011,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now DEBUG_SET(DEBUG_CRUISE, 0, 1); - if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Try the next iteration. + if (checkForPositionSensorTimeout()) { + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } // Switch to IDLE if we do not have an healty position. Try the next iteration. if (!(prevFlags & NAV_CTL_POS)) { resetPositionController(); @@ -1028,7 +1030,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS { const timeMs_t currentTimeMs = millis(); - if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration. + if (checkForPositionSensorTimeout()) { + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration. DEBUG_SET(DEBUG_CRUISE, 0, 2); DEBUG_SET(DEBUG_CRUISE, 2, 0); @@ -1114,7 +1118,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) { // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing - // Relevant to failsafe forced RTH only. Switched RTH blocked in selectNavEventFromBoxModeInput if sensors unavailanle. + // Relevant to failsafe forced RTH only. Switched RTH blocked in selectNavEventFromBoxModeInput if sensors unavailable. // If we are in dead-reckoning mode - also fail, since coordinates may be unreliable return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1175,90 +1179,82 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } /* No valid POS sensor but still within valid timeout - wait */ - else { - return NAV_FSM_EVENT_NONE; - } + return NAV_FSM_EVENT_NONE; } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState) { UNUSED(previousState); - rthAltControlStickOverrideCheck(PITCH); - - if ((posControl.flags.estHeadingStatus == EST_NONE)) { - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } - if (!STATE(ALTITUDE_CONTROL)) { //If altitude control is not a thing, switch to RTH in progress instead return NAV_FSM_EVENT_SUCCESS; //Will cause NAV_STATE_RTH_HEAD_HOME } - // If we have valid pos sensor OR we are configured to ignore GPS loss - if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) { - const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT; - const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); + rthAltControlStickOverrideCheck(PITCH); - // If we reached desired initial RTH altitude or we don't want to climb first - if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF) || rthAltControlStickOverrideCheck(ROLL)) { - - // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance - if (STATE(FIXED_WING_LEGACY)) { - initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); - } - - // Save initial home distance for future use - posControl.rthState.rthInitialDistance = posControl.homeDistance; - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL); - - if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) { - setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); - } - else { - setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); - } - - return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME - - } else { - - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL); - - /* For multi-rotors execute sanity check during initial ascent as well */ - if (!STATE(FIXED_WING_LEGACY) && !validateRTHSanityChecker()) { - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } - - // Climb to safe altitude and turn to correct direction - if (STATE(FIXED_WING_LEGACY)) { - if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) { - float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; - updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); - } else { - tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM; - setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); - } - } else { - // Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach - // it in a reasonable time. Immediately after we finish this phase - target the original altitude. - tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM; - - if (navConfig()->general.flags.rth_tail_first) { - setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); - } else { - setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); - } - } - - return NAV_FSM_EVENT_NONE; - - } - } - /* Position sensor failure timeout - land */ - else { + /* Position sensor failure timeout and not configured to ignore GPS loss - land */ + if ((posControl.flags.estHeadingStatus == EST_NONE) || + (checkForPositionSensorTimeout() && !navConfig()->general.flags.rth_climb_ignore_emerg)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } + + const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT; + const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); + + // If we reached desired initial RTH altitude or we don't want to climb first + if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF) || rthAltControlStickOverrideCheck(ROLL)) { + + // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance + if (STATE(FIXED_WING_LEGACY)) { + initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); + } + + // Save initial home distance for future use + posControl.rthState.rthInitialDistance = posControl.homeDistance; + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL); + + if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) { + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); + } + else { + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + } + + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME + + } else { + + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL); + + /* For multi-rotors execute sanity check during initial ascent as well */ + if (!STATE(FIXED_WING_LEGACY) && !validateRTHSanityChecker()) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } + + // Climb to safe altitude and turn to correct direction + if (STATE(FIXED_WING_LEGACY)) { + if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) { + float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; + updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); + } else { + tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM; + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); + } + } else { + // Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach + // it in a reasonable time. Immediately after we finish this phase - target the original altitude. + tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM; + + if (navConfig()->general.flags.rth_tail_first) { + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); + } else { + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + } + } + + return NAV_FSM_EVENT_NONE; + } } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState) @@ -1267,7 +1263,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio rthAltControlStickOverrideCheck(PITCH); - if ((posControl.flags.estHeadingStatus == EST_NONE)) { + /* If position sensors unavailable - land immediately */ + if ((posControl.flags.estHeadingStatus == EST_NONE) || !validateRTHSanityChecker()) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1279,12 +1276,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio // Successfully reached position target - update XYZ-position setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING - } - else if (!validateRTHSanityChecker()) { - // Sanity check of RTH - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } - else { + } else { setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY); return NAV_FSM_EVENT_NONE; } @@ -1294,52 +1286,44 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } /* No valid POS sensor but still within valid timeout - wait */ - else { - return NAV_FSM_EVENT_NONE; - } + return NAV_FSM_EVENT_NONE; } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState) { UNUSED(previousState); - if ((posControl.flags.estHeadingStatus == EST_NONE)) { - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } - //On ROVER and BOAT we immediately switch to the next event if (!STATE(ALTITUDE_CONTROL)) { return NAV_FSM_EVENT_SUCCESS; } - // If position ok OR within valid timeout - continue - if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) { - // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing - if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) { - resetLandingDetector(); - updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); - return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land - } - else if (!validateRTHSanityChecker()) { - // Continue to check for RTH sanity during pre-landing hover - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } - else { - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); - setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - return NAV_FSM_EVENT_NONE; - } - } else { + /* If position sensors unavailable - land immediately (wait for timeout on GPS) */ + if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } + + // If position ok OR within valid timeout - continue + // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing + if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) { + resetLandingDetector(); + updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); + 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); + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return NAV_FSM_EVENT_NONE; + } } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState) { UNUSED(previousState); - if (posControl.flags.estHeadingStatus == EST_NONE || !(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout())) + /* If position sensors unavailable - land immediately (wait for timeout on GPS) */ + if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER); @@ -1370,42 +1354,35 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF return NAV_FSM_EVENT_SUCCESS; } - if (!ARMING_FLAG(ARMED)) { + if (!ARMING_FLAG(ARMED) || isLandingDetected()) { return NAV_FSM_EVENT_SUCCESS; } - else if (isLandingDetected()) { - return NAV_FSM_EVENT_SUCCESS; + + /* 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()) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - else { - /* 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()) { - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } - float descentVelLimited = 0; + float descentVelLimited = 0; - // A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed - if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) { - // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown - // Do not allow descent velocity slower than -30cm/s so the landing detector works. - descentVelLimited = navConfig()->general.land_minalt_vspd; - } - else { + // A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed + if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) { + // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown + // Do not allow descent velocity slower than -30cm/s so the landing detector works. + descentVelLimited = navConfig()->general.land_minalt_vspd; + } else { + // Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm. + float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z, + navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt, + navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); - // Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm. - float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z, - navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt, - navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); - - descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); - - } - - updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL); - - return NAV_FSM_EVENT_NONE; + descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); } + + updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL); + + return NAV_FSM_EVENT_NONE; } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState) @@ -1588,11 +1565,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na else if (checkForPositionSensorTimeout() || (posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - else { - return NAV_FSM_EVENT_NONE; // will re-process state in >10ms - } - UNREACHABLE(); + return NAV_FSM_EVENT_NONE; // will re-process state in >10ms } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState) @@ -1632,11 +1606,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi timeMs_t currentTime = millis(); - if(posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0) + if (posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0 || + (posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)) { return NAV_FSM_EVENT_SUCCESS; + } - if(posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L) - return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_NONE; // will re-process state in >10ms } @@ -1681,17 +1655,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig clearJumpCounters(); posControl.wpMissionRestart = true; - // If no position sensor available - land immediately - if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) { - return NAV_FSM_EVENT_NONE; - } - /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ - else if (checkForPositionSensorTimeout()) { + /* 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; } - else { - return NAV_FSM_EVENT_NONE; // will re-process state in >10ms - } + + return NAV_FSM_EVENT_NONE; // will re-process state in >10ms } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState) @@ -1959,21 +1928,21 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali if (estPosValid && estVelValid) { posControl.flags.estPosStatus = EST_TRUSTED; posControl.flags.estVelStatus = EST_TRUSTED; - posControl.flags.horizontalPositionDataNew = 1; + posControl.flags.horizontalPositionDataNew = true; posControl.lastValidPositionTimeMs = millis(); } // CASE 1: POS invalid, VEL valid else if (!estPosValid && estVelValid) { posControl.flags.estPosStatus = EST_USABLE; // Pos usable, but not trusted posControl.flags.estVelStatus = EST_TRUSTED; - posControl.flags.horizontalPositionDataNew = 1; + posControl.flags.horizontalPositionDataNew = true; posControl.lastValidPositionTimeMs = millis(); } // CASE 3: can't use pos/vel data else { posControl.flags.estPosStatus = EST_NONE; posControl.flags.estVelStatus = EST_NONE; - posControl.flags.horizontalPositionDataNew = 0; + posControl.flags.horizontalPositionDataNew = false; } //Update blackbox data @@ -2007,13 +1976,13 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo posControl.flags.estAglStatus = surfaceStatus; // Could be TRUSTED or USABLE posControl.flags.estAltStatus = EST_TRUSTED; - posControl.flags.verticalPositionDataNew = 1; + posControl.flags.verticalPositionDataNew = true; posControl.lastValidAltitudeTimeMs = millis(); } else { posControl.flags.estAltStatus = EST_NONE; posControl.flags.estAglStatus = EST_NONE; - posControl.flags.verticalPositionDataNew = 0; + posControl.flags.verticalPositionDataNew = false; } if (ARMING_FLAG(ARMED)) { @@ -2068,8 +2037,6 @@ void updateActualHeading(bool headingValid, int32_t newHeading) /* Precompute sin/cos of yaw angle */ posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(newHeading)); posControl.actualState.cosYaw = cos_approx(CENTIDEGREES_TO_RADIANS(newHeading)); - - posControl.flags.headingDataNew = 1; } /*----------------------------------------------------------- @@ -3133,10 +3100,10 @@ void applyWaypointNavigationAndAltitudeHold(void) /* Consume position data */ if (posControl.flags.horizontalPositionDataConsumed) - posControl.flags.horizontalPositionDataNew = 0; + posControl.flags.horizontalPositionDataNew = false; if (posControl.flags.verticalPositionDataConsumed) - posControl.flags.verticalPositionDataNew = 0; + posControl.flags.verticalPositionDataNew = false; //Update blackbox data if (posControl.flags.isAdjustingPosition) navFlags |= (1 << 6); @@ -3260,10 +3227,10 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) // Prevent MANUAL falling back to RTH if selected during active mission (canActivateWaypoint is set false on MANUAL selection) if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !IS_RC_MODE_ACTIVE(BOXMANUAL))) { // Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss - // If don't keep this, loss of any of the canActivatePosHold && canActivateNavigation && canActivateAltHold + // If don't keep this, loss of any of the canActivateNavigation && canActivateAltHold // will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back // logic to kick in (waiting for GPS on airplanes, switch to emergency landing etc) - if (isExecutingRTH || (canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { + if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { return NAV_FSM_EVENT_SWITCH_TO_RTH; } } @@ -3276,7 +3243,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) // Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded if (IS_RC_MODE_ACTIVE(BOXNAVWP)) { - if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) + if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT; } else { @@ -3607,9 +3574,8 @@ void navigationInit(void) /* Initial state */ posControl.navState = NAV_STATE_IDLE; - posControl.flags.horizontalPositionDataNew = 0; - posControl.flags.verticalPositionDataNew = 0; - posControl.flags.headingDataNew = 0; + posControl.flags.horizontalPositionDataNew = false; + posControl.flags.verticalPositionDataNew = false; posControl.flags.estAltStatus = EST_NONE; posControl.flags.estPosStatus = EST_NONE; diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index ce4d7504da..775b4c971f 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -57,7 +57,7 @@ #define UNUSED(x) ((void)(x)) #define FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE "RAISE THE THROTTLE" #define FW_LAUNCH_MESSAGE_TEXT_WAIT_IDLE "WAITING FOR IDLE" -#define FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION "READY" +#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" diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 9a36a59c7e..d6d343c055 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -71,7 +71,6 @@ typedef enum { typedef struct navigationFlags_s { bool horizontalPositionDataNew; bool verticalPositionDataNew; - bool headingDataNew; bool horizontalPositionDataConsumed; bool verticalPositionDataConsumed; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 4ea232a977..8b14932ce2 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -93,9 +93,6 @@ STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT]; STATIC_FASTRAM filterApplyFnPtr gyroLpf2ApplyFn; STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT]; -STATIC_FASTRAM filterApplyFnPtr notchFilter1ApplyFn; -STATIC_FASTRAM void *notchFilter1[XYZ_AXIS_COUNT]; - #ifdef USE_DYNAMIC_FILTERS EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState; @@ -103,7 +100,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, @@ -115,8 +112,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, #ifdef USE_DUAL_GYRO .gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT, #endif - .gyro_notch_hz = SETTING_GYRO_NOTCH_HZ_DEFAULT, - .gyro_notch_cutoff = SETTING_GYRO_NOTCH_CUTOFF_DEFAULT, .gyro_main_lpf_hz = SETTING_GYRO_MAIN_LPF_HZ_DEFAULT, .gyro_main_lpf_type = SETTING_GYRO_MAIN_LPF_TYPE_DEFAULT, .useDynamicLpf = SETTING_GYRO_USE_DYN_LPF_DEFAULT, @@ -282,24 +277,12 @@ static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t static void gyroInitFilters(void) { - STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT]; - notchFilter1ApplyFn = nullFilterApply; - //First gyro LPF running at full gyro frequency 8kHz initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_type, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime()); //Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_type, gyroConfig()->gyro_main_lpf_hz, getLooptime()); - //Static Gyro notch running and PID frequency - if (gyroConfig()->gyro_notch_hz) { - notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; - for (int axis = 0; axis < 3; axis++) { - notchFilter1[axis] = &gyroFilterNotch_1[axis]; - biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_notch_hz, gyroConfig()->gyro_notch_cutoff); - } - } - #ifdef USE_GYRO_KALMAN if (gyroConfig()->kalmanEnabled) { gyroKalmanInitialize(gyroConfig()->kalman_q); @@ -470,7 +453,6 @@ void FAST_CODE NOINLINE gyroFilter() #endif gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf); - gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf); #ifdef USE_DYNAMIC_FILTERS if (dynamicGyroNotchState.enabled) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index bacce4b458..44d567e570 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -59,8 +59,6 @@ typedef struct gyroConfig_s { #ifdef USE_DUAL_GYRO uint8_t gyro_to_use; #endif - uint16_t gyro_notch_hz; - uint16_t gyro_notch_cutoff; uint16_t gyro_main_lpf_hz; uint8_t gyro_main_lpf_type; uint8_t useDynamicLpf; diff --git a/src/main/target/FALCORE/config.c b/src/main/target/FALCORE/config.c index d1e6e71818..2deb505226 100755 --- a/src/main/target/FALCORE/config.c +++ b/src/main/target/FALCORE/config.c @@ -61,8 +61,6 @@ void targetConfiguration(void) gyroConfigMutable()->looptime = 1000; gyroConfigMutable()->gyro_lpf = 0; // 256 Hz gyroConfigMutable()->gyro_main_lpf_hz = 90; - gyroConfigMutable()->gyro_notch_hz = 150; - gyroConfigMutable()->gyro_notch_cutoff = 80; accelerometerConfigMutable()->acc_hardware = ACC_MPU6500; accelerometerConfigMutable()->acc_lpf_hz = 15; diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 1f6e7dcd00..a7a20a5324 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -9,6 +9,11 @@ set_property(SOURCE alignsensor_unittest.cc PROPERTY depends set_property(SOURCE bitarray_unittest.cc PROPERTY depends "common/bitarray.c") +set_property(SOURCE flight_imu_unittest.cc PROPERTY depends "build/debug.c" + "common/maths.c" "common/calibration.c" "common/filter.c" + "drivers/accgyro/accgyro_fake.c" "flight/imu.c" "sensors/boardalignment.c" + "sensors/gyro.c") + set_property(SOURCE maths_unittest.cc PROPERTY depends "common/maths.c") set_property(SOURCE olc_unittest.cc PROPERTY depends "common/olc.c") diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc new file mode 100644 index 0000000000..25e2c438c3 --- /dev/null +++ b/src/test/unit/flight_imu_unittest.cc @@ -0,0 +1,140 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include + +extern "C" { + #include "sensors/gyro.h" + #include "sensors/compass.h" + #include "sensors/acceleration.h" + + #include "scheduler/scheduler.h" + #include "fc/runtime_config.h" + + #include "io/gps.h" + + #include "flight/imu.h" +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +extern "C" { +STATIC_UNIT_TESTED void imuUpdateEulerAngles(void); +STATIC_UNIT_TESTED void imuComputeQuaternionFromRPY(int16_t initialRoll, int16_t initialPitch, int16_t initialYaw); +} + +TEST(FlightImuTest, TestEulerAngleCalculation) +{ + imuComputeQuaternionFromRPY(0, 0, 0); + imuUpdateEulerAngles(); + EXPECT_NEAR(attitude.values.roll, 0, 1); + EXPECT_NEAR(attitude.values.pitch, 0, 1); + EXPECT_NEAR(attitude.values.yaw, 0, 1); + + imuComputeQuaternionFromRPY(450, 450, 0); + imuUpdateEulerAngles(); + EXPECT_NEAR(attitude.values.roll, 450, 1); + EXPECT_NEAR(attitude.values.pitch, 450, 1); + EXPECT_NEAR(attitude.values.yaw, 0, 1); + + imuComputeQuaternionFromRPY(-450, -450, 0); + imuUpdateEulerAngles(); + EXPECT_NEAR(attitude.values.roll, -450, 1); + EXPECT_NEAR(attitude.values.pitch, -450, 1); + EXPECT_NEAR(attitude.values.yaw, 0, 1); + + imuComputeQuaternionFromRPY(1790, 0, 0); + imuUpdateEulerAngles(); + EXPECT_NEAR(attitude.values.roll, 1790, 1); + EXPECT_NEAR(attitude.values.pitch, 0, 1); + EXPECT_NEAR(attitude.values.yaw, 0, 1); + + imuComputeQuaternionFromRPY(-1790, 0, 0); + imuUpdateEulerAngles(); + EXPECT_NEAR(attitude.values.roll, -1790, 1); + EXPECT_NEAR(attitude.values.pitch, 0, 1); + EXPECT_NEAR(attitude.values.yaw, 0, 1); + + imuComputeQuaternionFromRPY(0, 0, 900); + imuUpdateEulerAngles(); + EXPECT_NEAR(attitude.values.roll, 0, 1); + EXPECT_NEAR(attitude.values.pitch, 0, 1); + EXPECT_NEAR(attitude.values.yaw, 900, 1); + + imuComputeQuaternionFromRPY(0, 0, 2700); + imuUpdateEulerAngles(); + EXPECT_NEAR(attitude.values.roll, 0, 1); + EXPECT_NEAR(attitude.values.pitch, 0, 1); + EXPECT_NEAR(attitude.values.yaw, 2700, 1); +} + +// STUBS + +extern "C" { + +uint32_t stateFlags; +uint32_t flightModeFlags; +uint32_t armingFlags; + +acc_t acc; +mag_t mag; + +gpsSolutionData_t gpsSol; + +compassConfig_t compassConfig_System; + +uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE }; + +bool sensors(uint32_t mask) +{ + UNUSED(mask); + return false; +}; +uint32_t millis(void) { return 0; } +timeDelta_t getLooptime(void) { return gyro.targetLooptime; } +timeDelta_t getGyroLooptime(void) { return gyro.targetLooptime; } +void schedulerResetTaskStatistics(cfTaskId_e) {} +void sensorsSet(uint32_t) {} +bool compassIsHealthy(void) { return true; } +void accGetVibrationLevels(fpVector3_t *accVibeLevels) +{ + accVibeLevels->x = fast_fsqrtf(acc.accVibeSq[X]); + accVibeLevels->y = fast_fsqrtf(acc.accVibeSq[Y]); + accVibeLevels->z = fast_fsqrtf(acc.accVibeSq[Z]); +} +void accGetMeasuredAcceleration(fpVector3_t *measuredAcc) +{ + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS; + } +} +uint32_t accGetClipCount(void) +{ + return acc.accClipCount; +} +void accUpdate(void) +{ +} +void resetHeadingHoldTarget(int16_t heading) +{ + UNUSED(heading); +} +bool isGPSHeadingValid(void) { return true; } +} diff --git a/src/test/unit/flight_imu_unittest.cc.txt b/src/test/unit/flight_imu_unittest.cc.txt deleted file mode 100644 index 28886c89ff..0000000000 --- a/src/test/unit/flight_imu_unittest.cc.txt +++ /dev/null @@ -1,182 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include - -#include - -#define USE_BARO - -extern "C" { - #include "debug.h" - - #include "common/axis.h" - #include "common/maths.h" - - #include "sensors/sensors.h" - #include "drivers/sensor.h" - #include "drivers/accgyro.h" - #include "drivers/compass.h" - #include "sensors/gyro.h" - #include "sensors/compass.h" - #include "sensors/acceleration.h" - #include "sensors/barometer.h" - - #include "config/runtime_config.h" - - #include "rx/rx.h" - - #include "flight/mixer.h" - #include "flight/pid.h" - #include "flight/imu.h" -} - -#include "unittest_macros.h" -#include "gtest/gtest.h" - -extern float q0, q1, q2, q3; -extern "C" { -void imuComputeRotationMatrix(void); -void imuUpdateEulerAngles(void); -} - -void imuComputeQuaternionFromRPY(int16_t initialRoll, int16_t initialPitch, int16_t initialYaw) -{ - if (initialRoll > 1800) initialRoll -= 3600; - if (initialPitch > 1800) initialPitch -= 3600; - if (initialYaw > 1800) initialYaw -= 3600; - - float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f); - float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f); - - float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f); - float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f); - - float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f); - float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f); - - q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; - q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; - q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; - q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; - - imuComputeRotationMatrix(); -} - -TEST(FlightImuTest, TestEulerAngleCalculation) -{ - imuComputeQuaternionFromRPY(0, 0, 0); - imuUpdateEulerAngles(); - EXPECT_FLOAT_EQ(attitude.values.roll, 0); - EXPECT_FLOAT_EQ(attitude.values.pitch, 0); - EXPECT_FLOAT_EQ(attitude.values.yaw, 0); - - imuComputeQuaternionFromRPY(450, 450, 0); - imuUpdateEulerAngles(); - EXPECT_FLOAT_EQ(attitude.values.roll, 450); - EXPECT_FLOAT_EQ(attitude.values.pitch, 450); - EXPECT_FLOAT_EQ(attitude.values.yaw, 0); - - imuComputeQuaternionFromRPY(-450, -450, 0); - imuUpdateEulerAngles(); - EXPECT_FLOAT_EQ(attitude.values.roll, -450); - EXPECT_FLOAT_EQ(attitude.values.pitch, -450); - EXPECT_FLOAT_EQ(attitude.values.yaw, 0); - - imuComputeQuaternionFromRPY(1790, 0, 0); - imuUpdateEulerAngles(); - EXPECT_FLOAT_EQ(attitude.values.roll, 1790); - EXPECT_FLOAT_EQ(attitude.values.pitch, 0); - EXPECT_FLOAT_EQ(attitude.values.yaw, 0); - - imuComputeQuaternionFromRPY(-1790, 0, 0); - imuUpdateEulerAngles(); - EXPECT_FLOAT_EQ(attitude.values.roll, -1790); - EXPECT_FLOAT_EQ(attitude.values.pitch, 0); - EXPECT_FLOAT_EQ(attitude.values.yaw, 0); - - imuComputeQuaternionFromRPY(0, 0, 900); - imuUpdateEulerAngles(); - EXPECT_FLOAT_EQ(attitude.values.roll, 0); - EXPECT_FLOAT_EQ(attitude.values.pitch, 0); - EXPECT_FLOAT_EQ(attitude.values.yaw, 900); - - imuComputeQuaternionFromRPY(0, 0, 2700); - imuUpdateEulerAngles(); - EXPECT_FLOAT_EQ(attitude.values.roll, 0); - EXPECT_FLOAT_EQ(attitude.values.pitch, 0); - EXPECT_FLOAT_EQ(attitude.values.yaw, 2700); -} - -// STUBS - -extern "C" { -uint32_t rcModeActivationMask; -int16_t rcCommand[4]; -int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; - -uint16_t acc_1G; -int16_t heading; -gyro_t gyro; -int32_t magADC[XYZ_AXIS_COUNT]; -int32_t BaroAlt; -int32_t debug[DEBUG32_VALUE_COUNT]; - -uint8_t stateFlags; -uint16_t flightModeFlags; -uint8_t armingFlags; - -int32_t sonarAlt; -int16_t sonarCfAltCm; -int32_t accADC[XYZ_AXIS_COUNT]; -int32_t gyroADC[XYZ_AXIS_COUNT]; - -int16_t GPS_speed; -int16_t GPS_ground_course; -int16_t GPS_numSat; -int16_t cycleTime = 2000; - - -uint16_t enableFlightMode(flightModeFlags_e mask) -{ - return flightModeFlags |= (mask); -} - -uint16_t disableFlightMode(flightModeFlags_e mask) -{ - return flightModeFlags &= ~(mask); -} - -void gyroUpdate(void) {}; -bool sensors(uint32_t mask) -{ - UNUSED(mask); - return false; -}; -void updateAccelerationReadings(void) -{ -} -bool isGyroCalibrationComplete(void) { return 1; } -bool isCompassReady(void) { return 1; } -uint32_t micros(void) { return 0; } -uint32_t millis(void) { return 0; } -bool isBaroCalibrationComplete(void) { return true; } -void performBaroCalibrationCycle(void) {} -int32_t baroCalculateAltitude(void) { return 0; } - -}