diff --git a/docs/Rx.md b/docs/Rx.md index c52fe151571..43f9d852716 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -200,7 +200,7 @@ Note: * It is necessary to update `MSP_SET_RAW_RC` at 5Hz or faster. * `MSP_SET_RAW_RC` uses the defined RC channel map * `MSP_RC` returns `AERT` regardless of channel map -* You can combine "real" RC radio and MSP RX by compiling custom firmware with `USE_MSP_RC_OVERRIDE` defined. Then use `msp_override_channels` to set the channels to be overridden. +* You can combine "real" RC radio and MSP RX by using `msp_override_channels` to set the channels to be overridden. * The [wiki Remote Control and Management article](https://github.com/iNavFlight/inav/wiki/INAV-Remote-Management,-Control-and-Telemetry) provides more information, including links to 3rd party projects that exercise `MSP_SET_RAW_RC` and `USE_MSP_RC_OVERRIDE` ## SIM (SITL) Joystick diff --git a/docs/Settings.md b/docs/Settings.md index d9b72a058e2..89c7bf1c700 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3034,7 +3034,7 @@ If enabled, motor will stop when throttle is low on this mixer_profile ### msp_override_channels -Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. +Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires the `MSP RC Override` flight mode. | Default | Min | Max | | --- | --- | --- | diff --git a/docs/VTOL.md b/docs/VTOL.md index 78f2a4f330b..025a7711c68 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -42,7 +42,7 @@ We highly value your feedback as it plays a crucial role in the development and 5. *(Optional)* **Automated Switching (RTH):** - Optionally, set up automated switching in case of failsafe. -# STEP0: Load parameter preset/templates +# STEP 0: Load parameter preset/templates Find a working diff file if you can and start from there. If not, select keep current settings and apply following parameter in cli but read description about which one to apply. ``` @@ -113,7 +113,7 @@ set mc_iterm_relax = RPY save ``` -# STEP1: Configuring as a normal fixed-wing in Profile 1 +# STEP 1: Configuring as a normal fixed-wing in Profile 1 1. **Select the fisrt Mixer Profile and PID Profile:** - In the CLI, switch to the mixer_profile and pid_profile you wish to set first. You can also switch mixer_profile/pid_profile through gui if with aforementioned presets loaded. @@ -133,7 +133,7 @@ save ![Alt text](Screenshots/mixerprofile_fw_mixer.png) -# STEP2: Configuring as a Multi-Copter in Profile 2 +# STEP 2: Configuring as a Multi-Copter in Profile 2 1. **Switch to Another Mixer Profile with PID Profile:** - In the CLI, switch to another mixer_profile along with the appropriate pid_profile. You can also switch mixer_profile/pid_profile through gui if with aforementioned presets loaded. @@ -161,7 +161,7 @@ save - Configure mixer ROLL/YAW mixing according to tail_sitting orientation in the tail_sitting MC mode. YAW axis is the trust axis. - Conduct a bench test and see the orientation of the model changes in inav-configurator setup tab -# STEP3: Mode Tab Settings: +# STEP 3: Mode Tab Settings: ### We recommend using an 3-pos switch on you radio to activate these modes, So pilot can jump in or bell out at any moment. ### Here is a example, in the bottom of inav-configurator Modes tab: ![Alt text](Screenshots/mixer_profile.png) @@ -177,7 +177,7 @@ save Conduct a bench test on the model (without props attached). The model can now switch between fixed-wing and multi-copter modes while armed. Furthermore, it is capable of mid-air switching, resulting in an immediate stall upon entering fixed-wing profile -# STEP4: Tilting Servo Setup (Recommended) +# STEP 4: Tilting Servo Setup (Recommended) ### Setting up the tilting servos to operate correclty is crucial for correct yaw control of the craft. Using the default setup works, but will most likely result in your craft crawling forward with evey yaw input. The steps below describe how you can fine-tune the tilting servos such as to obtian the desired result. @@ -229,7 +229,7 @@ Optional Setup Step for Tilt Servos: If you have set up the mixer as suggested in STEP1 and STEP2, you may have to deal with negative values for the mixer. You may wish to reverese a servo so that you don't have to deal with the negative signs. In that case, you may have to adjust the MIN and MAX values from point 4 again, so that your tilt servos are operating correctly. Check the operation of the servos once again for the YAW control in multicopter/tricipter mode as well as the horixontal position of the tilt servos in fixed-wing mode. -# STEP5: Transition Mixing (Multi-Rotor Profile)(Recommended) +# STEP 5: Transition Mixing (Multi-Rotor Profile)(Recommended) ### Transition Mixing is typically useful in multi-copter profile to gain airspeed in prior to entering the fixed-wing profile. When the `MIXER TRANSITION` mode is activated, the associated motor or servo will move according to your configured Transition Mixing. Please note that transition input is disabled when a navigation mode is activated. The use of Transition Mixing is necessary to enable additional features such as VTOL RTH with out stalling. diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b4b6fce2824..23b400aaf37 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1737,6 +1737,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; #endif + + case MSP2_COMMON_GET_RADAR_GPS: + for (uint8_t i = 0; i < RADAR_MAX_POIS; i++){ + sbufWriteDataSafe(dst, &radar_pois[i].gps, sizeof(gpsLocation_t)); + } + break; + default: return false; } @@ -1820,7 +1827,7 @@ static mspResult_e mspFcGeozoneVerteciesOutCommand(sbuf_t *dst, sbuf_t *src) return MSP_RESULT_ACK; } else { return MSP_RESULT_ERROR; - } + } } else { return MSP_RESULT_ERROR; } @@ -3431,13 +3438,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) { return MSP_RESULT_ERROR; } - + geozoneResetVertices(geozoneId, -1); - geoZonesConfigMutable(geozoneId)->type = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->type = sbufReadU8(src); geoZonesConfigMutable(geozoneId)->shape = sbufReadU8(src); geoZonesConfigMutable(geozoneId)->minAltitude = sbufReadU32(src); - geoZonesConfigMutable(geozoneId)->maxAltitude = sbufReadU32(src); - geoZonesConfigMutable(geozoneId)->isSealevelRef = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->maxAltitude = sbufReadU32(src); + geoZonesConfigMutable(geozoneId)->isSealevelRef = sbufReadU8(src); geoZonesConfigMutable(geozoneId)->fenceAction = sbufReadU8(src); geoZonesConfigMutable(geozoneId)->vertexCount = sbufReadU8(src); } else { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 74afae23d94..17f44e3a9bf 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -770,7 +770,7 @@ groups: field: halfDuplex table: tristate - name: msp_override_channels - description: "Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode." + description: "Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires the `MSP RC Override` flight mode." default_value: 0 field: mspOverrideChannels condition: USE_MSP_RC_OVERRIDE diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h index 8784b253114..411fbecf046 100644 --- a/src/main/msp/msp_protocol_v2_common.h +++ b/src/main/msp/msp_protocol_v2_common.h @@ -19,21 +19,23 @@ #define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16)) #define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting #define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting - + #define MSP2_COMMON_MOTOR_MIXER 0x1005 #define MSP2_COMMON_SET_MOTOR_MIXER 0x1006 - + #define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..). #define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings - + #define MSP2_COMMON_SERIAL_CONFIG 0x1009 #define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A - -// radar commands + +// radar commands #define MSP2_COMMON_SET_RADAR_POS 0x100B //SET radar position information #define MSP2_COMMON_SET_RADAR_ITD 0x100C //SET radar information to display #define MSP2_COMMON_SET_MSP_RC_LINK_STATS 0x100D //in message Sets the MSP RC stats #define MSP2_COMMON_SET_MSP_RC_INFO 0x100E //in message Sets the MSP RC info +#define MSP2_COMMON_GET_RADAR_GPS 0x100F //get radar position for other planes + #define MSP2_BETAFLIGHT_BIND 0x3000 diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 69777a0390c..c5136e3b7ae 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -45,6 +45,7 @@ enum { CRSF_FRAME_GPS_PAYLOAD_SIZE = 15, CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, + CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE = 2, CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6, @@ -86,6 +87,7 @@ typedef enum { CRSF_FRAMETYPE_GPS = 0x02, CRSF_FRAMETYPE_VARIO_SENSOR = 0x07, CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, + CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09, CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, CRSF_FRAMETYPE_ATTITUDE = 0x1E, diff --git a/src/main/target/DAKEFPVF722X8/target.h b/src/main/target/DAKEFPVF722X8/target.h index 240ebf408ca..fc8ac731b84 100644 --- a/src/main/target/DAKEFPVF722X8/target.h +++ b/src/main/target/DAKEFPVF722X8/target.h @@ -105,6 +105,10 @@ #define UART1_RX_PIN PB6 #define UART1_TX_PIN PB7 +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PD8 + #define USE_UART4 #define UART4_RX_PIN PC11 #define UART4_TX_PIN PC10 @@ -117,7 +121,7 @@ #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 6 #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_CRSF @@ -153,14 +157,13 @@ #define RSSI_ADC_CHANNEL ADC_CHN_2 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 #define ADC1_DMA_STREAM DMA2_Stream3 +#define VBAT_SCALE_DEFAULT 1600 -// unkonw #define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_LED_STRIP | FEATURE_GPS) #define USE_LED_STRIP #define WS2811_PIN PA0 -// unkonw #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/common.h b/src/main/target/common.h index af0de4bf30f..45eb12ac4bc 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -155,7 +155,7 @@ #define USE_TELEMETRY_JETIEXBUS // These are rather exotic serial protocols #define USE_RX_MSP -//#define USE_MSP_RC_OVERRIDE +#define USE_MSP_RC_OVERRIDE #define USE_SERIALRX_CRSF #define USE_SERIAL_PASSTHROUGH #define NAV_MAX_WAYPOINTS 120 diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 8b7f289db20..3a804485f20 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -258,6 +258,33 @@ static void crsfFrameBatterySensor(sbuf_t *dst) crsfSerialize8(dst, batteryRemainingPercentage); } +const int32_t ALT_MIN_DM = 10000; +const int32_t ALT_THRESHOLD_DM = 0x8000 - ALT_MIN_DM; +const int32_t ALT_MAX_DM = 0x7ffe * 10 - 5; + +/* +0x09 Barometer altitude and vertical speed +Payload: +uint16_t altitude_packed ( dm - 10000 ) +*/ +static void crsfBarometerAltitude(sbuf_t *dst) +{ + int32_t altitude_dm = lrintf(getEstimatedActualPosition(Z) / 10); + uint16_t altitude_packed; + if (altitude_dm < -ALT_MIN_DM) { + altitude_packed = 0; + } else if (altitude_dm > ALT_MAX_DM) { + altitude_packed = 0xfffe; + } else if (altitude_dm < ALT_THRESHOLD_DM) { + altitude_packed = altitude_dm + ALT_MIN_DM; + } else { + altitude_packed = ((altitude_dm + 5) / 10) | 0x8000; + } + sbufWriteU8(dst, CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_BAROMETER_ALTITUDE); + crsfSerialize16(dst, altitude_packed); +} + typedef enum { CRSF_ACTIVE_ANTENNA1 = 0, CRSF_ACTIVE_ANTENNA2 = 1 @@ -415,6 +442,7 @@ typedef enum { CRSF_FRAME_FLIGHT_MODE_INDEX, CRSF_FRAME_GPS_INDEX, CRSF_FRAME_VARIO_SENSOR_INDEX, + CRSF_FRAME_BAROMETER_ALTITUDE_INDEX, CRSF_SCHEDULE_COUNT_MAX } crsfFrameTypeIndex_e; @@ -481,6 +509,11 @@ static void processCrsf(void) crsfFrameVarioSensor(dst); crsfFinalize(dst); } + if (currentSchedule & BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX)) { + crsfInitializeFrame(dst); + crsfBarometerAltitude(dst); + crsfFinalize(dst); + } #endif crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } @@ -514,6 +547,11 @@ void initCrsfTelemetry(void) if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) { crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX); } +#endif +#ifdef USE_BARO + if (sensors(SENSOR_BARO)) { + crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX); + } #endif crsfScheduleCount = (uint8_t)index; }