diff --git a/.dir-locals.el b/.dir-locals.el new file mode 100644 index 00000000000..9c3727aef8e --- /dev/null +++ b/.dir-locals.el @@ -0,0 +1,5 @@ +;;; Directory Local Variables -*- no-byte-compile: t -*- +;;; For more information see (info "(emacs) Directory Variables") + +((nil . ((c-basic-offset . 4) + (c-default-style . "k&r")))) diff --git a/.gitignore b/.gitignore index ea13b9f1789..3cbe312ed07 100644 --- a/.gitignore +++ b/.gitignore @@ -12,7 +12,7 @@ __pycache__ startup_stm32f10x_md_gcc.s .vagrant/ -.vscode/ +#.vscode/ cov-int* /build/ /obj/ @@ -31,3 +31,4 @@ README.pdf # local changes only make/local.mk +launch.json diff --git a/.vimrc b/.vimrc new file mode 100644 index 00000000000..547d37812cb --- /dev/null +++ b/.vimrc @@ -0,0 +1,9 @@ +filetype on +filetype indent on + +set expandtab +set bs=2 +set sw=4 +set ts=4 + + diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000000..2cece3ee127 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,12 @@ +{ + "files.associations": { + "chrono": "c", + "cmath": "c", + "ranges": "c" + }, + "editor.tabSize": 4, + "editor.insertSpaces": true, + "editor.detectIndentation": false, + "editor.expandTabs": true, + "C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4, BreakBeforeBraces: Mozilla }" +} diff --git a/docs/OSD.md b/docs/OSD.md new file mode 100644 index 00000000000..64530f0e2c7 --- /dev/null +++ b/docs/OSD.md @@ -0,0 +1,167 @@ +# On Screen Display + +The On Screen Display, or OSD, is a feature that overlays flight data over the video image. This can be done on the flight controller, using the analogue MAX7456 chip. Digital systems take the OSD data, via MSP DisplayPort, send it to the video receiver; which combines the data with the image. You can specify what elements are displayed, and their locations on the image. Most systems are character based, and use the MAX7456 analogue setup, or MSP DisplayPort. However, there are some different systems which are also supported. Such as the canvas based FrSKY PixelOSD on analogue. Canvas OSDs draw shapes on the image. Whereas character based OSDs use font characters to display the data. + +## Features and Limitations +Not all OSDs are created equally. This table shows the differences between the different systems available. + +| OSD System | Character grid | Character | Canvas | MSP DisplayPort | All elements supported | +|---------------|-------------------|-----------|-----------|-------------------|---------------------------| +| Analogue PAL | 30 x 16 | X | | | YES | +| Analogue NTSC | 30 x 13 | X | | | YES | +| PixelOSD | As PAL or NTSC | | X | | YES | +| DJI OSD | 30 x 16 | X | | | NO - BF Characters only | +| DJI WTFOS | 60 x 22 | X | | X | YES | +| HDZero | 50 x 18 | X | | X | YES | +| Avatar | 53 x 20 | X | | X | YES | +| DJI O3 | 53 x 20 (HD) | X | | X (partial) | NO - BF Characters only | + +## OSD Elements +Here are the OSD Elements provided by INAV. + +| ID | Element | Added | +|-------|-----------------------------------------------|-------| +| 0 | OSD_RSSI_VALUE | 1.0.0 | +| 1 | OSD_MAIN_BATT_VOLTAGE | 1.0.0 | +| 2 | OSD_CROSSHAIRS | 1.0.0 | +| 3 | OSD_ARTIFICIAL_HORIZON | 1.0.0 | +| 4 | OSD_HORIZON_SIDEBARS | 1.0.0 | +| 5 | OSD_ONTIME | 1.0.0 | +| 6 | OSD_FLYTIME | 1.0.0 | +| 7 | OSD_FLYMODE | 1.0.0 | +| 8 | OSD_CRAFT_NAME | 1.0.0 | +| 9 | OSD_THROTTLE_POS | 1.0.0 | +| 10 | OSD_VTX_CHANNEL | 1.0.0 | +| 11 | OSD_CURRENT_DRAW | 1.0.0 | +| 12 | OSD_MAH_DRAWN | 1.0.0 | +| 13 | OSD_GPS_SPEED | 1.0.0 | +| 14 | OSD_GPS_SATS | 1.0.0 | +| 15 | OSD_ALTITUDE | 1.0.0 | +| 16 | OSD_ROLL_PIDS | 1.6.0 | +| 17 | OSD_PITCH_PIDS | 1.6.0 | +| 18 | OSD_YAW_PIDS | 1.6.0 | +| 19 | OSD_POWER | 1.6.0 | +| 20 | OSD_GPS_LON | 1.6.0 | +| 21 | OSD_GPS_LAT | 1.6.0 | +| 22 | OSD_HOME_DIR | 1.6.0 | +| 23 | OSD_HOME_DIST | 1.6.0 | +| 24 | OSD_HEADING | 1.6.0 | +| 25 | OSD_VARIO | 1.6.0 | +| 26 | OSD_VARIO_NUM | 1.6.0 | +| 27 | OSD_AIR_SPEED | 1.7.3 | +| 28 | OSD_ONTIME_FLYTIME | 1.8.0 | +| 29 | OSD_RTC_TIME | 1.8.0 | +| 30 | OSD_MESSAGES | 1.8.0 | +| 31 | OSD_GPS_HDOP | 1.8.0 | +| 32 | OSD_MAIN_BATT_CELL_VOLTAGE | 1.8.0 | +| 33 | OSD_SCALED_THROTTLE_POS | 1.8.0 | +| 34 | OSD_HEADING_GRAPH | 1.8.0 | +| 35 | OSD_EFFICIENCY_MAH_PER_KM | 1.9.0 | +| 36 | OSD_WH_DRAWN | 1.9.0 | +| 37 | OSD_BATTERY_REMAINING_CAPACITY | 1.9.0 | +| 38 | OSD_BATTERY_REMAINING_PERCENT | 1.9.0 | +| 39 | OSD_EFFICIENCY_WH_PER_KM | 1.9.0 | +| 40 | OSD_TRIP_DIST | 1.9.1 | +| 41 | OSD_ATTITUDE_PITCH | 2.0.0 | +| 42 | OSD_ATTITUDE_ROLL | 2.0.0 | +| 43 | OSD_MAP_NORTH | 2.0.0 | +| 44 | OSD_MAP_TAKEOFF | 2.0.0 | +| 45 | OSD_RADAR | 2.0.0 | +| 46 | OSD_WIND_SPEED_HORIZONTAL | 2.0.0 | +| 47 | OSD_WIND_SPEED_VERTICAL | 2.0.0 | +| 48 | OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH | 2.0.0 | +| 49 | OSD_REMAINING_DISTANCE_BEFORE_RTH | 2.0.0 | +| 50 | OSD_HOME_HEADING_ERROR | 2.0.0 | +| 51 | OSD_COURSE_HOLD_ERROR | 2.0.0 | +| 52 | OSD_COURSE_HOLD_ADJUSTMENT | 2.0.0 | +| 53 | OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE | 2.0.0 | +| 54 | OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE | 2.0.0 | +| 55 | OSD_POWER_SUPPLY_IMPEDANCE | 2.0.0 | +| 56 | OSD_LEVEL_PIDS | 2.0.0 | +| 57 | OSD_POS_XY_PIDS | 2.0.0 | +| 58 | OSD_POS_Z_PIDS | 2.0.0 | +| 59 | OSD_VEL_XY_PIDS | 2.0.0 | +| 60 | OSD_VEL_Z_PIDS | 2.0.0 | +| 61 | OSD_HEADING_P | 2.0.0 | +| 62 | OSD_BOARD_ALIGN_ROLL | 2.0.0 | +| 63 | OSD_BOARD_ALIGN_PITCH | 2.0.0 | +| 64 | OSD_RC_EXPO | 2.0.0 | +| 65 | OSD_RC_YAW_EXPO | 2.0.0 | +| 66 | OSD_THROTTLE_EXPO | 2.0.0 | +| 67 | OSD_PITCH_RATE | 2.0.0 | +| 68 | OSD_ROLL_RATE | 2.0.0 | +| 69 | OSD_YAW_RATE | 2.0.0 | +| 70 | OSD_MANUAL_RC_EXPO | 2.0.0 | +| 71 | OSD_MANUAL_RC_YAW_EXPO | 2.0.0 | +| 72 | OSD_MANUAL_PITCH_RATE | 2.0.0 | +| 73 | OSD_MANUAL_ROLL_RATE | 2.0.0 | +| 74 | OSD_MANUAL_YAW_RATE | 2.0.0 | +| 75 | OSD_NAV_FW_CRUISE_THR | 2.0.0 | +| 76 | OSD_NAV_FW_PITCH2THR | 2.0.0 | +| 77 | OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE | 2.0.0 | +| 78 | OSD_DEBUG | 2.0.0 | +| 79 | OSD_FW_ALT_PID_OUTPUTS | 2.0.0 | +| 80 | OSD_FW_POS_PID_OUTPUTS | 2.0.0 | +| 81 | OSD_MC_VEL_X_PID_OUTPUTS | 2.0.0 | +| 82 | OSD_MC_VEL_Y_PID_OUTPUTS | 2.0.0 | +| 83 | OSD_MC_VEL_Z_PID_OUTPUTS | 2.0.0 | +| 84 | OSD_MC_POS_XYZ_P_OUTPUTS | 2.0.0 | +| 85 | OSD_3D_SPEED | 2.1.0 | +| 86 | OSD_IMU_TEMPERATURE | 2.1.0 | +| 87 | OSD_BARO_TEMPERATURE | 2.1.0 | +| 88 | OSD_TEMP_SENSOR_0_TEMPERATURE | 2.1.0 | +| 89 | OSD_TEMP_SENSOR_1_TEMPERATURE | 2.1.0 | +| 90 | OSD_TEMP_SENSOR_2_TEMPERATURE | 2.1.0 | +| 91 | OSD_TEMP_SENSOR_3_TEMPERATURE | 2.1.0 | +| 92 | OSD_TEMP_SENSOR_4_TEMPERATURE | 2.1.0 | +| 93 | OSD_TEMP_SENSOR_5_TEMPERATURE | 2.1.0 | +| 94 | OSD_TEMP_SENSOR_6_TEMPERATURE | 2.1.0 | +| 95 | OSD_TEMP_SENSOR_7_TEMPERATURE | 2.1.0 | +| 96 | OSD_ALTITUDE_MSL | 2.1.0 | +| 97 | OSD_PLUS_CODE | 2.1.0 | +| 98 | OSD_MAP_SCALE | 2.2.0 | +| 99 | OSD_MAP_REFERENCE | 2.2.0 | +| 100 | OSD_GFORCE | 2.2.0 | +| 101 | OSD_GFORCE_X | 2.2.0 | +| 102 | OSD_GFORCE_Y | 2.2.0 | +| 103 | OSD_GFORCE_Z | 2.2.0 | +| 104 | OSD_RC_SOURCE | 2.2.0 | +| 105 | OSD_VTX_POWER | 2.2.0 | +| 106 | OSD_ESC_RPM | 2.3.0 | +| 107 | OSD_ESC_TEMPERATURE | 2.5.0 | +| 108 | OSD_AZIMUTH | 2.6.0 | +| 109 | OSD_CRSF_RSSI_DBM | 2.6.0 | +| 110 | OSD_CRSF_LQ | 2.6.0 | +| 111 | OSD_CRSF_SNR_DB | 2.6.0 | +| 112 | OSD_CRSF_TX_POWER | 2.6.0 | +| 113 | OSD_GVAR_0 | 2.6.0 | +| 114 | OSD_GVAR_1 | 2.6.0 | +| 115 | OSD_GVAR_2 | 2.6.0 | +| 116 | OSD_GVAR_3 | 2.6.0 | +| 117 | OSD_TPA | 2.6.0 | +| 118 | OSD_NAV_FW_CONTROL_SMOOTHNESS | 2.6.0 | +| 119 | OSD_VERSION | 3.0.0 | +| 120 | OSD_RANGEFINDER | 3.0.0 | +| 121 | OSD_PLIMIT_REMAINING_BURST_TIME | 3.0.0 | +| 122 | OSD_PLIMIT_ACTIVE_CURRENT_LIMIT | 3.0.0 | +| 123 | OSD_PLIMIT_ACTIVE_POWER_LIMIT | 3.0.0 | +| 124 | OSD_GLIDESLOPE | 3.0.1 | +| 125 | OSD_GPS_MAX_SPEED | 4.0.0 | +| 126 | OSD_3D_MAX_SPEED | 4.0.0 | +| 127 | OSD_AIR_MAX_SPEED | 4.0.0 | +| 128 | OSD_ACTIVE_PROFILE | 4.0.0 | +| 129 | OSD_MISSION | 4.0.0 | +| 130 | OSD_SWITCH_INDICATOR_0 | 5.0.0 | +| 131 | OSD_SWITCH_INDICATOR_1 | 5.0.0 | +| 132 | OSD_SWITCH_INDICATOR_2 | 5.0.0 | +| 133 | OSD_SWITCH_INDICATOR_3 | 5.0.0 | +| 134 | OSD_TPA_TIME_CONSTANT | 5.0.0 | +| 135 | OSD_FW_LEVEL_TRIM | 5.0.0 | +| 136 | OSD_GLIDE_TIME_REMAINING | 6.0.0 | +| 137 | OSD_GLIDE_RANGE | 6.0.0 | +| 138 | OSD_CLIMB_EFFICIENCY | 6.0.0 | +| 139 | OSD_NAV_WP_MULTI_MISSION_INDEX | 6.0.0 | +| 140 | OSD_GROUND_COURSE | 6.0.0 | +| 141 | OSD_CROSS_TRACK_ERROR | 6.0.0 | +| 142 | OSD_PILOT_NAME | 6.0.0 | +| 143 | OSD_PAN_SERVO_CENTRED | 6.0.0 | \ No newline at end of file diff --git a/docs/Settings.md b/docs/Settings.md index 98b47b91499..7c65ce34200 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1372,6 +1372,16 @@ Automatic configuration of GPS baudrate(The specified baudrate in configured in --- +### gps_auto_baud_max_supported + +Max baudrate supported by GPS unit. This is used during autobaud. M8 supports up to 460400, M10 supports up to 921600 and 230400 is the value used before INAV 7.0 + +| Default | Min | Max | +| --- | --- | --- | +| 230400 | | | + +--- + ### gps_auto_config Enable automatic configuration of UBlox GPS receivers. diff --git a/docs/boards/SPEEDYBEEF405V3.md b/docs/boards/SPEEDYBEEF405V3.md index 8dd310c3851..7ec4b1ce944 100644 --- a/docs/boards/SPEEDYBEEF405V3.md +++ b/docs/boards/SPEEDYBEEF405V3.md @@ -1,3 +1,18 @@ # SpeedyBee F405 V3 -> Notice: At the moment, DSHOT is not supported on SpeedyBe F405 V3. Use Multishot instead \ No newline at end of file +> Notice: At the moment, DSHOT is not supported on SpeedyBe F405 V3. Use Multishot instead + +> Notice: The analog OSD and the SD card (blackbox) share the same SPI bus, which can cause problems when trying to use analog OSD and blackbox at the same time. + +## Flashing firmware + +We often see reports of users having problems flashing new firmware with this baord. The following sugestions usually seem to fix the issues. + +* Remove SD Card +* Disconnect devices from UART1 and UART3 + +Try removing the SD Card first, and if that still does not fix the issue, disconnect the devices connected to UART1 and UART3. + + + + diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 6325396c7ec..5d39761b647 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -534,6 +534,8 @@ main_sources(COMMON_SRC io/vtx_ffpv24g.h io/vtx_control.c io/vtx_control.h + io/vtx_msp.c + io/vtx_msp.h navigation/navigation.c navigation/navigation.h diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index eeb65acfab9..679723e0a83 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -47,6 +47,13 @@ #ifndef ADC_CHANNEL_4_INSTANCE #define ADC_CHANNEL_4_INSTANCE ADC_INSTANCE #endif +#ifndef ADC_CHANNEL_5_INSTANCE +#define ADC_CHANNEL_5_INSTANCE ADC_INSTANCE +#endif +#ifndef ADC_CHANNEL_6_INSTANCE +#define ADC_CHANNEL_6_INSTANCE ADC_INSTANCE +#endif + #ifdef USE_ADC @@ -99,7 +106,7 @@ uint16_t adcGetChannel(uint8_t function) } } -#if defined(ADC_CHANNEL_1_PIN) || defined(ADC_CHANNEL_2_PIN) || defined(ADC_CHANNEL_3_PIN) || defined(ADC_CHANNEL_4_PIN) +#if defined(ADC_CHANNEL_1_PIN) || defined(ADC_CHANNEL_2_PIN) || defined(ADC_CHANNEL_3_PIN) || defined(ADC_CHANNEL_4_PIN) || defined(ADC_CHANNEL_5_PIN) || defined(ADC_CHANNEL_6_PIN) static bool isChannelInUse(int channel) { for (int i = 0; i < ADC_FUNCTION_COUNT; i++) { @@ -111,7 +118,7 @@ static bool isChannelInUse(int channel) } #endif -#if !defined(ADC_CHANNEL_1_PIN) || !defined(ADC_CHANNEL_2_PIN) || !defined(ADC_CHANNEL_3_PIN) || !defined(ADC_CHANNEL_4_PIN) +#if !defined(ADC_CHANNEL_1_PIN) || !defined(ADC_CHANNEL_2_PIN) || !defined(ADC_CHANNEL_3_PIN) || !defined(ADC_CHANNEL_4_PIN) || !defined(ADC_CHANNEL_5_PIN) || !defined(ADC_CHANNEL_6_PIN) static void disableChannelMapping(int channel) { for (int i = 0; i < ADC_FUNCTION_COUNT; i++) { @@ -192,6 +199,33 @@ void adcInit(drv_adc_config_t *init) disableChannelMapping(ADC_CHN_4); #endif +#ifdef ADC_CHANNEL_5_PIN + if (isChannelInUse(ADC_CHN_5)) { + adcConfig[ADC_CHN_5].adcDevice = adcDeviceByInstance(ADC_CHANNEL_5_INSTANCE); + if (adcConfig[ADC_CHN_5].adcDevice != ADCINVALID) { + adcConfig[ADC_CHN_5].tag = IO_TAG(ADC_CHANNEL_5_PIN); +#if defined(USE_ADC_AVERAGING) + activeChannelCount[adcConfig[ADC_CHN_5].adcDevice] += 1; +#endif + } + } +#else + disableChannelMapping(ADC_CHN_5); +#endif + +#ifdef ADC_CHANNEL_6_PIN + if (isChannelInUse(ADC_CHN_6)) { + adcConfig[ADC_CHN_6].adcDevice = adcDeviceByInstance(ADC_CHANNEL_6_INSTANCE); + if (adcConfig[ADC_CHN_6].adcDevice != ADCINVALID) { + adcConfig[ADC_CHN_6].tag = IO_TAG(ADC_CHANNEL_6_PIN); +#if defined(USE_ADC_AVERAGING) + activeChannelCount[adcConfig[ADC_CHN_6].adcDevice] += 1; +#endif + } + } +#else + disableChannelMapping(ADC_CHN_6); +#endif adcHardwareInit(init); } diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index 5ba4b999da0..1749c9fa63e 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -33,7 +33,9 @@ typedef enum { ADC_CHN_2, ADC_CHN_3, ADC_CHN_4, - ADC_CHN_MAX = ADC_CHN_4, + ADC_CHN_5, + ADC_CHN_6, + ADC_CHN_MAX = ADC_CHN_6, ADC_CHN_COUNT } adcChannel_e; diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index eb14d6772b6..4fc5a00b83e 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -195,6 +195,16 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw) return true; } #endif +#if defined(ADC_CHANNEL_5_PIN) + if (timHw->tag == IO_TAG(ADC_CHANNEL_5_PIN)) { + return true; + } +#endif +#if defined(ADC_CHANNEL_6_PIN) + if (timHw->tag == IO_TAG(ADC_CHANNEL_6_PIN)) { + return true; + } +#endif #endif return false; diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index 6f4cd52373f..b175dad8683 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -24,6 +24,7 @@ #include "platform.h" #include "build/debug.h" +#include "common/log.h" #include "vtx_common.h" @@ -73,8 +74,9 @@ void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t ch if (!vtxDevice) return; - if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount)) + if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount)) { return; + } if (vtxDevice->vTable->setBandAndChannel) { vtxDevice->vTable->setBandAndChannel(vtxDevice, band, channel); diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 1910075a831..ed4d5e251a1 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -36,7 +36,7 @@ #define VTX_SETTINGS_MIN_FREQUENCY_MHZ 0 //min freq (in MHz) for 'vtx_freq' setting #define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999 //max freq (in MHz) for 'vtx_freq' setting -#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) #define VTX_SETTINGS_POWER_COUNT 5 #define VTX_SETTINGS_DEFAULT_POWER 1 @@ -63,6 +63,7 @@ typedef enum { VTXDEV_SMARTAUDIO = 3, VTXDEV_TRAMP = 4, VTXDEV_FFPV = 5, + VTXDEV_MSP = 6, VTXDEV_UNKNOWN = 0xFF, } vtxDevType_e; diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 9558c0deaf9..30ba9f98bd5 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -123,6 +123,21 @@ timeUs_t lastDisarmTimeUs = 0; static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible static timeMs_t prearmActivationTime = 0; +static bool isAccRequired(void) { + return isModeActivationConditionPresent(BOXNAVPOSHOLD) || + isModeActivationConditionPresent(BOXNAVRTH) || + isModeActivationConditionPresent(BOXNAVWP) || + isModeActivationConditionPresent(BOXANGLE) || + isModeActivationConditionPresent(BOXHORIZON) || + isModeActivationConditionPresent(BOXNAVALTHOLD) || + isModeActivationConditionPresent(BOXHEADINGHOLD) || + isModeActivationConditionPresent(BOXNAVLAUNCH) || + isModeActivationConditionPresent(BOXTURNASSIST) || + isModeActivationConditionPresent(BOXNAVCOURSEHOLD) || + isModeActivationConditionPresent(BOXSOARING) || + failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_DROP_IT; +} + bool areSensorsCalibrating(void) { #ifdef USE_BARO @@ -143,11 +158,11 @@ bool areSensorsCalibrating(void) } #endif - if (!navIsCalibrationComplete()) { + if (!navIsCalibrationComplete() && isAccRequired()) { return true; } - if (!accIsCalibrationComplete() && sensors(SENSOR_ACC)) { + if (!accIsCalibrationComplete() && sensors(SENSOR_ACC) && isAccRequired()) { return true; } @@ -265,21 +280,7 @@ static void updateArmingStatus(void) sensors(SENSOR_ACC) && !STATE(ACCELEROMETER_CALIBRATED) && // Require ACC calibration only if any of the setting might require it - ( - isModeActivationConditionPresent(BOXNAVPOSHOLD) || - isModeActivationConditionPresent(BOXNAVRTH) || - isModeActivationConditionPresent(BOXNAVWP) || - isModeActivationConditionPresent(BOXANGLE) || - isModeActivationConditionPresent(BOXHORIZON) || - isModeActivationConditionPresent(BOXNAVALTHOLD) || - isModeActivationConditionPresent(BOXHEADINGHOLD) || - isModeActivationConditionPresent(BOXNAVLAUNCH) || - isModeActivationConditionPresent(BOXTURNASSIST) || - isModeActivationConditionPresent(BOXNAVCOURSEHOLD) || - isModeActivationConditionPresent(BOXSOARING) || - failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_DROP_IT - - ) + isAccRequired() ) { ENABLE_ARMING_FLAG(ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED); } diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 34b2dfa8227..1b6d2df4715 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -119,6 +119,7 @@ #include "io/vtx_control.h" #include "io/vtx_smartaudio.h" #include "io/vtx_tramp.h" +#include "io/vtx_msp.h" #include "io/vtx_ffpv24g.h" #include "io/piniobox.h" @@ -665,6 +666,10 @@ void init(void) vtxFuriousFPVInit(); #endif +#ifdef USE_VTX_MSP + vtxMspInit(); +#endif + #endif // USE_VTX_CONTROL // Now that everything has powered up the voltage and cell count be determined. diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6d648d7855c..008d1e587be 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2482,12 +2482,24 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel const uint8_t newBand = (newFrequency / 8) + 1; const uint8_t newChannel = (newFrequency % 8) + 1; + + if(vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) { + vtxCommonSetBandAndChannel(vtxDevice, newBand, newChannel); + } + vtxSettingsConfigMutable()->band = newBand; vtxSettingsConfigMutable()->channel = newChannel; } if (sbufBytesRemaining(src) > 1) { - vtxSettingsConfigMutable()->power = sbufReadU8(src); + uint8_t newPower = sbufReadU8(src); + uint8_t currentPower = 0; + vtxCommonGetPowerIndex(vtxDevice, ¤tPower); + if (newPower != currentPower) { + vtxCommonSetPowerByIndex(vtxDevice, newPower); + vtxSettingsConfigMutable()->power = newPower; + } + // Delegate pitmode to vtx directly const uint8_t newPitmode = sbufReadU8(src); uint8_t currentPitmode = 0; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 7b2ba3b5d55..b41acdb51b1 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -64,6 +64,7 @@ #include "io/rcdevice_cam.h" #include "io/smartport_master.h" #include "io/vtx.h" +#include "io/vtx_msp.h" #include "io/osd_dji_hd.h" #include "io/displayport_msp_osd.h" #include "io/servo_sbus.h" @@ -109,6 +110,9 @@ void taskHandleSerial(timeUs_t currentTimeUs) #ifdef USE_MSP_OSD // Capture MSP Displayport messages to determine if VTX is connected mspOsdSerialProcess(mspFcProcessCommand); +#ifdef USE_VTX_MSP + mspVtxSerialProcess(mspFcProcessCommand); +#endif #endif } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 36a6b5785da..5d84a4573e5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -190,6 +190,9 @@ tables: - name: nav_fw_wp_turn_smoothing values: ["OFF", "ON", "ON-CUT"] enum: wpFwTurnSmoothing_e + - name: gps_auto_baud_max + values: [ '115200', '57600', '38400', '19200', '9600', '230400', '460800', '921600'] + enum: gpsBaudRate_e # CR103 - name: nav_mc_althold_throttle values: ["STICK", "MID_STICK", "HOVER"] @@ -1506,6 +1509,7 @@ groups: min: 1 max: 3000 - name: PG_GPS_CONFIG + headers: [ "io/gps.h" ] type: gpsConfig_t condition: USE_GPS members: @@ -1537,6 +1541,12 @@ groups: default_value: ON field: autoBaud type: bool + - name: gps_auto_baud_max_supported + description: "Max baudrate supported by GPS unit. This is used during autobaud. M8 supports up to 460400, M10 supports up to 921600 and 230400 is the value used before INAV 7.0" + default_value: "230400" + table: gps_auto_baud_max + field: autoBaudMax + type: uint8_t - name: gps_ublox_use_galileo description: "Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]." default_value: OFF diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a223d65d256..7a9b46976f8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -154,7 +154,7 @@ static EXTENDED_FASTRAM float iTermAntigravityGain; #endif static EXTENDED_FASTRAM uint8_t usedPidControllerType; -typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT); +typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv); static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; @@ -626,7 +626,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam } } - float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f); + float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f); // Apply simple LPF to angleRateTarget to make response less jerky // Ideas behind this: @@ -682,13 +682,13 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) { } #ifdef USE_D_BOOST -static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT) { +static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) { float dBoost = 1.0f; - const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT; + const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) * dT_inv; const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta)); - const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) / dT); + const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) * dT_inv); if (dBoostGyroAcceleration >= dBoostRateAcceleration) { //Gyro is accelerating faster than setpoint, we want to smooth out @@ -711,7 +711,7 @@ static float applyDBoost(pidState_t *pidState, float dT) { } #endif -static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT) { +static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) { // Calculate new D-term float newDTerm = 0; if (pidState->kD == 0) { @@ -723,7 +723,7 @@ static float dTermProcess(pidState_t *pidState, float currentRateTarget, float d delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta); // Calculate derivative - newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, currentRateTarget, dT); + newDTerm = delta * (pidState->kD * dT_inv) * applyDBoost(pidState, currentRateTarget, dT, dT_inv); } return(newDTerm); } @@ -737,19 +737,20 @@ static void applyItermLimiting(pidState_t *pidState) { } } -static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { +static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) { UNUSED(pidState); UNUSED(axis); UNUSED(dT); + UNUSED(dT_inv); } -static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) +static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) { const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); const float rateError = rateTarget - pidState->gyroRate; const float newPTerm = pTermProcess(pidState, rateError, dT); - const float newDTerm = dTermProcess(pidState, rateTarget, dT); + const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv); const float newFFTerm = rateTarget * pidState->kFF; /* @@ -807,14 +808,14 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, return itermErrorRate; } -static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) +static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) { const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); const float rateError = rateTarget - pidState->gyroRate; const float newPTerm = pTermProcess(pidState, rateError, dT); - const float newDTerm = dTermProcess(pidState, rateTarget, dT); + const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv); const float rateTargetDelta = rateTarget - pidState->previousRateTarget; const float rateTargetDeltaFiltered = pt3FilterApply(&pidState->rateTargetFilter, rateTargetDelta); @@ -1059,6 +1060,9 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis void FAST_CODE pidController(float dT) { + + const float dT_inv = 1.0f / dT; + if (!pidFiltersConfigured) { return; } @@ -1132,7 +1136,7 @@ void FAST_CODE pidController(float dT) } // Prevent strong Iterm accumulation during stick inputs - antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f); + antiWindupScaler = constrainf((1.0f - getMotorMixRange()) * motorItermWindupPoint, 0.0f, 1.0f); for (int axis = 0; axis < 3; axis++) { // Apply setpoint rate of change limits @@ -1142,7 +1146,7 @@ void FAST_CODE pidController(float dT) checkItermLimitingActive(&pidState[axis]); checkItermFreezingActive(&pidState[axis], axis); - pidControllerApplyFn(&pidState[axis], axis, dT); + pidControllerApplyFn(&pidState[axis], axis, dT, dT_inv); } } @@ -1174,7 +1178,7 @@ void pidInit(void) itermRelax = pidProfile()->iterm_relax; yawLpfHz = pidProfile()->yaw_lpf_hz; - motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f); + motorItermWindupPoint = 1.0f / (1.0f - (pidProfile()->itermWindupPointPercent / 100.0f)); #ifdef USE_D_BOOST dBoostMin = pidProfile()->dBoostMin; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index a53e23aaa41..fe56a927a5c 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -47,7 +47,7 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13 #define FP_PID_RATE_I_MULTIPLIER 4.0f #define FP_PID_RATE_D_MULTIPLIER 1905.0f #define FP_PID_RATE_D_FF_MULTIPLIER 7270.0f -#define FP_PID_LEVEL_P_MULTIPLIER 6.56f // Level P gain units is [1/sec] and angle error is [deg] => [deg/s] +#define FP_PID_LEVEL_P_MULTIPLIER 1.0f / 6.56f // Level P gain units is [1/sec] and angle error is [deg] => [deg/s] #define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f #define MC_ITERM_RELAX_SETPOINT_THRESHOLD 40.0f diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index d9688f1e829..2eaef1175ca 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -187,7 +187,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa // Use different max rate in ANLGE mode if (FLIGHT_MODE(ANGLE_MODE)) { - float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER; + float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER; maxDesiredRate = MIN(maxRateSetting, maxDesiredRateInAngleMode); } diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c index 2b0d05f022e..17f49c8b78d 100644 --- a/src/main/io/displayport_msp_osd.c +++ b/src/main/io/displayport_msp_osd.c @@ -515,4 +515,13 @@ void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn) } } +mspPort_t *getMspOsdPort() +{ + if (mspPort.port) { + return &mspPort; + } + + return NULL; +} + #endif // USE_MSP_OSD diff --git a/src/main/io/displayport_msp_osd.h b/src/main/io/displayport_msp_osd.h index cea787538d3..0a2f64c48a2 100644 --- a/src/main/io/displayport_msp_osd.h +++ b/src/main/io/displayport_msp_osd.h @@ -31,6 +31,7 @@ typedef struct displayPort_s displayPort_t; displayPort_t *mspOsdDisplayPortInit(const videoSystem_e videoSystem); void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn); +mspPort_t *getMspOsdPort(void); // MSP displayport V2 attribute byte bit functions #define DISPLAYPORT_MSP_ATTR_FONTPAGE 0 // Select bank of 256 characters as per displayPortSeverity_e diff --git a/src/main/io/gps.c b/src/main/io/gps.c index f8312069afd..4d7380e84df 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -74,7 +74,7 @@ gpsStatistics_t gpsStats; gpsSolutionData_t gpsSol; // Map gpsBaudRate_e index to baudRate_e -baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400 }; +baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400, BAUD_460800, BAUD_921600 }; static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = { /* NMEA GPS */ @@ -113,7 +113,7 @@ static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = { }; -PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 4); PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .provider = SETTING_GPS_PROVIDER_DEFAULT, @@ -125,13 +125,13 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT, .ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT, .ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT, - .ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT + .ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT, + .autoBaudMax = SETTING_GPS_AUTO_BAUD_MAX_SUPPORTED_DEFAULT ); - -int getGpsBaudrate(void) +int gpsBaudRateToInt(gpsBaudRate_e baudrate) { - switch(gpsState.baudrateIndex) + switch(baudrate) { case GPS_BAUDRATE_115200: return 115200; @@ -145,11 +145,20 @@ int getGpsBaudrate(void) return 9600; case GPS_BAUDRATE_230400: return 230400; + case GPS_BAUDRATE_460800: + return 460800; + case GPS_BAUDRATE_921600: + return 921600; default: return 0; } } +int getGpsBaudrate(void) +{ + return gpsBaudRateToInt(gpsState.baudrateIndex); +} + const char *getGpsHwVersion(void) { switch(gpsState.hwVersion) diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 0e0780dea3e..99b6aafbdf0 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -59,6 +59,8 @@ typedef enum { GPS_BAUDRATE_19200, GPS_BAUDRATE_9600, GPS_BAUDRATE_230400, + GPS_BAUDRATE_460800, + GPS_BAUDRATE_921600, GPS_BAUDRATE_COUNT } gpsBaudRate_e; @@ -97,6 +99,7 @@ typedef struct gpsConfig_s { bool ubloxUseGlonass; uint8_t gpsMinSats; uint8_t ubloxNavHz; + gpsBaudRate_e autoBaudMax; } gpsConfig_t; PG_DECLARE(gpsConfig_t, gpsConfig); @@ -173,6 +176,7 @@ uint8_t getGpsProtoMajorVersion(void); uint8_t getGpsProtoMinorVersion(void); int getGpsBaudrate(void); +int gpsBaudRateToInt(gpsBaudRate_e baudrate); #if defined(USE_GPS_FAKE) void gpsFakeSet( diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 2864d0f69c9..2baa97ce9b7 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -55,17 +55,20 @@ // SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE -// note PRNs last upadted 2020-12-18 +// note PRNs last upadted 2023-08-10 +// https://www.gps.gov/technical/prn-codes/L1-CA-PRN-code-assignments-2023-Apr.pdf #define SBASMASK1_BASE 120 #define SBASMASK1_BITS(prn) (1 << (prn-SBASMASK1_BASE)) static const uint32_t ubloxScanMode1[] = { 0x00000000, // AUTO - (SBASMASK1_BITS(123) | SBASMASK1_BITS(126) | SBASMASK1_BITS(136)), // SBAS - (SBASMASK1_BITS(131) | SBASMASK1_BITS(133) | SBASMASK1_BITS(138)), // WAAS - (SBASMASK1_BITS(129) | SBASMASK1_BITS(137)), // MAAS - (SBASMASK1_BITS(127) | SBASMASK1_BITS(128)), // GAGAN + (SBASMASK1_BITS(121) | SBASMASK1_BITS(123) | SBASMASK1_BITS(126) | SBASMASK1_BITS(136) | SBASMASK1_BITS(150)), // SBAS_EGNOS + (SBASMASK1_BITS(131) | SBASMASK1_BITS(133) | SBASMASK1_BITS(135) | SBASMASK1_BITS(138)), // WAAS + + (SBASMASK1_BITS(129) | SBASMASK1_BITS(137) | SBASMASK1_BITS(139)), // MSAS + + (SBASMASK1_BITS(127) | SBASMASK1_BITS(128) | SBASMASK1_BITS(132)), // GAGAN 0x00000000, // NONE }; @@ -76,6 +79,8 @@ static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = { "$PUBX,41,1,0003,0001,19200,0*23\r\n", // GPS_BAUDRATE_19200 "$PUBX,41,1,0003,0001,9600,0*16\r\n", // GPS_BAUDRATE_9600 "$PUBX,41,1,0003,0001,230400,0*1C\r\n", // GPS_BAUDRATE_230400 + "$PUBX,41,1,0003,0001,460800,0*13\r\n", // GPS_BAUDRATE_460800 + "$PUBX,41,1,0003,0001,921600,0*15\r\n" // GPS_BAUDRATE_921600 }; // Packet checksum accumulators @@ -377,9 +382,9 @@ static void configureGNSS10(void) { ubx_config_data8_payload_t gnssConfigValues[] = { // SBAS - {UBLOX_CFG_SIGNAL_SBAS_ENA, 1}, - {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, 1}, - + {UBLOX_CFG_SIGNAL_SBAS_ENA, gpsState.gpsConfig->sbasMode == SBAS_NONE ? 0 : 1}, + {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, gpsState.gpsConfig->sbasMode == SBAS_NONE ? 0 : 1}, + // Galileo {UBLOX_CFG_SIGNAL_GAL_ENA, gpsState.gpsConfig->ubloxUseGalileo}, {UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo}, @@ -995,6 +1000,11 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) // Try sending baud rate switch command at all common baud rates gpsSetProtocolTimeout((GPS_BAUD_CHANGE_DELAY + 50) * (GPS_BAUDRATE_COUNT)); for (gpsState.autoBaudrateIndex = 0; gpsState.autoBaudrateIndex < GPS_BAUDRATE_COUNT; gpsState.autoBaudrateIndex++) { + if (gpsBaudRateToInt(gpsState.autoBaudrateIndex) > gpsBaudRateToInt(gpsState.gpsConfig->autoBaudMax)) { + // trying higher baud rates fails on m8 gps + // autoBaudRateIndex is not sorted by baud rate + continue; + } // 2. Set serial port to baud rate and send an $UBX command to switch the baud rate specified by portConfig [baudrateIndex] serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]); serialPrint(gpsState.gpsPort, baudInitDataNMEA[gpsState.baudrateIndex]); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 91790578a53..d43566cd439 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3595,10 +3595,10 @@ uint8_t osdIncElementIndex(uint8_t elementIndex) elementIndex = OSD_AIR_MAX_SPEED; } if (elementIndex == OSD_GLIDE_RANGE) { - elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_MULTI_FUNCTION; // CR88 + elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_PILOT_NAME; } if (elementIndex == OSD_NAV_WP_MULTI_MISSION_INDEX) { - elementIndex = OSD_MULTI_FUNCTION; // CR88 + elementIndex = OSD_PILOT_NAME; } } diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 9cd0e7e1d7b..7766679106b 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -59,6 +59,8 @@ typedef enum { FUNCTION_MSP_OSD = (1 << 25), // 33554432 } serialPortFunction_e; +#define FUNCTION_VTX_MSP FUNCTION_MSP_OSD + typedef enum { BAUD_AUTO = 0, BAUD_1200, diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index 02fc8010d03..58651444f92 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -18,8 +18,10 @@ // Get target build configuration #include "platform.h" +#include "build/debug.h" #include "common/maths.h" +#include "common/log.h" #include "config/config_eeprom.h" #include "config/parameter_group.h" @@ -37,7 +39,6 @@ #include "io/osd.h" #include "io/vtx_control.h" - #if defined(USE_VTX_CONTROL) PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 4); diff --git a/src/main/io/vtx_msp.c b/src/main/io/vtx_msp.c new file mode 100644 index 00000000000..696918e5705 --- /dev/null +++ b/src/main/io/vtx_msp.c @@ -0,0 +1,701 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +/* Created by phobos- */ + +#include +#include +#include +#include +#include +#include + +#include "platform.h" + +#if defined(USE_VTX_MSP) && defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON) + +#include "build/debug.h" + +//#include "cms/cms_menu_vtx_msp.h" +#include "common/crc.h" +#include "common/log.h" +#include "config/feature.h" + +#include "drivers/vtx_common.h" +//#include "drivers/vtx_table.h" + +#include "fc/runtime_config.h" +#include "flight/failsafe.h" + +#include "io/serial.h" +#include "io/vtx_msp.h" +#include "io/vtx_control.h" +#include "io/vtx_string.h" +#include "io/vtx_smartaudio.h" +#include "io/vtx.h" +#include "io/displayport_msp_osd.h" + +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" +#include "msp/msp.h" + +//#include "pg/vtx_table.h" +#include "fc/settings.h" + +#include "rx/crsf.h" +//#include "rx/crsf_protocol.h" +#include "rx/rx.h" + +#include "telemetry/msp_shared.h" + +//static uint16_t mspConfFreq = 0; +static uint8_t mspConfBand = SETTING_VTX_BAND_DEFAULT; +static uint8_t mspConfChannel = SETTING_VTX_CHANNEL_DEFAULT; +//static uint16_t mspConfPower = 0; +static uint16_t mspConfPowerIndex = SETTING_VTX_POWER_DEFAULT; +static uint8_t mspConfPitMode = 0; +static bool mspVtxConfigChanged = false; +static timeUs_t mspVtxLastTimeUs = 0; +static bool prevLowPowerDisarmedState = false; + +static const vtxVTable_t mspVTable; // forward +static vtxDevice_t vtxMsp = { + .vTable = &mspVTable, + .capability.bandCount = VTX_MSP_TABLE_MAX_BANDS, + .capability.channelCount = VTX_MSP_TABLE_MAX_CHANNELS, + .capability.powerCount = VTX_MSP_TABLE_MAX_POWER_LEVELS, + .capability.bandNames = (char **)vtx58BandNames, + .capability.channelNames = (char **)vtx58ChannelNames, + .capability.powerNames = (char**)saPowerNames + +}; + +STATIC_UNIT_TESTED mspVtxStatus_e mspVtxStatus = MSP_VTX_STATUS_OFFLINE; +static uint8_t mspVtxPortIdentifier = 255; + +#define MSP_VTX_REQUEST_PERIOD_US (200 * 1000) // 200ms + +static bool isCrsfPortConfig(const serialPortConfig_t *portConfig) +{ + return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & FUNCTION_VTX_MSP && rxConfig()->serialrx_provider == SERIALRX_CRSF; +} + +static bool isLowPowerDisarmed(void) +{ + return (!ARMING_FLAG(ARMED) && !failsafeIsActive() && + (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS || + (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED)))); +} + +bool isVtxConfigValid(const vtxConfig_t *cfg) +{ + LOG_DEBUG(VTX, "msp isVtxConfigValid\r\n"); + for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; ++i) { + + if (cfg->vtxChannelActivationConditions[i].band || + (cfg->vtxChannelActivationConditions[i].range.startStep && cfg->vtxChannelActivationConditions[i].range.endStep) || + cfg->vtxChannelActivationConditions[i].auxChannelIndex || + cfg->vtxChannelActivationConditions[i].channel) { + return true; + } + } + + LOG_DEBUG(VTX, "msp Invalid Config!\r\n"); + return false; +} + + +void setMspVtxDeviceStatusReady(const int descriptor) +{ + LOG_DEBUG(VTX, "msp setMspVtxDeviceStatusReady\r\n"); + UNUSED(descriptor); + + mspVtxStatus = MSP_VTX_STATUS_READY; + mspVtxConfigChanged = true; +} + + +void prepareMspFrame(uint8_t *mspFrame) +{ + LOG_DEBUG(VTX, "msp PrepareMspFrame\r\n"); +/* +HDZERO parsing + fc_band_rx = msp_rx_buf[1]; + fc_channel_rx = msp_rx_buf[2]; + fc_pwr_rx = msp_rx_buf[3]; + fc_pit_rx = msp_rx_buf[4]; + fc_lp_rx = msp_rx_buf[8]; +*/ + + uint8_t pitmode = 0; + vtxCommonGetPitMode(&vtxMsp, &pitmode); + + mspFrame[0] = VTXDEV_MSP, + mspFrame[1] = vtxSettingsConfig()->band; + mspFrame[2] = vtxSettingsConfig()->channel; + mspFrame[3] = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based + mspFrame[4] = pitmode; + mspFrame[5] = 0; // Freq_L + mspFrame[6] = 0; // Freq_H + mspFrame[7] = (mspVtxStatus == MSP_VTX_STATUS_READY) ? 1 : 0; + mspFrame[8] = isLowPowerDisarmed(); + mspFrame[9] = 0; // Pitmode freq Low + mspFrame[10] = 0; // pitmod freq High + mspFrame[11] = 0; // 1 if using vtx table + mspFrame[12] = 0; // vtx table bands or 0 + mspFrame[13] = 0; // vtx table channels or 0 + mspFrame[14] = 0; // vtx table power levels or 0 +} + +static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize) +{ + + LOG_DEBUG(VTX, "msp CrsfPush\r\n"); +#ifndef USE_TELEMETRY_CRSF + UNUSED(mspCommand); + UNUSED(mspFrame); + UNUSED(mspFrameSize); +#else + sbuf_t crsfPayloadBuf; + sbuf_t *dst = &crsfPayloadBuf; + + uint8_t mspHeader[6] = {0x50, 0, mspCommand & 0xFF, (mspCommand >> 8) & 0xFF, mspFrameSize & 0xFF, (mspFrameSize >> 8) & 0xFF }; // MSP V2 over CRSF header + uint8_t mspChecksum; + + mspChecksum = crc8_dvb_s2_update(0, &mspHeader[1], 5); // first character is not checksummable + mspChecksum = crc8_dvb_s2_update(mspChecksum, mspFrame, mspFrameSize); + + uint8_t fullMspFrameSize = mspFrameSize + sizeof(mspHeader) + 1; // add 1 for msp checksum + uint8_t crsfFrameSize = CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_LENGTH_TYPE_CRC + fullMspFrameSize; + + uint8_t crsfFrame[crsfFrameSize]; + + dst->ptr = crsfFrame; + dst->end = ARRAYEND(crsfFrame); + + sbufWriteU8(dst, CRSF_SYNC_BYTE); + sbufWriteU8(dst, fullMspFrameSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); // size of CRSF frame (everything except sync and size itself) + sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); // CRSF type + sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER); // response destination is the receiver the vtx connection + sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); // origin is always this device + sbufWriteData(dst, mspHeader, sizeof(mspHeader)); + sbufWriteData(dst, mspFrame, mspFrameSize); + sbufWriteU8(dst, mspChecksum); + crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length + sbufSwitchToReader(dst, crsfFrame); + + crsfRxSendTelemetryData(); //give the FC a chance to send outstanding telemetry + crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst)); + crsfRxSendTelemetryData(); +#endif +} + +static uint16_t packetCounter = 0; + +static bool isVtxConfigChanged(void) +{ + if(mspVtxStatus == MSP_VTX_STATUS_READY) { + if (mspVtxConfigChanged == true) + return true; + + if (isLowPowerDisarmed() != prevLowPowerDisarmedState) { + LOG_DEBUG(VTX, "msp vtx config changed (lower power disarm 2)\r\n"); + mspVtxConfigChanged = true; + prevLowPowerDisarmedState = isLowPowerDisarmed(); + } + + if (mspConfPowerIndex != vtxSettingsConfig()->power) { + LOG_DEBUG(VTX, "msp vtx config changed (power 2)\r\n"); + mspVtxConfigChanged = true; + } + + if (mspConfBand != vtxSettingsConfig()->band || mspConfChannel != vtxSettingsConfig()->channel) { + LOG_DEBUG(VTX, "msp vtx config changed (band and channel 2)\r\n"); + mspVtxConfigChanged = true; + } + + return mspVtxConfigChanged; + } + + return false; +} + +static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) +{ + UNUSED(vtxDevice); + + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD); + uint8_t frame[15]; + + switch (mspVtxStatus) { + case MSP_VTX_STATUS_OFFLINE: + LOG_DEBUG(VTX, "msp MspProcess: OFFLINE\r\n"); + // wait for MSP communication from the VTX +#ifdef USE_CMS + //mspCmsUpdateStatusString(); +#endif + break; + case MSP_VTX_STATUS_READY: + LOG_DEBUG(VTX, "msp MspProcess: READY\r\n"); + // send an update if stuff has changed with 200ms period + if ((isVtxConfigChanged()) && cmp32(currentTimeUs, mspVtxLastTimeUs) >= MSP_VTX_REQUEST_PERIOD_US) { + + LOG_DEBUG(VTX, "msp-vtx: vtxInfo Changed\r\n"); + prepareMspFrame(frame); + + if (isCrsfPortConfig(portConfig)) { + mspCrsfPush(MSP_VTX_CONFIG, frame, sizeof(frame)); + } else { + mspPort_t *port = getMspOsdPort(); + if(port != NULL && port->port) { + LOG_DEBUG(VTX, "msp-vtx: mspSerialPushPort\r\n"); + int sent = mspSerialPushPort(MSP_VTX_CONFIG, frame, sizeof(frame), port, MSP_V2_NATIVE); + if (sent <= 0) { + break; + } + } + } + packetCounter++; + mspVtxLastTimeUs = currentTimeUs; + mspVtxConfigChanged = false; + +#ifdef USE_CMS + //mspCmsUpdateStatusString(); +#endif + } + break; + default: + mspVtxStatus = MSP_VTX_STATUS_OFFLINE; + break; + } + +#if 0 + DEBUG_SET(DEBUG_VTX_MSP, 0, packetCounter); + DEBUG_SET(DEBUG_VTX_MSP, 1, isCrsfPortConfig(portConfig)); + DEBUG_SET(DEBUG_VTX_MSP, 2, isLowPowerDisarmed()); +#if defined(USE_MSP_OVER_TELEMETRY) + DEBUG_SET(DEBUG_VTX_MSP, 3, isCrsfPortConfig(portConfig) ? getMspTelemetryDescriptor() : getMspSerialPortDescriptor(mspVtxPortIdentifier)); +#else + DEBUG_SET(DEBUG_VTX_MSP, 3, getMspSerialPortDescriptor(mspVtxPortIdentifier)); +#endif +#endif +} + +static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice) +{ + //LOG_DEBUG(VTX, "msp GetDeviceType\r\n"); + UNUSED(vtxDevice); + return VTXDEV_MSP; +} + +static bool vtxMspIsReady(const vtxDevice_t *vtxDevice) +{ + //LOG_DEBUG(VTX, "msp vtxIsReady: %s\r\n", (vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY) ? "Y": "N"); + return vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY; +} + +static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) +{ + LOG_DEBUG(VTX, "msp SetBandAndChannel\r\n"); + UNUSED(vtxDevice); + if (band != mspConfBand || channel != mspConfChannel) { + LOG_DEBUG(VTX, "msp vtx config changed (band and channel)\r\n"); + mspVtxConfigChanged = true; + } + mspConfBand = band; + mspConfChannel = channel; +} + +static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) +{ + LOG_DEBUG(VTX, "msp SetPowerByIndex\r\n"); + UNUSED(vtxDevice); + + if (index > 0 && (index < VTX_MSP_TABLE_MAX_POWER_LEVELS)) + { + if (index != mspConfPowerIndex) + { + LOG_DEBUG(VTX, "msp vtx config changed (power by index)\r\n"); + mspVtxConfigChanged = true; + } + mspConfPowerIndex = index; + } +} + +static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) +{ + LOG_DEBUG(VTX, "msp SetPitMode\r\n"); + UNUSED(vtxDevice); + if (onoff != mspConfPitMode) { + LOG_DEBUG(VTX, "msp vtx config changed (pitmode)\r\n"); + mspVtxConfigChanged = true; + } + mspConfPitMode = onoff; +} + +#if 0 +static void vtxMspSetFreq(vtxDevice_t *vtxDevice, uint16_t freq) +{ + UNUSED(vtxDevice); + if (freq != mspConfFreq) { + mspVtxConfigChanged = true; + } + mspConfFreq = freq; +} +#endif + + + + +static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) +{ + if (!vtxMspIsReady(vtxDevice)) { + return false; + } + + *pBand = vtxSettingsConfig()->band; + *pChannel = vtxSettingsConfig()->channel; + + //LOG_DEBUG(VTX, "msp GetBandAndChannel: %02x:%02x\r\n", vtxSettingsConfig()->band, vtxSettingsConfig()->channel); + + return true; +} + +static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) +{ + if (!vtxMspIsReady(vtxDevice)) { + return false; + } + + uint8_t power = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; + // Special case, power not set + if (power > VTX_MSP_TABLE_MAX_POWER_LEVELS) { + *pIndex = 0; + //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex); + return true; + } + + *pIndex = power; + + //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex); + return true; +} + +static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) +{ + LOG_DEBUG(VTX, "msp GetFreq\r\n"); + if (!vtxMspIsReady(vtxDevice)) { + return false; + } + + *pFreq = 5800; + return true; +} + +static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw) +{ + LOG_DEBUG(VTX, "msp GetPower\r\n"); + uint8_t powerIndex; + + if (!vtxMspGetPowerIndex(vtxDevice, &powerIndex)) { + return false; + } + + + *pIndex = powerIndex; + *pPowerMw = *pIndex; + return true; +} + +static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo) +{ + LOG_DEBUG(VTX, "msp GetOsdInfo\r\n"); + uint8_t powerIndex; + uint16_t powerMw; + uint16_t freq; + uint8_t band, channel; + + if (!vtxMspGetBandAndChannel(vtxDevice, &band, &channel)) { + return false; + } + + if (!vtxMspGetFreq(vtxDevice, &freq)) { + return false; + } + + if (!vtxMspGetPower(vtxDevice, &powerIndex, &powerMw)) { + return false; + } + + pOsdInfo->band = band; + pOsdInfo->channel = channel; + pOsdInfo->frequency = freq; + pOsdInfo->powerIndex = powerIndex; + pOsdInfo->powerMilliwatt = powerMw; + pOsdInfo->bandLetter = vtx58BandNames[band][0]; + pOsdInfo->bandName = vtx58BandNames[band]; + pOsdInfo->channelName = vtx58ChannelNames[channel]; + pOsdInfo->powerIndexLetter = '0' + powerIndex; + return true; +} + + +bool vtxMspInit(void) +{ + LOG_DEBUG(VTX, "msp %s\r\n", __FUNCTION__); + // don't bother setting up this device if we don't have MSP vtx enabled + // Port is shared with MSP_OSD + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD); + if (!portConfig) { + return false; + } + + mspVtxPortIdentifier = portConfig->identifier; + + // XXX Effect of USE_VTX_COMMON should be reviewed, as following call to vtxInit will do nothing if vtxCommonSetDevice is not called. +#if defined(USE_VTX_COMMON) + vtxCommonSetDevice(&vtxMsp); +#endif + + mspConfBand = vtxSettingsConfig()->band; + mspConfChannel = vtxSettingsConfig()->channel; + mspConfPowerIndex = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based + vtxCommonGetPitMode(&vtxMsp, &mspConfPitMode); + + vtxInit(); + + return true; +} + +static const vtxVTable_t mspVTable = { + .process = vtxMspProcess, + .getDeviceType = vtxMspGetDeviceType, + .isReady = vtxMspIsReady, + .setBandAndChannel = vtxMspSetBandAndChannel, + .setPowerByIndex = vtxMspSetPowerByIndex, + .setPitMode = vtxMspSetPitMode, + //.setFrequency = vtxMspSetFreq, + .getBandAndChannel = vtxMspGetBandAndChannel, + .getPowerIndex = vtxMspGetPowerIndex, + .getFrequency = vtxMspGetFreq, + //.getStatus = vtxMspGetStatus, + .getPower = vtxMspGetPower, + //.serializeCustomDeviceStatus = NULL, + .getOsdInfo = vtxMspGetOsdInfo, +}; + +static mspResult_e mspVtxProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) +{ + //LOG_DEBUG(VTX, "msp VTX_MSP_PROCESS\r\n"); + UNUSED(mspPostProcessFn); + + sbuf_t *dst = &reply->buf; + sbuf_t *src = &cmd->buf; + + const unsigned int dataSize = sbufBytesRemaining(src); + UNUSED(dst); + UNUSED(src); + + // Start initializing the reply message + reply->cmd = cmd->cmd; + reply->result = MSP_RESULT_ACK; + + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_MSP) { + LOG_DEBUG(VTX, "msp wrong vtx\r\n"); + return MSP_RESULT_ERROR; + } + + switch (cmd->cmd) + { + case MSP_VTXTABLE_BAND: + { + LOG_DEBUG(VTX, "msp MSP_VTXTABLE_BAND\r\n"); + uint8_t deviceType = vtxCommonGetDeviceType(vtxDevice); + if (deviceType == VTXDEV_MSP) + { + /* + char bandName[MSP_VTX_TABLE_BAND_NAME_LENGTH + 1]; + memset(bandName, 0, MSP_VTX_TABLE_BAND_NAME_LENGTH + 1); + uint16_t frequencies[MSP_VTX_TABLE_MAX_CHANNELS]; + const uint8_t band = sbufReadU8(src); + const uint8_t bandNameLength = sbufReadU8(src); + for (int i = 0; i < bandNameLength; i++) { + const char nameChar = sbufReadU8(src); + if (i < MSP_VTX_TABLE_BAND_NAME_LENGTH) { + bandName[i] = toupper(nameChar); + } + } + const char bandLetter = toupper(sbufReadU8(src)); + const bool isFactoryBand = (bool)sbufReadU8(src); + const uint8_t channelCount = sbufReadU8(src); + for (int i = 0; i < channelCount; i++) + { + const uint16_t frequency = sbufReadU16(src); + if (i < vtxTableConfig()->channels) + { + frequencies[i] = frequency; + } + } + */ + + setMspVtxDeviceStatusReady(1); + } + break; + } + case MSP_VTXTABLE_POWERLEVEL: + { + LOG_DEBUG(VTX, "msp MSP_VTXTABLE_POWERLEVEL\r\n"); + + /* + char powerLevelLabel[VTX_TABLE_POWER_LABEL_LENGTH + 1]; + memset(powerLevelLabel, 0, VTX_TABLE_POWER_LABEL_LENGTH + 1); + const uint8_t powerLevel = sbufReadU8(src); + const uint16_t powerValue = sbufReadU16(src); + const uint8_t powerLevelLabelLength = sbufReadU8(src); + for (int i = 0; i < powerLevelLabelLength; i++) + { + const char labelChar = sbufReadU8(src); + if (i < VTX_TABLE_POWER_LABEL_LENGTH) + { + powerLevelLabel[i] = toupper(labelChar); + } + } + */ + setMspVtxDeviceStatusReady(1); + } + break; + case MSP_VTX_CONFIG: + { + LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG received\r\n"); + LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG VTXDEV_MSP\r\n"); + uint8_t pitmode = 0; + vtxCommonGetPitMode(vtxDevice, &pitmode); + + // VTXDEV_MSP, + sbufWriteU8(dst, VTXDEV_MSP); + // band; + sbufWriteU8(dst, vtxSettingsConfig()->band); + // channel; + sbufWriteU8(dst, vtxSettingsConfig()->channel); + // power; // index based + sbufWriteU8(dst, vtxSettingsConfig()->power); + // pit mode; + // Freq_L + sbufWriteU8(dst, 0); + // Freq_H + sbufWriteU8(dst, 0); + // vtx status + sbufWriteU8(dst, 1); + // lowPowerDisarm + + sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm); + // Pitmode freq Low + sbufWriteU8(dst, 0); + // pitmod freq High + sbufWriteU8(dst, 0); + // 1 if using vtx table + sbufWriteU8(dst, 0); + // vtx table bands or 0 + sbufWriteU8(dst, 0); + // vtx table channels or 0 + sbufWriteU8(dst, 0); + + setMspVtxDeviceStatusReady(1); + break; + } + case MSP_SET_VTX_CONFIG: + LOG_DEBUG(VTX, "msp MSP_SET_VTX_CONFIG\r\n"); + if (dataSize == 15) + { + if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) + { + for (int i = 0; i < 15; ++i) + { + uint8_t data = sbufReadU8(src); + switch (i) + { + case 1: + vtxSettingsConfigMutable()->band = data; + break; + case 2: + vtxSettingsConfigMutable()->channel = data; + break; + case 3: + vtxSettingsConfigMutable()->power = data; + break; + case 4: + vtxCommonSetPitMode(vtxDevice, data); + break; + case 7: + // vtx ready + break; + case 8: + vtxSettingsConfigMutable()->lowPowerDisarm = data; + break; + } + } + } + + setMspVtxDeviceStatusReady(1); + break; + } + LOG_DEBUG(VTX, "msp MSP_RESULT_ERROR\r\n"); + return MSP_RESULT_ERROR; + default: + // debug[1]++; + // debug[2] = cmd->cmd; + if(cmd->cmd != MSP_STATUS && cmd->cmd != MSP_STATUS_EX && cmd->cmd != MSP_RC) { + LOG_DEBUG(VTX, "msp default: %02x\r\n", cmd->cmd); + } + reply->result = MSP_RESULT_ERROR; + break; + } + + // Process DONT_REPLY flag + if (cmd->flags & MSP_FLAG_DONT_REPLY) + { + reply->result = MSP_RESULT_NO_REPLY; + } + + return reply->result; +} + +void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn) +{ + UNUSED(mspProcessCommandFn); + // Check if VTX is ready + /* + if (mspVtxStatus != MSP_VTX_STATUS_READY) { + LOG_DEBUG(VTX, "msp VTX NOT READY, skipping\r\n"); + return; + } + */ + + mspPort_t *port = getMspOsdPort(); + + if(port) { + mspSerialProcessOnePort(port, MSP_SKIP_NON_MSP_DATA, mspVtxProcessMspCommand); + } + +} + + +#endif \ No newline at end of file diff --git a/src/main/io/vtx_msp.h b/src/main/io/vtx_msp.h new file mode 100644 index 00000000000..30ca245fed3 --- /dev/null +++ b/src/main/io/vtx_msp.h @@ -0,0 +1,53 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include + +#include "build/build_config.h" + +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + +typedef enum { + // Offline - device hasn't responded yet + MSP_VTX_STATUS_OFFLINE = 0, + MSP_VTX_STATUS_READY, +} mspVtxStatus_e; + +typedef struct mspPowerTable_s { + int mW; // rfpower + int16_t dbi; // valueV1 +} mspPowerTable_t; + +#define VTX_MSP_TABLE_MAX_BANDS 5 // default freq table has 5 bands +#define VTX_MSP_TABLE_MAX_CHANNELS 8 // and eight channels +#define VTX_MSP_TABLE_MAX_POWER_LEVELS 5 //max of VTX_TRAMP_POWER_COUNT, VTX_SMARTAUDIO_POWER_COUNT and VTX_RTC6705_POWER_COUNT +#define VTX_MSP_TABLE_CHANNEL_NAME_LENGTH 1 +#define VTX_MSP_TABLE_BAND_NAME_LENGTH 8 +#define VTX_MSP_TABLE_POWER_LABEL_LENGTH 3 + + +bool vtxMspInit(void); +void setMspVtxDeviceStatusReady(const int descriptor); +void prepareMspFrame(uint8_t *mspFrame); + +void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn); diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index f94b04fe1e1..b44037f2f88 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -217,6 +217,9 @@ #define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility #define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility +#define MSP_VTXTABLE_BAND 137 //out message vtxTable band/channel data - needed by msp vtx +#define MSP_VTXTABLE_POWERLEVEL 138 //out message vtxTable powerLevel data - neede by msp vtx + // // OSD specific // diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 5fa10e819e0..0aaecba2d7d 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -530,7 +530,7 @@ int mspSerialPushPort(uint16_t cmd, const uint8_t *data, int datalen, mspPort_t return mspSerialEncode(mspPort, &push, version); } -int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen) +int mspSerialPushVersion(uint8_t cmd, const uint8_t *data, int datalen, mspVersion_e version) { int ret = 0; @@ -545,11 +545,16 @@ int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen) continue; } - ret = mspSerialPushPort(cmd, data, datalen, mspPort, MSP_V1); + ret = mspSerialPushPort(cmd, data, datalen, mspPort, version); } return ret; // return the number of bytes written } +int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen) +{ + return mspSerialPushVersion(cmd, data, datalen, MSP_V1); +} + uint32_t mspSerialTxBytesFree(serialPort_t *port) { return serialTxBytesFree(port); diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 8132f077a09..67487606da0 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -107,5 +107,6 @@ void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); int mspSerialPushPort(uint16_t cmd, const uint8_t *data, int datalen, mspPort_t *mspPort, mspVersion_e version); int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen); +int mspSerialPushVersion(uint8_t cmd, const uint8_t *data, int datalen, mspVersion_e version); uint32_t mspSerialTxBytesFree(serialPort_t *port); mspPort_t * mspSerialPortFind(const struct serialPort_s *serialPort); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b5d9683038c..fb4c1128b38 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1311,7 +1311,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n // Climb to safe altitude and turn to correct direction // 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. - if (STATE(FIXED_WING_LEGACY)) { // CR96 + if (STATE(FIXED_WING_LEGACY)) { tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM; setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); } else { @@ -1425,7 +1425,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 ((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); // CR96 + updateClimbRateToAltitudeController(0, 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); @@ -1444,7 +1444,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na } fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER); - setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); // CR96 + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); return NAV_FSM_EVENT_NONE; } @@ -1486,7 +1486,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); } - updateClimbRateToAltitudeController(-descentVelLimited, 0, ROC_TO_ALT_CONSTANT); // CR96 + updateClimbRateToAltitudeController(-descentVelLimited, 0, ROC_TO_ALT_CONSTANT); return NAV_FSM_EVENT_NONE; } @@ -1509,7 +1509,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation UNUSED(previousState); if (STATE(ALTITUDE_CONTROL)) { - updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT); // FIXME CR96 + updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT); // FIXME } // Prevent I-terms growing when already landed @@ -1711,7 +1711,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { // Adjust altitude to waypoint setting - setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z); // CR96 + setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z); + posControl.wpAltitudeReached = isWaypointAltitudeReached(); if (posControl.wpAltitudeReached) { @@ -2816,7 +2817,7 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag // Z-position if ((useMask & NAV_POS_UPDATE_Z) != 0) { - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller CR96 + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller posControl.desiredState.pos.z = pos->z; } @@ -2907,31 +2908,31 @@ bool isFlightDetected(void) /*----------------------------------------------------------- * Z-position controller *-----------------------------------------------------------*/ -void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode) // CR96 +void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode) { -#define MIN_TARGET_CLIMB_RATE 100.0f // CR96 +#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; - // CR96 + if (mode != ROC_TO_ALT_RESET && desiredClimbRate) { - // DEBUG_SET(DEBUG_ALWAYS, 0, posControl.desiredState.pos.z); - /* ROC_TO_ALT_CONSTANT - constant climb rate always - * ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min value when altitude reached + /* 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) { - int8_t direction = desiredClimbRate > 0 ? 1 : -1; - float absClimbRate = fabsf(desiredClimbRate); - uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate; - float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude, - 0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate); + 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); } - // DEBUG_SET(DEBUG_ALWAYS, 1, desiredClimbRate); + /* * 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 @@ -2967,7 +2968,6 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt } lastUpdateTimeUs = currentTimeUs; - // CR96 } static void resetAltitudeController(bool useTerrainFollowing) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 5a7249c7e79..da324027405 100644 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -117,13 +117,13 @@ bool adjustFixedWingAltitudeFromRCInput(void) if (rcAdjustment) { // set velocity proportional to stick movement float rcClimbRate = -rcAdjustment * navConfig()->general.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband); - updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT); // CR96 + updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT); return true; } 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); // CR96 + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); } return false; } @@ -196,17 +196,14 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) 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); // CR96 + float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle); - // CR96 - // Reduce max allowed climb pitch if performing loiter - // DEBUG_SET(DEBUG_ALWAYS, 2, 101); + // Reduce max allowed climb pitch if performing loiter (stall prevention) if (needToCalculateCircularLoiter) { maxClimbDeciDeg = maxClimbDeciDeg * 0.67f; - // DEBUG_SET(DEBUG_ALWAYS, 2, 201); } - // CR96 + // 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); @@ -816,7 +813,8 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs) rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; if (posControl.flags.estAltStatus >= EST_USABLE) { - updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000, ROC_TO_ALT_TARGET); // CR96 + // target min descent rate 10m above takeoff altitude + updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000.0f, ROC_TO_ALT_TARGET); 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)); diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index fdce4c32b54..19c97f64595 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -113,15 +113,15 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) { // Calculate min and max throttle boundaries (to compensate for integral windup) - const int16_t thrCorrectionMin = getThrottleIdleValue() - currentBatteryProfile->nav.mc.hover_throttle; // CR102 - const int16_t thrCorrectionMax = motorConfig()->maxthrottle - currentBatteryProfile->nav.mc.hover_throttle; // CR102 + const int16_t thrCorrectionMin = getThrottleIdleValue() - currentBatteryProfile->nav.mc.hover_throttle; + const int16_t thrCorrectionMax = motorConfig()->maxthrottle - currentBatteryProfile->nav.mc.hover_throttle; float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrCorrectionMin, thrCorrectionMax, 0); - // CR102 + int16_t rcThrottleCorrection = pt1FilterApply4(&altholdThrottleFilterState, velocity_controller, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros)); rcThrottleCorrection = constrain(rcThrottleCorrection, thrCorrectionMin, thrCorrectionMax); - // CR102 - posControl.rcAdjustment[THROTTLE] = constrain(currentBatteryProfile->nav.mc.hover_throttle + rcThrottleCorrection, getThrottleIdleValue(), motorConfig()->maxthrottle); // CR102 + + posControl.rcAdjustment[THROTTLE] = constrain(currentBatteryProfile->nav.mc.hover_throttle + rcThrottleCorrection, getThrottleIdleValue(), motorConfig()->maxthrottle); } bool adjustMulticopterAltitudeFromRCInput(void) @@ -132,11 +132,11 @@ 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); // CR96 + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); posControl.desiredState.pos.z = altTarget; } else { - updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT); // CR96 + updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT); } // In surface tracking we always indicate that we're adjusting altitude @@ -158,14 +158,14 @@ bool adjustMulticopterAltitudeFromRCInput(void) rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband); } - updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT); // CR96 + updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT); return true; } 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); // CR96 + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); } return false; @@ -210,7 +210,7 @@ void resetMulticopterAltitudeController(void) navPidReset(&posControl.pids.vel[Z]); navPidReset(&posControl.pids.surface); - posControl.rcAdjustment[THROTTLE] = currentBatteryProfile->nav.mc.hover_throttle; // CR102 + posControl.rcAdjustment[THROTTLE] = currentBatteryProfile->nav.mc.hover_throttle; posControl.desiredState.vel.z = posToUse->vel.z; // Gradually transition from current climb @@ -937,33 +937,27 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) /* Attempt to stabilise */ rcCommand[YAW] = 0; - // CR102 rcCommand[ROLL] = 0; rcCommand[PITCH] = 0; - rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle == SETTING_FAILSAFE_THROTTLE_DEFAULT ? - 1000 + 0.9 * (currentBatteryProfile->nav.mc.hover_throttle - 1000) : currentBatteryProfile->failsafe_throttle; - // CR102 + rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; - if ((posControl.flags.estAltStatus < EST_USABLE)) { - /* Sensors has gone haywire, attempt to land regardless */ + /* Altitude sensors gone haywire, attempt to land regardless */ + if (posControl.flags.estAltStatus < EST_USABLE) { if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) { rcCommand[THROTTLE] = getThrottleIdleValue(); } - // else { // CR102 - // rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; - // } - return; } - // Normal sensor data + // Normal sensor data available, use controlled landing descent if (posControl.flags.verticalPositionDataNew) { const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; // Check if last correction was not too long ago if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { - updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500, ROC_TO_ALT_TARGET); // CR96 + // target min descent rate 5m above takeoff altitude + updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500.0f, ROC_TO_ALT_TARGET); updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate); updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate); } @@ -982,9 +976,6 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) // Hold position if possible if ((posControl.flags.estPosStatus >= EST_USABLE)) { applyMulticopterPositionController(currentTimeUs); - // } else { // CR102 - // rcCommand[ROLL] = 0; - // rcCommand[PITCH] = 0; } } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index df3752ef707..c6db96eba28 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -62,7 +62,7 @@ typedef enum { typedef enum { ROC_TO_ALT_RESET, ROC_TO_ALT_CONSTANT, - ROC_TO_ALT_TARGET // CR96 + ROC_TO_ALT_TARGET } climbRateToAltitudeControllerMode_e; typedef enum { @@ -461,7 +461,7 @@ void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFla 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 updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode); // CR96 +void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode); bool isNavHoldPositionActive(void); bool isLastMissionWaypoint(void); diff --git a/src/main/target/AOCODARCF405AIO/target.c b/src/main/target/AOCODARCF405AIO/target.c index 406098702b7..9548cb179a2 100644 --- a/src/main/target/AOCODARCF405AIO/target.c +++ b/src/main/target/AOCODARCF405AIO/target.c @@ -35,7 +35,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM1, CH1, PB1, TIM_USE_LED, 0, 0), // LED + DEF_TIM(TIM1, CH3N, PB1, TIM_USE_LED, 0, 0), // LED }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/AXISFLYINGF7PRO/target.c b/src/main/target/AXISFLYINGF7PRO/target.c index 532e668b3a1..fc46a9c0b8f 100644 --- a/src/main/target/AXISFLYINGF7PRO/target.c +++ b/src/main/target/AXISFLYINGF7PRO/target.c @@ -25,8 +25,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S3 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 diff --git a/src/main/target/AXISFLYINGF7PRO/target.h b/src/main/target/AXISFLYINGF7PRO/target.h index 1d8c3663bb4..4a4b95657ec 100644 --- a/src/main/target/AXISFLYINGF7PRO/target.h +++ b/src/main/target/AXISFLYINGF7PRO/target.h @@ -170,6 +170,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define MAX_PWM_OUTPUT_PORTS 4 +#define MAX_PWM_OUTPUT_PORTS 8 #define USE_DSHOT #define USE_ESC_SENSOR diff --git a/src/main/target/FOXEERH743/target.h b/src/main/target/FOXEERH743/target.h index 72fef026b15..ffb582ea9ad 100644 --- a/src/main/target/FOXEERH743/target.h +++ b/src/main/target/FOXEERH743/target.h @@ -135,7 +135,7 @@ // *************** ADC ***************************** #define USE_ADC -#define ADC_INSTANCE ADC1 +#define ADC_INSTANCE ADC3 #define ADC_CHANNEL_1_PIN PC3 #define ADC_CHANNEL_2_PIN PC5 diff --git a/src/main/target/KAKUTEH7WING/config.c b/src/main/target/KAKUTEH7WING/config.c index 6ec606bf404..1b2670c9526 100644 --- a/src/main/target/KAKUTEH7WING/config.c +++ b/src/main/target/KAKUTEH7WING/config.c @@ -22,12 +22,24 @@ #include "fc/fc_msp_box.h" #include "fc/config.h" +#include "rx/rx.h" + #include "io/piniobox.h" #include "drivers/serial.h" #include "io/serial.h" void targetConfiguration(void) { + serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_GPS; + serialConfigMutable()->portConfigs[1].gps_baudrateIndex = BAUD_115200; + + serialConfigMutable()->portConfigs[5].functionMask = FUNCTION_RX_SERIAL; + rxConfigMutable()->receiverType = RX_TYPE_SERIAL; + rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; + + serialConfigMutable()->portConfigs[6].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[6].msp_baudrateIndex = BAUD_115200; + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; pinioBoxConfigMutable()->permanentId[2] = BOX_PERMANENT_ID_USER3; diff --git a/src/main/target/KAKUTEH7WING/target.h b/src/main/target/KAKUTEH7WING/target.h index 7a5381b5efc..72cf6b68069 100644 --- a/src/main/target/KAKUTEH7WING/target.h +++ b/src/main/target/KAKUTEH7WING/target.h @@ -186,7 +186,7 @@ #define USE_LED_STRIP #define WS2811_PIN PA15 -#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_GPS) #define CURRENT_METER_SCALE 3660 // 36.6 #define VBAT_SCALE_DEFAULT 1818 // 18.18 #define VBAT_SCALE_DEFAULT2 1100 // 11.0 diff --git a/src/main/target/NEUTRONRCF435MINI/target.c b/src/main/target/NEUTRONRCF435MINI/target.c index e964c19156e..995bf8f3d1a 100644 --- a/src/main/target/NEUTRONRCF435MINI/target.c +++ b/src/main/target/NEUTRONRCF435MINI/target.c @@ -29,7 +29,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TMR1, CH1, PA8, TIM_USE_ANY |TIM_USE_LED, 0,7), // PWM1 - LED MCO1 DMA1 CH2 DEF_TIM(TMR4, CH1, PB6, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,0), // motor1 DMA2 CH7 - DEF_TIM(TMR1, CH3, PA10, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,2), // motor2 DMA2 CH6 + DEF_TIM(TMR4, CH2, PB7, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,2), // motor2 DMA2 CH6 DEF_TIM(TMR2, CH4, PA3, TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,1), // motor3 DMA2 CH5 DEF_TIM(TMR3, CH4, PB1, TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,3), // motor4 DMA2 CH4 diff --git a/src/main/target/NEUTRONRCF435MINI/target.h b/src/main/target/NEUTRONRCF435MINI/target.h index 42cddf5681e..27fc03ae5e1 100644 --- a/src/main/target/NEUTRONRCF435MINI/target.h +++ b/src/main/target/NEUTRONRCF435MINI/target.h @@ -137,7 +137,7 @@ #define USE_USB_DETECT #define USE_UART1 -#define UART1_RX_PIN PB7 +#define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 #define USE_UART2 diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c index eb164c42939..23e77bcde04 100644 --- a/src/main/target/SITL/sim/realFlight.c +++ b/src/main/target/SITL/sim/realFlight.c @@ -403,6 +403,9 @@ static void exchangeData(void) constrainToInt16(north.y * 16000.0f), constrainToInt16(north.z * 16000.0f) ); + + free(rfValues.m_currentAircraftStatus); + free(response); } static void* soapWorker(void* arg) @@ -412,9 +415,9 @@ static void* soapWorker(void* arg) { if (!isInitalised) { startRequest("RestoreOriginalControllerDevice", "12"); - endRequest(); + free(endRequest()); startRequest("InjectUAVControllerInterface", "12"); - endRequest(); + free(endRequest()); exchangeData(); ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); diff --git a/src/main/target/SITL/sim/simple_soap_client.c b/src/main/target/SITL/sim/simple_soap_client.c index f715e10ace0..e6400ae3b14 100644 --- a/src/main/target/SITL/sim/simple_soap_client.c +++ b/src/main/target/SITL/sim/simple_soap_client.c @@ -93,6 +93,9 @@ void soapClientSendRequestVa(soap_client_t *client, const char* action, const ch } send(client->sockedFd, request, strlen(request), 0); + + free(requestBody); + free(request); } void soapClientSendRequest(soap_client_t *client, const char* action, const char *fmt, ...) diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 5ec93c583c2..78f5022a9eb 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -70,6 +70,9 @@ #define USE_RANGEFINDER_FAKE #define USE_RX_SIM +#define USE_MSP_OSD +#define USE_OSD + #undef USE_DASHBOARD #undef USE_GYRO_KALMAN // Strange behaviour under x86/x64 ?!? @@ -87,9 +90,6 @@ #undef USE_TELEMETRY_JETIEXBUS #undef USE_TELEMETRY_SRXL #undef USE_TELEMETRY_GHST -#undef USE_VTX_COMMON -#undef USE_VTX_CONTROL -#undef USE_VTX_SMARTAUDIO #undef USE_VTX_TRAMP #undef USE_CAMERA_CONTROL #undef USE_BRUSHED_ESC_AUTODETECT diff --git a/src/main/target/SPEEDYBEEF7V3/target.h b/src/main/target/SPEEDYBEEF7V3/target.h index f8705f97da7..7c1d565c377 100644 --- a/src/main/target/SPEEDYBEEF7V3/target.h +++ b/src/main/target/SPEEDYBEEF7V3/target.h @@ -149,6 +149,7 @@ // ********** Optiical Flow adn Lidar ************** #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP +#define RANGEFINDER_I2C_BUS BUS_I2C1 #define USE_OPFLOW #define USE_OPFLOW_MSP diff --git a/src/main/target/common.h b/src/main/target/common.h index b9ac2bb8558..ce6c00e7e28 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -166,6 +166,7 @@ #define USE_VTX_CONTROL #define USE_VTX_SMARTAUDIO #define USE_VTX_TRAMP +#define USE_VTX_MSP #define USE_PROGRAMMING_FRAMEWORK #define USE_CLI_BATCH