diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 5f2cf7f1797c0b369f2368e1cf66b402e6316c93..aa62037882c797dbbab95a6a557dc928c6ab1b19 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -1,6 +1,8 @@ - src/comm/MockLink.params + src/comm/PX4MockLink.params + src/comm/APMArduCopterMockLink.params + src/comm/APMArduPlaneMockLink.params src/FactSystem/FactSystemTest.qml diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index 873a84acf6d0a7cdb3cc12446d384a704eba1690..18736e9878b2a528411ae77851daa224994899d4 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -75,6 +75,8 @@ Item { property var _flightVideo: null property var _savedZoomLevel: 0 + property real _pipSize: ScreenTools.isAndroid ? ScreenTools.defaultFontPixelSize * (8) : ScreenTools.defaultFontPixelSize * (9) + FlightDisplayViewController { id: _controller } MissionController { @@ -125,8 +127,8 @@ Item { anchors.margins: ScreenTools.defaultFontPixelHeight anchors.left: parent.left anchors.bottom: parent.bottom - height: ScreenTools.defaultFontPixelSize * (9) - width: ScreenTools.defaultFontPixelSize * (9) * (16/9) + height: _pipSize + width: _pipSize * (16/9) color: "#000010" border.width: 4 radius: 4 diff --git a/src/FlightDisplay/FlightDisplayViewWidgets.qml b/src/FlightDisplay/FlightDisplayViewWidgets.qml index d2d9a9a5353538e91047edcbc7e60d1d5983bef5..b3a57b7cf6d176113bebfbf1834a02aa05143d67 100644 --- a/src/FlightDisplay/FlightDisplayViewWidgets.qml +++ b/src/FlightDisplay/FlightDisplayViewWidgets.qml @@ -87,7 +87,6 @@ Item { altitude: _altitudeWGS84 groundSpeed: _groundSpeed airSpeed: _airSpeed - climbRate: _climbRate isSatellite: _mainIsMap ? _flightMap ? _flightMap.isSatelliteMap : true : true z: QGroundControl.zOrderWidgets } @@ -97,7 +96,7 @@ Item { id: toolColumn anchors.margins: ScreenTools.defaultFontPixelHeight anchors.left: parent.left - anchors.verticalCenter: parent.verticalCenter + anchors.top: parent.top spacing: ScreenTools.defaultFontPixelHeight //-- Map Center Control diff --git a/src/FlightMap/Widgets/QGCInstrumentWidget.qml b/src/FlightMap/Widgets/QGCInstrumentWidget.qml index 329dfd7061bf383884154d77a58b081892b58f34..73675693993f6b25d591fb7103cf05bda3562d39 100644 --- a/src/FlightMap/Widgets/QGCInstrumentWidget.qml +++ b/src/FlightMap/Widgets/QGCInstrumentWidget.qml @@ -42,7 +42,6 @@ Item { property real altitude: 0 property real groundSpeed: 0 property real airSpeed: 0 - property real climbRate: 0 property real size: ScreenTools.defaultFontPixelSize * (10) property bool isSatellite: false property bool active: false @@ -78,24 +77,21 @@ Item { color: isSatellite ? Qt.rgba(0,0,0,0.25) : Qt.rgba(1,1,1,0.25) anchors.horizontalCenter: parent.horizontalCenter } - Row { - width: root.size * 0.8 - anchors.horizontalCenter: parent.horizontalCenter - QGCLabel { - text: "H" - font.pixelSize: ScreenTools.defaultFontPixelSize * 1.25 - width: parent.width * 0.45 - color: isSatellite ? "black" : "white" - horizontalAlignment: TextEdit.AlignHCenter - } - QGCLabel { - text: altitude.toFixed(1) - font.pixelSize: ScreenTools.defaultFontPixelSize * 1.25 - font.weight: Font.DemiBold - width: parent.width * 0.549 - color: isSatellite ? "black" : "white" - horizontalAlignment: TextEdit.AlignHCenter - } + QGCLabel { + text: "Altitude" + font.pixelSize: ScreenTools.defaultFontPixelSize * 0.75 + width: parent.width + height: ScreenTools.defaultFontPixelHeight * 0.5 + color: isSatellite ? "black" : "white" + horizontalAlignment: TextEdit.AlignHCenter + } + QGCLabel { + text: altitude < 10000 ? altitude.toFixed(1) : altitude.toFixed(0) + font.pixelSize: ScreenTools.defaultFontPixelSize * 2.5 + font.weight: Font.DemiBold + width: parent.width + color: isSatellite ? "black" : "white" + horizontalAlignment: TextEdit.AlignHCenter } //-- Ground Speed Rectangle { @@ -103,25 +99,25 @@ Item { width: parent.width * 0.9 color: isSatellite ? Qt.rgba(0,0,0,0.25) : Qt.rgba(1,1,1,0.25) anchors.horizontalCenter: parent.horizontalCenter + visible: airSpeed <= 0 } - Row { - width: root.size * 0.8 - anchors.horizontalCenter: parent.horizontalCenter - QGCLabel { - text: "GS" - font.pixelSize: ScreenTools.defaultFontPixelSize * 0.75 - width: parent.width * 0.45 - color: isSatellite ? "black" : "white" - horizontalAlignment: TextEdit.AlignHCenter - } - QGCLabel { - text: groundSpeed.toFixed(1) - font.pixelSize: ScreenTools.defaultFontPixelSize * 0.75 - font.weight: Font.DemiBold - width: parent.width * 0.549 - color: isSatellite ? "black" : "white" - horizontalAlignment: TextEdit.AlignHCenter - } + QGCLabel { + text: "Ground Speed" + font.pixelSize: ScreenTools.defaultFontPixelSize * 0.75 + width: parent.width + height: ScreenTools.defaultFontPixelHeight * 0.75 + color: isSatellite ? "black" : "white" + horizontalAlignment: TextEdit.AlignHCenter + visible: airSpeed <= 0 + } + QGCLabel { + text: groundSpeed.toFixed(1) + font.pixelSize: ScreenTools.defaultFontPixelSize * 1.25 + font.weight: Font.DemiBold + width: parent.width + color: isSatellite ? "black" : "white" + horizontalAlignment: TextEdit.AlignHCenter + visible: airSpeed <= 0 } //-- Air Speed Rectangle { @@ -131,51 +127,23 @@ Item { anchors.horizontalCenter: parent.horizontalCenter visible: airSpeed > 0 } - Row { - width: root.size * 0.8 - anchors.horizontalCenter: parent.horizontalCenter + QGCLabel { + text: "Air Speed" + font.pixelSize: ScreenTools.defaultFontPixelSize * 0.75 + width: parent.width + height: ScreenTools.defaultFontPixelHeight * 0.75 + color: isSatellite ? "black" : "white" visible: airSpeed > 0 - QGCLabel { - text: "AS" - font.pixelSize: ScreenTools.defaultFontPixelSize * 0.75 - width: parent.width * 0.45 - color: isSatellite ? "black" : "white" - horizontalAlignment: TextEdit.AlignHCenter - } - QGCLabel { - text: airSpeed.toFixed(1) - font.pixelSize: ScreenTools.defaultFontPixelSize * 0.75 - font.weight: Font.DemiBold - width: parent.width * 0.549 - color: isSatellite ? "black" : "white" - horizontalAlignment: TextEdit.AlignHCenter - } + horizontalAlignment: TextEdit.AlignHCenter } - //-- Climb Rate - Rectangle { - height: 1 - width: parent.width * 0.9 - color: isSatellite ? Qt.rgba(0,0,0,0.25) : Qt.rgba(1,1,1,0.25) - anchors.horizontalCenter: parent.horizontalCenter - } - Row { - width: root.size * 0.8 - anchors.horizontalCenter: parent.horizontalCenter - QGCLabel { - text: "VS" - font.pixelSize: ScreenTools.defaultFontPixelSize * 0.75 - width: parent.width * 0.45 - color: isSatellite ? "black" : "white" - horizontalAlignment: TextEdit.AlignHCenter - } - QGCLabel { - text: climbRate.toFixed(1) - font.pixelSize: ScreenTools.defaultFontPixelSize * 0.75 - font.weight: Font.DemiBold - width: parent.width * 0.549 - color: isSatellite ? "black" : "white" - horizontalAlignment: TextEdit.AlignHCenter - } + QGCLabel { + text: airSpeed.toFixed(1) + font.pixelSize: ScreenTools.defaultFontPixelSize * 1.25 + font.weight: Font.DemiBold + width: parent.width + color: isSatellite ? "black" : "white" + visible: airSpeed > 0 + horizontalAlignment: TextEdit.AlignHCenter } //-- Compass Rectangle { diff --git a/src/VideoStreaming/README.md b/src/VideoStreaming/README.md index f0986a9a1c34e3004d89079a8968453679e9ba1f..5b989b6bcd8d558536129aa0bd1d74051e159e34 100644 --- a/src/VideoStreaming/README.md +++ b/src/VideoStreaming/README.md @@ -60,7 +60,16 @@ TODO: Binaries found in http://gstreamer.freedesktop.org/data/pkg/ios ### Android Binaries found in http://gstreamer.freedesktop.org/data/pkg/android -Download the [gstreamer-1.0-android-armv7-1.5.2.tar.bz2](http://gstreamer.freedesktop.org/data/pkg/android/1.5.2/gstreamer-1.0-android-armv7-1.5.2.tar.bz2) archive (assuming you want the ARM V7 platform) and extract it to the root qgroundcontrol directory (the same directory qgroundcontrol.pro is located). That's where the build system will look for it. +Download the [gstreamer-1.0-android-armv7-1.5.2.tar.bz2](http://gstreamer.freedesktop.org/data/pkg/android/1.5.2/gstreamer-1.0-android-armv7-1.5.2.tar.bz2) archive (assuming you want the ARM V7 platform). + +Create a directory named "gstreamer-1.0-android-armv7-1.5.2" under the root qgroundcontrol directory (the same directory qgroundcontrol.pro is located). Extract the gstreamer tar file under this directory. That's where the build system will look for it. Make sure your archive tool doesn't create any additional top level directories. The structure after extracting the archive should look like this: + +qgroundcontrol +├── gstreamer-1.0-android-armv7-1.5.2 +│   ├── etc +│   ├── include +│   ├── lib +│   └── share ### Windows diff --git a/src/comm/APMArduCopterMockLink.params b/src/comm/APMArduCopterMockLink.params new file mode 100644 index 0000000000000000000000000000000000000000..de8ee7732bedcab713bc1d1ccbd114833a0a85b0 --- /dev/null +++ b/src/comm/APMArduCopterMockLink.params @@ -0,0 +1,506 @@ +# Onboard parameters for system MAV 001 +# +# MAV ID COMPONENT ID PARAM NAME VALUE (FLOAT) +1 1 ACCEL_Z_D 0 9 +1 1 ACCEL_Z_FILT_HZ 20 9 +1 1 ACCEL_Z_I 1 9 +1 1 ACCEL_Z_IMAX 800 9 +1 1 ACCEL_Z_P 0.5 9 +1 1 ACRO_BAL_PITCH 1 9 +1 1 ACRO_BAL_ROLL 1 9 +1 1 ACRO_EXPO 0.3 9 +1 1 ACRO_RP_P 4.5 9 +1 1 ACRO_TRAINER 2 2 +1 1 ACRO_YAW_P 4.5 9 +1 1 AHRS_COMP_BETA 0.1 9 +1 1 AHRS_GPS_GAIN 1 9 +1 1 AHRS_GPS_MINSATS 6 2 +1 1 AHRS_GPS_USE 1 2 +1 1 AHRS_ORIENTATION 0 2 +1 1 AHRS_RP_P 0.2 9 +1 1 AHRS_TRIM_X 0 9 +1 1 AHRS_TRIM_Y 0 9 +1 1 AHRS_TRIM_Z 0 9 +1 1 AHRS_WIND_MAX 0 2 +1 1 AHRS_YAW_P 0.2 9 +1 1 ANGLE_MAX 4500 4 +1 1 ARMING_CHECK 1 2 +1 1 ATC_ACCEL_P_MAX 1.1e+05 9 +1 1 ATC_ACCEL_R_MAX 1.1e+05 9 +1 1 ATC_ACCEL_Y_MAX 2.7e+04 9 +1 1 ATC_RATE_FF_ENAB 1 2 +1 1 ATC_SLEW_YAW 1e+03 9 +1 1 AUTOTUNE_AGGR 0.1 9 +1 1 AUTOTUNE_AXES 7 2 +1 1 AUTOTUNE_MIN_D 0.004 9 +1 1 BATT2_AMP_OFFSET 0 9 +1 1 BATT2_AMP_PERVOL 17 9 +1 1 BATT2_CAPACITY 3300 6 +1 1 BATT2_CURR_PIN 3 2 +1 1 BATT2_MONITOR 0 2 +1 1 BATT2_VOLT_MULT 10.1 9 +1 1 BATT2_VOLT_PIN 2 2 +1 1 BATT_AMP_OFFSET 0 9 +1 1 BATT_AMP_PERVOLT 17 9 +1 1 BATT_CAPACITY 3300 6 +1 1 BATT_CURR_PIN 3 2 +1 1 BATT_MONITOR 0 2 +1 1 BATT_VOLT_MULT 10.1 9 +1 1 BATT_VOLT_PIN 2 2 +1 1 BRD_PWM_COUNT 4 2 +1 1 BRD_SAFETYENABLE 1 2 +1 1 BRD_SBUS_OUT 0 2 +1 1 BRD_SER1_RTSCTS 2 2 +1 1 BRD_SER2_RTSCTS 2 2 +1 1 BRD_SERIAL_NUM 0 4 +1 1 CAM_DURATION 10 2 +1 1 CAM_SERVO_OFF 1100 4 +1 1 CAM_SERVO_ON 1300 4 +1 1 CAM_TRIGG_DIST 0 9 +1 1 CAM_TRIGG_TYPE 0 2 +1 1 CH10_OPT 0 2 +1 1 CH11_OPT 0 2 +1 1 CH12_OPT 0 2 +1 1 CH7_OPT 0 2 +1 1 CH8_OPT 0 2 +1 1 CH9_OPT 0 2 +1 1 CHUTE_ALT_MIN 10 4 +1 1 CHUTE_ENABLED 0 2 +1 1 CHUTE_SERVO_OFF 1100 4 +1 1 CHUTE_SERVO_ON 1300 4 +1 1 CHUTE_TYPE 0 2 +1 1 CIRCLE_RADIUS 1e+03 9 +1 1 CIRCLE_RATE 20 9 +1 1 CLI_ENABLED 0 2 +1 1 COMPASS_AUTODEC 1 2 +1 1 COMPASS_DEC 0 9 +1 1 COMPASS_DEV_ID 131594 6 +1 1 COMPASS_DEV_ID2 0 6 +1 1 COMPASS_DEV_ID3 0 6 +1 1 COMPASS_EXTERN2 0 2 +1 1 COMPASS_EXTERN3 0 2 +1 1 COMPASS_EXTERNAL 0 2 +1 1 COMPASS_LEARN 0 2 +1 1 COMPASS_MOT2_X 0 9 +1 1 COMPASS_MOT2_Y 0 9 +1 1 COMPASS_MOT2_Z 0 9 +1 1 COMPASS_MOT3_X 0 9 +1 1 COMPASS_MOT3_Y 0 9 +1 1 COMPASS_MOT3_Z 0 9 +1 1 COMPASS_MOTCT 0 2 +1 1 COMPASS_MOT_X 0 9 +1 1 COMPASS_MOT_Y 0 9 +1 1 COMPASS_MOT_Z 0 9 +1 1 COMPASS_OFS2_X 0 9 +1 1 COMPASS_OFS2_Y 0 9 +1 1 COMPASS_OFS2_Z 0 9 +1 1 COMPASS_OFS3_X 0 9 +1 1 COMPASS_OFS3_Y 0 9 +1 1 COMPASS_OFS3_Z 0 9 +1 1 COMPASS_OFS_X 0 9 +1 1 COMPASS_OFS_Y 0 9 +1 1 COMPASS_OFS_Z 0 9 +1 1 COMPASS_ORIENT 0 2 +1 1 COMPASS_ORIENT2 0 2 +1 1 COMPASS_ORIENT3 0 2 +1 1 COMPASS_PRIMARY 0 2 +1 1 COMPASS_USE 1 2 +1 1 COMPASS_USE2 1 2 +1 1 COMPASS_USE3 1 2 +1 1 EKF_ABIAS_PNOISE 5e-05 9 +1 1 EKF_ACC_PNOISE 0.25 9 +1 1 EKF_ALT_NOISE 1 9 +1 1 EKF_ALT_SOURCE 1 2 +1 1 EKF_EAS_GATE 10 2 +1 1 EKF_EAS_NOISE 1.4 9 +1 1 EKF_FALLBACK 1 2 +1 1 EKF_FLOW_DELAY 10 2 +1 1 EKF_FLOW_GATE 3 2 +1 1 EKF_FLOW_NOISE 0.25 9 +1 1 EKF_GBIAS_PNOISE 1e-06 9 +1 1 EKF_GLITCH_ACCEL 100 4 +1 1 EKF_GLITCH_RAD 25 2 +1 1 EKF_GND_GRADIENT 2 2 +1 1 EKF_GPS_TYPE 0 2 +1 1 EKF_GYRO_PNOISE 0.015 9 +1 1 EKF_HGT_GATE 10 2 +1 1 EKF_MAGB_PNOISE 0.0003 9 +1 1 EKF_MAGE_PNOISE 0.0003 9 +1 1 EKF_MAG_CAL 1 2 +1 1 EKF_MAG_GATE 3 2 +1 1 EKF_MAG_NOISE 0.05 9 +1 1 EKF_MAX_FLOW 2.5 9 +1 1 EKF_POSNE_NOISE 0.5 9 +1 1 EKF_POS_DELAY 220 4 +1 1 EKF_POS_GATE 10 2 +1 1 EKF_RNG_GATE 5 2 +1 1 EKF_VELD_NOISE 0.7 9 +1 1 EKF_VELNE_NOISE 0.5 9 +1 1 EKF_VEL_DELAY 220 4 +1 1 EKF_VEL_GATE 5 2 +1 1 EKF_WIND_PNOISE 0.1 9 +1 1 EKF_WIND_PSCALE 0.5 9 +1 1 EPM_ENABLE 0 2 +1 1 EPM_GRAB 1900 4 +1 1 EPM_NEUTRAL 1500 4 +1 1 EPM_REGRAB 0 2 +1 1 EPM_RELEASE 1100 4 +1 1 ESC_CALIBRATION 0 2 +1 1 FENCE_ACTION 1 2 +1 1 FENCE_ALT_MAX 100 9 +1 1 FENCE_ENABLE 0 2 +1 1 FENCE_MARGIN 2 9 +1 1 FENCE_RADIUS 300 9 +1 1 FENCE_TYPE 3 2 +1 1 FLOW_ENABLE 0 2 +1 1 FLOW_FXSCALER 0 4 +1 1 FLOW_FYSCALER 0 4 +1 1 FLOW_ORIENT_YAW 0 4 +1 1 FLTMODE1 0 2 +1 1 FLTMODE2 0 2 +1 1 FLTMODE3 0 2 +1 1 FLTMODE4 0 2 +1 1 FLTMODE5 0 2 +1 1 FLTMODE6 0 2 +1 1 FRAME 1 2 +1 1 FS_BATT_ENABLE 0 2 +1 1 FS_BATT_MAH 0 9 +1 1 FS_BATT_VOLTAGE 10.5 9 +1 1 FS_EKF_ACTION 1 2 +1 1 FS_EKF_THRESH 0.8 9 +1 1 FS_GCS_ENABLE 1 2 +1 1 FS_THR_ENABLE 0 2 +1 1 FS_THR_VALUE 975 4 +1 1 GCS_PID_MASK 0 4 +1 1 GND_ABS_PRESS 1.01e+05 9 +1 1 GND_ALT_OFFSET 0 2 +1 1 GND_TEMP 29.5 9 +1 1 GPS_AUTO_SWITCH 1 2 +1 1 GPS_GNSS_MODE 0 2 +1 1 GPS_HDOP_GOOD 140 4 +1 1 GPS_INJECT_TO 127 2 +1 1 GPS_MIN_DGPS 100 2 +1 1 GPS_MIN_ELEV -100 2 +1 1 GPS_NAVFILTER 8 2 +1 1 GPS_RAW_DATA 0 2 +1 1 GPS_SBAS_MODE 2 2 +1 1 GPS_SBP_LOGMASK -256 4 +1 1 GPS_TYPE 1 2 +1 1 GPS_TYPE2 0 2 +1 1 INS_ACC2OFFS_X 0 9 +1 1 INS_ACC2OFFS_Y 0 9 +1 1 INS_ACC2OFFS_Z 0 9 +1 1 INS_ACC2SCAL_X 1 9 +1 1 INS_ACC2SCAL_Y 1 9 +1 1 INS_ACC2SCAL_Z 1 9 +1 1 INS_ACC3OFFS_X 0 9 +1 1 INS_ACC3OFFS_Y 0 9 +1 1 INS_ACC3OFFS_Z 0 9 +1 1 INS_ACC3SCAL_X 0 9 +1 1 INS_ACC3SCAL_Y 0 9 +1 1 INS_ACC3SCAL_Z 0 9 +1 1 INS_ACCEL_FILTER 20 2 +1 1 INS_ACCOFFS_X 0 9 +1 1 INS_ACCOFFS_Y 0 9 +1 1 INS_ACCOFFS_Z 0 9 +1 1 INS_ACCSCAL_X 1 9 +1 1 INS_ACCSCAL_Y 1 9 +1 1 INS_ACCSCAL_Z 1 9 +1 1 INS_GYR2OFFS_X 0.0184 9 +1 1 INS_GYR2OFFS_Y 0.00255 9 +1 1 INS_GYR2OFFS_Z -0.0246 9 +1 1 INS_GYR3OFFS_X 0 9 +1 1 INS_GYR3OFFS_Y 0 9 +1 1 INS_GYR3OFFS_Z 0 9 +1 1 INS_GYROFFS_X 0.0225 9 +1 1 INS_GYROFFS_Y 0.0483 9 +1 1 INS_GYROFFS_Z -0.00417 9 +1 1 INS_GYRO_FILTER 20 2 +1 1 INS_PRODUCT_ID 5 4 +1 1 INS_USE 1 2 +1 1 INS_USE2 1 2 +1 1 INS_USE3 0 2 +1 1 LAND_REPOSITION 1 2 +1 1 LAND_SPEED 50 4 +1 1 LGR_SERVO_DEPLOY 1750 4 +1 1 LGR_SERVO_RTRACT 1250 4 +1 1 LOG_BITMASK 176126 6 +1 1 MAG_ENABLE 1 2 +1 1 MIS_RESTART 0 2 +1 1 MIS_TOTAL 0 4 +1 1 MNT_ANGMAX_PAN 4500 4 +1 1 MNT_ANGMAX_ROL 4500 4 +1 1 MNT_ANGMAX_TIL 4500 4 +1 1 MNT_ANGMIN_PAN -4500 4 +1 1 MNT_ANGMIN_ROL -4500 4 +1 1 MNT_ANGMIN_TIL -4500 4 +1 1 MNT_DEFLT_MODE 3 2 +1 1 MNT_JSTICK_SPD 0 2 +1 1 MNT_K_RATE 5 9 +1 1 MNT_LEAD_PTCH 0 9 +1 1 MNT_LEAD_RLL 0 9 +1 1 MNT_NEUTRAL_X 0 9 +1 1 MNT_NEUTRAL_Y 0 9 +1 1 MNT_NEUTRAL_Z 0 9 +1 1 MNT_OFF_ACC_X 0 9 +1 1 MNT_OFF_ACC_Y 0 9 +1 1 MNT_OFF_ACC_Z 0 9 +1 1 MNT_OFF_GYRO_X 0 9 +1 1 MNT_OFF_GYRO_Y 0 9 +1 1 MNT_OFF_GYRO_Z 0 9 +1 1 MNT_OFF_JNT_X 0 9 +1 1 MNT_OFF_JNT_Y 0 9 +1 1 MNT_OFF_JNT_Z 0 9 +1 1 MNT_RC_IN_PAN 0 2 +1 1 MNT_RC_IN_ROLL 0 2 +1 1 MNT_RC_IN_TILT 0 2 +1 1 MNT_RETRACT_X 0 9 +1 1 MNT_RETRACT_Y 0 9 +1 1 MNT_RETRACT_Z 0 9 +1 1 MNT_STAB_PAN 0 2 +1 1 MNT_STAB_ROLL 0 2 +1 1 MNT_STAB_TILT 0 2 +1 1 MNT_TYPE 0 2 +1 1 MOT_CURR_MAX 0 9 +1 1 MOT_SPIN_ARMED 70 4 +1 1 MOT_THR_MIX_MAX 0.5 9 +1 1 MOT_THR_MIX_MIN 0.1 9 +1 1 MOT_THST_BAT_MAX 0 9 +1 1 MOT_THST_BAT_MIN 0 9 +1 1 MOT_THST_EXPO 0.65 9 +1 1 MOT_THST_MAX 0.95 9 +1 1 MOT_YAW_HEADROOM 200 4 +1 1 PHLD_BRAKE_ANGLE 3000 4 +1 1 PHLD_BRAKE_RATE 8 4 +1 1 PILOT_ACCEL_Z 250 4 +1 1 PILOT_THR_BHV 0 4 +1 1 PILOT_THR_FILT 0 9 +1 1 PILOT_TKOFF_ALT 0 9 +1 1 PILOT_TKOFF_DZ 100 4 +1 1 PILOT_VELZ_MAX 250 4 +1 1 POS_XY_P 1 9 +1 1 POS_Z_P 1 9 +1 1 PSC_ACC_XY_FILT 2 9 +1 1 RALLY_INCL_HOME 1 2 +1 1 RALLY_LIMIT_KM 0.3 9 +1 1 RALLY_TOTAL 0 2 +1 1 RATE_PIT_D 0.004 9 +1 1 RATE_PIT_FILT_HZ 20 9 +1 1 RATE_PIT_I 0.1 9 +1 1 RATE_PIT_IMAX 2e+03 9 +1 1 RATE_PIT_P 0.15 9 +1 1 RATE_RLL_D 0.004 9 +1 1 RATE_RLL_FILT_HZ 20 9 +1 1 RATE_RLL_I 0.1 9 +1 1 RATE_RLL_IMAX 2e+03 9 +1 1 RATE_RLL_P 0.15 9 +1 1 RATE_YAW_D 0 9 +1 1 RATE_YAW_FILT_HZ 5 9 +1 1 RATE_YAW_I 0.02 9 +1 1 RATE_YAW_IMAX 1e+03 9 +1 1 RATE_YAW_P 0.2 9 +1 1 RC10_DZ 0 4 +1 1 RC10_FUNCTION 0 2 +1 1 RC10_MAX 1900 4 +1 1 RC10_MIN 1100 4 +1 1 RC10_REV 1 2 +1 1 RC10_TRIM 1500 4 +1 1 RC11_DZ 0 4 +1 1 RC11_FUNCTION 0 2 +1 1 RC11_MAX 1900 4 +1 1 RC11_MIN 1100 4 +1 1 RC11_REV 1 2 +1 1 RC11_TRIM 1500 4 +1 1 RC12_DZ 0 4 +1 1 RC12_FUNCTION 0 2 +1 1 RC12_MAX 1900 4 +1 1 RC12_MIN 1100 4 +1 1 RC12_REV 1 2 +1 1 RC12_TRIM 1500 4 +1 1 RC13_DZ 0 4 +1 1 RC13_FUNCTION 0 2 +1 1 RC13_MAX 1900 4 +1 1 RC13_MIN 1100 4 +1 1 RC13_REV 1 2 +1 1 RC13_TRIM 1500 4 +1 1 RC14_DZ 0 4 +1 1 RC14_FUNCTION 0 2 +1 1 RC14_MAX 1900 4 +1 1 RC14_MIN 1100 4 +1 1 RC14_REV 1 2 +1 1 RC14_TRIM 1500 4 +1 1 RC1_DZ 30 4 +1 1 RC1_MAX 1900 4 +1 1 RC1_MIN 1100 4 +1 1 RC1_REV 1 2 +1 1 RC1_TRIM 1500 4 +1 1 RC2_DZ 30 4 +1 1 RC2_MAX 1900 4 +1 1 RC2_MIN 1100 4 +1 1 RC2_REV 1 2 +1 1 RC2_TRIM 1500 4 +1 1 RC3_DZ 30 4 +1 1 RC3_MAX 1900 4 +1 1 RC3_MIN 1100 4 +1 1 RC3_REV 1 2 +1 1 RC3_TRIM 1500 4 +1 1 RC4_DZ 40 4 +1 1 RC4_MAX 1900 4 +1 1 RC4_MIN 1100 4 +1 1 RC4_REV 1 2 +1 1 RC4_TRIM 1500 4 +1 1 RC5_DZ 0 4 +1 1 RC5_FUNCTION 0 2 +1 1 RC5_MAX 1900 4 +1 1 RC5_MIN 1100 4 +1 1 RC5_REV 1 2 +1 1 RC5_TRIM 1500 4 +1 1 RC6_DZ 0 4 +1 1 RC6_FUNCTION 0 2 +1 1 RC6_MAX 1900 4 +1 1 RC6_MIN 1100 4 +1 1 RC6_REV 1 2 +1 1 RC6_TRIM 1500 4 +1 1 RC7_DZ 0 4 +1 1 RC7_FUNCTION 0 2 +1 1 RC7_MAX 1900 4 +1 1 RC7_MIN 1100 4 +1 1 RC7_REV 1 2 +1 1 RC7_TRIM 1500 4 +1 1 RC8_DZ 0 4 +1 1 RC8_FUNCTION 0 2 +1 1 RC8_MAX 1900 4 +1 1 RC8_MIN 1100 4 +1 1 RC8_REV 1 2 +1 1 RC8_TRIM 1500 4 +1 1 RC9_DZ 0 4 +1 1 RC9_FUNCTION 0 2 +1 1 RC9_MAX 1900 4 +1 1 RC9_MIN 1100 4 +1 1 RC9_REV 1 2 +1 1 RC9_TRIM 1500 4 +1 1 RCMAP_PITCH 2 2 +1 1 RCMAP_ROLL 1 2 +1 1 RCMAP_THROTTLE 3 2 +1 1 RCMAP_YAW 4 2 +1 1 RC_FEEL_RP 50 2 +1 1 RC_SPEED 490 4 +1 1 RELAY_DEFAULT 0 2 +1 1 RELAY_PIN 54 2 +1 1 RELAY_PIN2 55 2 +1 1 RELAY_PIN3 -1 2 +1 1 RELAY_PIN4 -1 2 +1 1 RNGFND2_FUNCTION 0 2 +1 1 RNGFND2_GNDCLEAR 10 2 +1 1 RNGFND2_MAX_CM 700 4 +1 1 RNGFND2_MIN_CM 20 4 +1 1 RNGFND2_OFFSET 0 9 +1 1 RNGFND2_PIN -1 2 +1 1 RNGFND2_RMETRIC 1 2 +1 1 RNGFND2_SCALING 3 9 +1 1 RNGFND2_SETTLE 0 4 +1 1 RNGFND2_STOP_PIN -1 2 +1 1 RNGFND2_TYPE 0 2 +1 1 RNGFND_FUNCTION 0 2 +1 1 RNGFND_GAIN 0.8 9 +1 1 RNGFND_GNDCLEAR 10 2 +1 1 RNGFND_MAX_CM 700 4 +1 1 RNGFND_MIN_CM 20 4 +1 1 RNGFND_OFFSET 0 9 +1 1 RNGFND_PIN -1 2 +1 1 RNGFND_PWRRNG 0 4 +1 1 RNGFND_RMETRIC 1 2 +1 1 RNGFND_SCALING 3 9 +1 1 RNGFND_SETTLE 0 4 +1 1 RNGFND_STOP_PIN -1 2 +1 1 RNGFND_TYPE 0 2 +1 1 RPM2_SCALING 1 9 +1 1 RPM2_TYPE 0 2 +1 1 RPM_SCALING 1 9 +1 1 RPM_TYPE 0 2 +1 1 RSSI_PIN -1 2 +1 1 RSSI_RANGE 5 9 +1 1 RTL_ALT 1500 4 +1 1 RTL_ALT_FINAL 0 4 +1 1 RTL_CLIMB_MIN 0 4 +1 1 RTL_LOIT_TIME 5000 6 +1 1 SCHED_DEBUG 0 2 +1 1 SERIAL0_BAUD 115 6 +1 1 SERIAL1_BAUD 57 6 +1 1 SERIAL1_PROTOCOL 1 2 +1 1 SERIAL2_BAUD 57 6 +1 1 SERIAL2_PROTOCOL 1 2 +1 1 SERIAL3_BAUD 38 6 +1 1 SERIAL3_PROTOCOL 5 2 +1 1 SERIAL4_BAUD 38 6 +1 1 SERIAL4_PROTOCOL 5 2 +1 1 SIMPLE 0 2 +1 1 SR0_EXTRA1 10 4 +1 1 SR0_EXTRA2 0 4 +1 1 SR0_EXTRA3 0 4 +1 1 SR0_EXT_STAT 2 4 +1 1 SR0_PARAMS 10 4 +1 1 SR0_POSITION 3 4 +1 1 SR0_RAW_CTRL 0 4 +1 1 SR0_RAW_SENS 2 4 +1 1 SR0_RC_CHAN 2 4 +1 1 SR1_EXTRA1 0 4 +1 1 SR1_EXTRA2 0 4 +1 1 SR1_EXTRA3 0 4 +1 1 SR1_EXT_STAT 0 4 +1 1 SR1_PARAMS 0 4 +1 1 SR1_POSITION 0 4 +1 1 SR1_RAW_CTRL 0 4 +1 1 SR1_RAW_SENS 0 4 +1 1 SR1_RC_CHAN 0 4 +1 1 SR2_EXTRA1 0 4 +1 1 SR2_EXTRA2 0 4 +1 1 SR2_EXTRA3 0 4 +1 1 SR2_EXT_STAT 0 4 +1 1 SR2_PARAMS 0 4 +1 1 SR2_POSITION 0 4 +1 1 SR2_RAW_CTRL 0 4 +1 1 SR2_RAW_SENS 0 4 +1 1 SR2_RC_CHAN 0 4 +1 1 SR3_EXTRA1 0 4 +1 1 SR3_EXTRA2 0 4 +1 1 SR3_EXTRA3 0 4 +1 1 SR3_EXT_STAT 0 4 +1 1 SR3_PARAMS 0 4 +1 1 SR3_POSITION 0 4 +1 1 SR3_RAW_CTRL 0 4 +1 1 SR3_RAW_SENS 0 4 +1 1 SR3_RC_CHAN 0 4 +1 1 STB_PIT_P 4.5 9 +1 1 STB_RLL_P 4.5 9 +1 1 STB_YAW_P 4.5 9 +1 1 SUPER_SIMPLE 0 2 +1 1 SYSID_MYGCS 255 4 +1 1 SYSID_SW_MREV 120 4 +1 1 SYSID_SW_TYPE 10 2 +1 1 SYSID_THISMAV 1 4 +1 1 TELEM_DELAY 0 2 +1 1 TERRAIN_ENABLE 1 2 +1 1 TERRAIN_SPACING 100 4 +1 1 THR_DZ 100 4 +1 1 THR_MID 500 4 +1 1 THR_MIN 130 4 +1 1 TUNE 0 2 +1 1 TUNE_HIGH 1000 4 +1 1 TUNE_LOW 0 4 +1 1 VEL_XY_FILT_HZ 5 9 +1 1 VEL_XY_I 0.5 9 +1 1 VEL_XY_IMAX 1e+03 9 +1 1 VEL_XY_P 1 9 +1 1 VEL_Z_P 5 9 +1 1 WPNAV_ACCEL 100 9 +1 1 WPNAV_ACCEL_Z 100 9 +1 1 WPNAV_LOIT_JERK 1e+03 9 +1 1 WPNAV_LOIT_MAXA 250 9 +1 1 WPNAV_LOIT_MINA 25 9 +1 1 WPNAV_LOIT_SPEED 500 9 +1 1 WPNAV_RADIUS 200 9 +1 1 WPNAV_SPEED 500 9 +1 1 WPNAV_SPEED_DN 150 9 +1 1 WPNAV_SPEED_UP 250 9 +1 1 WP_YAW_BEHAVIOR 2 2 diff --git a/src/comm/APMArduPlaneMockLink.params b/src/comm/APMArduPlaneMockLink.params new file mode 100644 index 0000000000000000000000000000000000000000..e6a48fa0991a8b658a2e2d6cb372ade815b5db51 --- /dev/null +++ b/src/comm/APMArduPlaneMockLink.params @@ -0,0 +1,587 @@ +# Onboard parameters for system MAV 001 +# +# MAV ID COMPONENT ID PARAM NAME VALUE (FLOAT) +1 1 ACRO_LOCKING 0 2 +1 1 ACRO_PITCH_RATE 180 4 +1 1 ACRO_ROLL_RATE 180 4 +1 1 AFS_AMSL_ERR_GPS -1 6 +1 1 AFS_AMSL_LIMIT 0 6 +1 1 AFS_ENABLE 0 2 +1 1 AFS_HB_PIN -1 2 +1 1 AFS_MAN_PIN -1 2 +1 1 AFS_MAX_COM_LOSS 0 2 +1 1 AFS_MAX_GPS_LOSS 0 2 +1 1 AFS_QNH_PRESSURE 0 9 +1 1 AFS_RC_FAIL_MS 0 4 +1 1 AFS_TERMINATE 0 2 +1 1 AFS_TERM_ACTION 0 2 +1 1 AFS_TERM_PIN -1 2 +1 1 AFS_WP_COMMS 0 2 +1 1 AFS_WP_GPS_LOSS 0 2 +1 1 AHRS_COMP_BETA 0.1 9 +1 1 AHRS_EKF_USE 1 2 +1 1 AHRS_GPS_GAIN 1 9 +1 1 AHRS_GPS_MINSATS 6 2 +1 1 AHRS_GPS_USE 1 2 +1 1 AHRS_ORIENTATION 0 2 +1 1 AHRS_RP_P 0.2 9 +1 1 AHRS_TRIM_X 0 9 +1 1 AHRS_TRIM_Y 0 9 +1 1 AHRS_TRIM_Z 0 9 +1 1 AHRS_WIND_MAX 0 2 +1 1 AHRS_YAW_P 0.2 9 +1 1 ALT_CTRL_ALG 0 2 +1 1 ALT_HOLD_FBWCM 0 4 +1 1 ALT_HOLD_RTL 10000 6 +1 1 ALT_MIX 1 9 +1 1 ALT_OFFSET 0 4 +1 1 ARMING_CHECK 1 4 +1 1 ARMING_REQUIRE 1 2 +1 1 ARMING_RUDDER 1 2 +1 1 ARSPD_AUTOCAL 0 2 +1 1 ARSPD_ENABLE 1 2 +1 1 ARSPD_FBW_MAX 22 4 +1 1 ARSPD_FBW_MIN 9 4 +1 1 ARSPD_OFFSET 0 9 +1 1 ARSPD_PIN 15 2 +1 1 ARSPD_RATIO 1.99 9 +1 1 ARSPD_SKIP_CAL 0 2 +1 1 ARSPD_TUBE_ORDER 2 2 +1 1 ARSPD_USE 0 2 +1 1 AUTOTUNE_LEVEL 6 2 +1 1 AUTO_FBW_STEER 0 2 +1 1 BATT2_AMP_OFFSET 0 9 +1 1 BATT2_AMP_PERVOL 17 9 +1 1 BATT2_CAPACITY 3300 6 +1 1 BATT2_CURR_PIN 3 2 +1 1 BATT2_MONITOR 0 2 +1 1 BATT2_VOLT_MULT 10.1 9 +1 1 BATT2_VOLT_PIN 2 2 +1 1 BATT_AMP_OFFSET 0 9 +1 1 BATT_AMP_PERVOLT 17 9 +1 1 BATT_CAPACITY 3300 6 +1 1 BATT_CURR_PIN 3 2 +1 1 BATT_MONITOR 0 2 +1 1 BATT_VOLT_MULT 10.1 9 +1 1 BATT_VOLT_PIN 2 2 +1 1 BRD_PWM_COUNT 4 2 +1 1 BRD_SAFETYENABLE 1 2 +1 1 BRD_SBUS_OUT 0 2 +1 1 BRD_SER1_RTSCTS 2 2 +1 1 BRD_SER2_RTSCTS 2 2 +1 1 BRD_SERIAL_NUM 0 4 +1 1 CAM_DURATION 10 2 +1 1 CAM_RELAY_ON 1 2 +1 1 CAM_SERVO_OFF 1100 4 +1 1 CAM_SERVO_ON 1300 4 +1 1 CAM_TRIGG_DIST 0 9 +1 1 CAM_TRIGG_TYPE 0 2 +1 1 CLI_ENABLED 0 2 +1 1 COMPASS_AUTODEC 1 2 +1 1 COMPASS_CAL_FIT 8 9 +1 1 COMPASS_DEC 0 9 +1 1 COMPASS_DEV_ID 131594 6 +1 1 COMPASS_DEV_ID2 0 6 +1 1 COMPASS_DEV_ID3 0 6 +1 1 COMPASS_DIA2_X 0 9 +1 1 COMPASS_DIA2_Y 0 9 +1 1 COMPASS_DIA2_Z 0 9 +1 1 COMPASS_DIA3_X 0 9 +1 1 COMPASS_DIA3_Y 0 9 +1 1 COMPASS_DIA3_Z 0 9 +1 1 COMPASS_DIA_X 1 9 +1 1 COMPASS_DIA_Y 1 9 +1 1 COMPASS_DIA_Z 1 9 +1 1 COMPASS_EXTERN2 0 2 +1 1 COMPASS_EXTERN3 0 2 +1 1 COMPASS_EXTERNAL 0 2 +1 1 COMPASS_LEARN 1 2 +1 1 COMPASS_MOT2_X 0 9 +1 1 COMPASS_MOT2_Y 0 9 +1 1 COMPASS_MOT2_Z 0 9 +1 1 COMPASS_MOT3_X 0 9 +1 1 COMPASS_MOT3_Y 0 9 +1 1 COMPASS_MOT3_Z 0 9 +1 1 COMPASS_MOTCT 0 2 +1 1 COMPASS_MOT_X 0 9 +1 1 COMPASS_MOT_Y 0 9 +1 1 COMPASS_MOT_Z 0 9 +1 1 COMPASS_ODI2_X 0 9 +1 1 COMPASS_ODI2_Y 0 9 +1 1 COMPASS_ODI2_Z 0 9 +1 1 COMPASS_ODI3_X 0 9 +1 1 COMPASS_ODI3_Y 0 9 +1 1 COMPASS_ODI3_Z 0 9 +1 1 COMPASS_ODI_X 0 9 +1 1 COMPASS_ODI_Y 0 9 +1 1 COMPASS_ODI_Z 0 9 +1 1 COMPASS_OFS2_X 0 9 +1 1 COMPASS_OFS2_Y 0 9 +1 1 COMPASS_OFS2_Z 0 9 +1 1 COMPASS_OFS3_X 0 9 +1 1 COMPASS_OFS3_Y 0 9 +1 1 COMPASS_OFS3_Z 0 9 +1 1 COMPASS_OFS_X 0 9 +1 1 COMPASS_OFS_Y 0 9 +1 1 COMPASS_OFS_Z 0 9 +1 1 COMPASS_ORIENT 0 2 +1 1 COMPASS_ORIENT2 0 2 +1 1 COMPASS_ORIENT3 0 2 +1 1 COMPASS_PRIMARY 0 2 +1 1 COMPASS_USE 1 2 +1 1 COMPASS_USE2 1 2 +1 1 COMPASS_USE3 1 2 +1 1 CRASH_DETECT 0 2 +1 1 EKF_ABIAS_PNOISE 5e-05 9 +1 1 EKF_ACC_PNOISE 0.5 9 +1 1 EKF_ALT_NOISE 0.5 9 +1 1 EKF_ALT_SOURCE 1 2 +1 1 EKF_EAS_GATE 10 2 +1 1 EKF_EAS_NOISE 1.4 9 +1 1 EKF_FALLBACK 1 2 +1 1 EKF_FLOW_DELAY 25 2 +1 1 EKF_FLOW_GATE 3 2 +1 1 EKF_FLOW_NOISE 0.3 9 +1 1 EKF_GBIAS_PNOISE 8e-06 9 +1 1 EKF_GLITCH_ACCEL 150 4 +1 1 EKF_GLITCH_RAD 20 2 +1 1 EKF_GND_GRADIENT 2 2 +1 1 EKF_GPS_TYPE 0 2 +1 1 EKF_GYRO_PNOISE 0.015 9 +1 1 EKF_HGT_GATE 20 2 +1 1 EKF_MAGB_PNOISE 0.0003 9 +1 1 EKF_MAGE_PNOISE 0.0003 9 +1 1 EKF_MAG_CAL 0 2 +1 1 EKF_MAG_GATE 3 2 +1 1 EKF_MAG_NOISE 0.05 9 +1 1 EKF_MAX_FLOW 2.5 9 +1 1 EKF_POSNE_NOISE 0.5 9 +1 1 EKF_POS_DELAY 220 4 +1 1 EKF_POS_GATE 30 2 +1 1 EKF_RNG_GATE 5 2 +1 1 EKF_VELD_NOISE 0.7 9 +1 1 EKF_VELNE_NOISE 0.5 9 +1 1 EKF_VEL_DELAY 220 4 +1 1 EKF_VEL_GATE 6 2 +1 1 EKF_WIND_PNOISE 0.1 9 +1 1 EKF_WIND_PSCALE 0.5 9 +1 1 ELEVON_CH1_REV 0 2 +1 1 ELEVON_CH2_REV 0 2 +1 1 ELEVON_MIXING 0 2 +1 1 ELEVON_OUTPUT 0 2 +1 1 ELEVON_REVERSE 0 2 +1 1 FBWA_TDRAG_CHAN 0 2 +1 1 FBWB_CLIMB_RATE 2 2 +1 1 FBWB_ELEV_REV 0 2 +1 1 FENCE_ACTION 0 2 +1 1 FENCE_AUTOENABLE 0 2 +1 1 FENCE_CHANNEL 0 2 +1 1 FENCE_MAXALT 0 4 +1 1 FENCE_MINALT 0 4 +1 1 FENCE_RETALT 0 4 +1 1 FENCE_RET_RALLY 0 2 +1 1 FENCE_TOTAL 0 2 +1 1 FLAPERON_OUTPUT 0 2 +1 1 FLAP_1_PERCNT 0 2 +1 1 FLAP_1_SPEED 0 2 +1 1 FLAP_2_PERCNT 0 2 +1 1 FLAP_2_SPEED 0 2 +1 1 FLAP_IN_CHANNEL 0 2 +1 1 FLAP_SLEWRATE 75 2 +1 1 FLOW_ENABLE 0 2 +1 1 FLOW_FXSCALER 0 4 +1 1 FLOW_FYSCALER 0 4 +1 1 FLOW_ORIENT_YAW 0 4 +1 1 FLTMODE1 11 2 +1 1 FLTMODE2 11 2 +1 1 FLTMODE3 5 2 +1 1 FLTMODE4 5 2 +1 1 FLTMODE5 0 2 +1 1 FLTMODE6 0 2 +1 1 FLTMODE_CH 8 2 +1 1 FORMAT_VERSION 13 4 +1 1 FS_BATT_MAH 0 9 +1 1 FS_BATT_VOLTAGE 0 9 +1 1 FS_GCS_ENABL 0 2 +1 1 FS_LONG_ACTN 0 2 +1 1 FS_LONG_TIMEOUT 5 9 +1 1 FS_SHORT_ACTN 0 2 +1 1 FS_SHORT_TIMEOUT 1.5 9 +1 1 GCS_PID_MASK 0 4 +1 1 GLIDE_SLOPE_MIN 15 4 +1 1 GLIDE_SLOPE_THR 5 9 +1 1 GND_ABS_PRESS 1.01e+05 9 +1 1 GND_ALT_OFFSET 0 9 +1 1 GND_PRIMARY 0 2 +1 1 GND_TEMP 0 9 +1 1 GPS_AUTO_SWITCH 1 2 +1 1 GPS_GNSS_MODE 0 2 +1 1 GPS_INJECT_TO 127 2 +1 1 GPS_MIN_DGPS 100 2 +1 1 GPS_MIN_ELEV -100 2 +1 1 GPS_NAVFILTER 8 2 +1 1 GPS_RAW_DATA 0 2 +1 1 GPS_SBAS_MODE 2 2 +1 1 GPS_SBP_LOGMASK -256 4 +1 1 GPS_TYPE 1 2 +1 1 GPS_TYPE2 0 2 +1 1 GROUND_STEER_ALT 0 9 +1 1 GROUND_STEER_DPS 90 4 +1 1 HIL_ERR_LIMIT 5 9 +1 1 HIL_MODE 0 2 +1 1 HIL_SERVOS 0 2 +1 1 INITIAL_MODE 0 2 +1 1 INS_ACC2OFFS_X 0 9 +1 1 INS_ACC2OFFS_Y 0 9 +1 1 INS_ACC2OFFS_Z 0 9 +1 1 INS_ACC2SCAL_X 1 9 +1 1 INS_ACC2SCAL_Y 1 9 +1 1 INS_ACC2SCAL_Z 1 9 +1 1 INS_ACC3OFFS_X 0 9 +1 1 INS_ACC3OFFS_Y 0 9 +1 1 INS_ACC3OFFS_Z 0 9 +1 1 INS_ACC3SCAL_X 0 9 +1 1 INS_ACC3SCAL_Y 0 9 +1 1 INS_ACC3SCAL_Z 0 9 +1 1 INS_ACCEL_FILTER 20 2 +1 1 INS_ACCOFFS_X 0 9 +1 1 INS_ACCOFFS_Y 0 9 +1 1 INS_ACCOFFS_Z 0 9 +1 1 INS_ACCSCAL_X 1 9 +1 1 INS_ACCSCAL_Y 1 9 +1 1 INS_ACCSCAL_Z 1 9 +1 1 INS_GYR2OFFS_X 0.0165 9 +1 1 INS_GYR2OFFS_Y 0.00344 9 +1 1 INS_GYR2OFFS_Z -0.0258 9 +1 1 INS_GYR3OFFS_X 0 9 +1 1 INS_GYR3OFFS_Y 0 9 +1 1 INS_GYR3OFFS_Z 0 9 +1 1 INS_GYROFFS_X 0.0234 9 +1 1 INS_GYROFFS_Y 0.0507 9 +1 1 INS_GYROFFS_Z -0.00498 9 +1 1 INS_GYRO_FILTER 20 2 +1 1 INS_GYR_CAL 1 2 +1 1 INS_PRODUCT_ID 5 4 +1 1 INS_STILL_THRESH 0.1 9 +1 1 INS_USE 1 2 +1 1 INS_USE2 1 2 +1 1 INS_USE3 0 2 +1 1 INVERTEDFLT_CH 0 2 +1 1 KFF_RDDRMIX 0.5 9 +1 1 KFF_THR2PTCH 0 9 +1 1 LAND_ABORT_THR 0 2 +1 1 LAND_DISARMDELAY 20 2 +1 1 LAND_FLAP_PERCNT 0 2 +1 1 LAND_FLARE_ALT 3 9 +1 1 LAND_FLARE_SEC 2 9 +1 1 LAND_PITCH_CD 0 4 +1 1 LEVEL_ROLL_LIMIT 5 2 +1 1 LIM_PITCH_MAX 2000 4 +1 1 LIM_PITCH_MIN -2500 4 +1 1 LIM_ROLL_CD 4500 4 +1 1 LOG_BITMASK 65535 6 +1 1 MAG_ENABLE 1 2 +1 1 MIN_GNDSPD_CM 0 6 +1 1 MIS_RESTART 0 2 +1 1 MIS_TOTAL 0 4 +1 1 MIXING_GAIN 0.5 9 +1 1 MNT_ANGMAX_PAN 4500 4 +1 1 MNT_ANGMAX_ROL 4500 4 +1 1 MNT_ANGMAX_TIL 4500 4 +1 1 MNT_ANGMIN_PAN -4500 4 +1 1 MNT_ANGMIN_ROL -4500 4 +1 1 MNT_ANGMIN_TIL -4500 4 +1 1 MNT_DEFLT_MODE 3 2 +1 1 MNT_JSTICK_SPD 0 2 +1 1 MNT_K_RATE 5 9 +1 1 MNT_LEAD_PTCH 0 9 +1 1 MNT_LEAD_RLL 0 9 +1 1 MNT_NEUTRAL_X 0 9 +1 1 MNT_NEUTRAL_Y 0 9 +1 1 MNT_NEUTRAL_Z 0 9 +1 1 MNT_OFF_ACC_X 0 9 +1 1 MNT_OFF_ACC_Y 0 9 +1 1 MNT_OFF_ACC_Z 0 9 +1 1 MNT_OFF_GYRO_X 0 9 +1 1 MNT_OFF_GYRO_Y 0 9 +1 1 MNT_OFF_GYRO_Z 0 9 +1 1 MNT_OFF_JNT_X 0 9 +1 1 MNT_OFF_JNT_Y 0 9 +1 1 MNT_OFF_JNT_Z 0 9 +1 1 MNT_RC_IN_PAN 0 2 +1 1 MNT_RC_IN_ROLL 0 2 +1 1 MNT_RC_IN_TILT 0 2 +1 1 MNT_RETRACT_X 0 9 +1 1 MNT_RETRACT_Y 0 9 +1 1 MNT_RETRACT_Z 0 9 +1 1 MNT_STAB_PAN 0 2 +1 1 MNT_STAB_ROLL 0 2 +1 1 MNT_STAB_TILT 0 2 +1 1 MNT_TYPE 0 2 +1 1 NAVL1_DAMPING 0.75 9 +1 1 NAVL1_PERIOD 20 9 +1 1 NAV_CONTROLLER 1 2 +1 1 OVERRIDE_CHAN 0 2 +1 1 PTCH2SRV_D 0.02 9 +1 1 PTCH2SRV_FF 0 9 +1 1 PTCH2SRV_I 0.04 9 +1 1 PTCH2SRV_IMAX 3000 4 +1 1 PTCH2SRV_P 0.4 9 +1 1 PTCH2SRV_RLL 1 9 +1 1 PTCH2SRV_RMAX_DN 0 4 +1 1 PTCH2SRV_RMAX_UP 0 4 +1 1 PTCH2SRV_TCONST 0.5 9 +1 1 RALLY_INCL_HOME 0 2 +1 1 RALLY_LIMIT_KM 5 9 +1 1 RALLY_TOTAL 0 2 +1 1 RC10_DZ 0 4 +1 1 RC10_FUNCTION 0 2 +1 1 RC10_MAX 1900 4 +1 1 RC10_MIN 1100 4 +1 1 RC10_REV 1 2 +1 1 RC10_TRIM 1500 4 +1 1 RC11_DZ 0 4 +1 1 RC11_FUNCTION 0 2 +1 1 RC11_MAX 1900 4 +1 1 RC11_MIN 1100 4 +1 1 RC11_REV 1 2 +1 1 RC11_TRIM 1500 4 +1 1 RC12_DZ 0 4 +1 1 RC12_FUNCTION 0 2 +1 1 RC12_MAX 1900 4 +1 1 RC12_MIN 1100 4 +1 1 RC12_REV 1 2 +1 1 RC12_TRIM 1500 4 +1 1 RC13_DZ 0 4 +1 1 RC13_FUNCTION 0 2 +1 1 RC13_MAX 1900 4 +1 1 RC13_MIN 1100 4 +1 1 RC13_REV 1 2 +1 1 RC13_TRIM 1500 4 +1 1 RC14_DZ 0 4 +1 1 RC14_FUNCTION 0 2 +1 1 RC14_MAX 1900 4 +1 1 RC14_MIN 1100 4 +1 1 RC14_REV 1 2 +1 1 RC14_TRIM 1500 4 +1 1 RC1_DZ 30 4 +1 1 RC1_MAX 1900 4 +1 1 RC1_MIN 1100 4 +1 1 RC1_REV 1 2 +1 1 RC1_TRIM 1500 4 +1 1 RC2_DZ 30 4 +1 1 RC2_MAX 1900 4 +1 1 RC2_MIN 1100 4 +1 1 RC2_REV 1 2 +1 1 RC2_TRIM 1500 4 +1 1 RC3_DZ 30 4 +1 1 RC3_MAX 1900 4 +1 1 RC3_MIN 1100 4 +1 1 RC3_REV 1 2 +1 1 RC3_TRIM 1100 4 +1 1 RC4_DZ 30 4 +1 1 RC4_MAX 1900 4 +1 1 RC4_MIN 1100 4 +1 1 RC4_REV 1 2 +1 1 RC4_TRIM 1500 4 +1 1 RC5_DZ 0 4 +1 1 RC5_FUNCTION 0 2 +1 1 RC5_MAX 1900 4 +1 1 RC5_MIN 1100 4 +1 1 RC5_REV 1 2 +1 1 RC5_TRIM 1500 4 +1 1 RC6_DZ 0 4 +1 1 RC6_FUNCTION 0 2 +1 1 RC6_MAX 1900 4 +1 1 RC6_MIN 1100 4 +1 1 RC6_REV 1 2 +1 1 RC6_TRIM 1500 4 +1 1 RC7_DZ 0 4 +1 1 RC7_FUNCTION 0 2 +1 1 RC7_MAX 1900 4 +1 1 RC7_MIN 1100 4 +1 1 RC7_REV 1 2 +1 1 RC7_TRIM 1500 4 +1 1 RC8_DZ 0 4 +1 1 RC8_FUNCTION 0 2 +1 1 RC8_MAX 1900 4 +1 1 RC8_MIN 1100 4 +1 1 RC8_REV 1 2 +1 1 RC8_TRIM 1500 4 +1 1 RC9_DZ 0 4 +1 1 RC9_FUNCTION 0 2 +1 1 RC9_MAX 1900 4 +1 1 RC9_MIN 1100 4 +1 1 RC9_REV 1 2 +1 1 RC9_TRIM 1500 4 +1 1 RCMAP_PITCH 2 2 +1 1 RCMAP_ROLL 1 2 +1 1 RCMAP_THROTTLE 3 2 +1 1 RCMAP_YAW 4 2 +1 1 RELAY_DEFAULT 0 2 +1 1 RELAY_PIN 54 2 +1 1 RELAY_PIN2 55 2 +1 1 RELAY_PIN3 -1 2 +1 1 RELAY_PIN4 -1 2 +1 1 RLL2SRV_D 0.02 9 +1 1 RLL2SRV_FF 0 9 +1 1 RLL2SRV_I 0.04 9 +1 1 RLL2SRV_IMAX 3000 4 +1 1 RLL2SRV_P 0.4 9 +1 1 RLL2SRV_RMAX 0 4 +1 1 RLL2SRV_TCONST 0.5 9 +1 1 RNGFND2_ADDR 0 2 +1 1 RNGFND2_FUNCTION 0 2 +1 1 RNGFND2_GNDCLEAR 10 2 +1 1 RNGFND2_MAX_CM 700 4 +1 1 RNGFND2_MIN_CM 20 4 +1 1 RNGFND2_OFFSET 0 9 +1 1 RNGFND2_PIN -1 2 +1 1 RNGFND2_RMETRIC 1 2 +1 1 RNGFND2_SCALING 3 9 +1 1 RNGFND2_SETTLE 0 4 +1 1 RNGFND2_STOP_PIN -1 2 +1 1 RNGFND2_TYPE 0 2 +1 1 RNGFND_ADDR 0 2 +1 1 RNGFND_FUNCTION 0 2 +1 1 RNGFND_GNDCLEAR 10 2 +1 1 RNGFND_LANDING 0 2 +1 1 RNGFND_MAX_CM 700 4 +1 1 RNGFND_MIN_CM 20 4 +1 1 RNGFND_OFFSET 0 9 +1 1 RNGFND_PIN -1 2 +1 1 RNGFND_PWRRNG 0 4 +1 1 RNGFND_RMETRIC 1 2 +1 1 RNGFND_SCALING 3 9 +1 1 RNGFND_SETTLE 0 4 +1 1 RNGFND_STOP_PIN -1 2 +1 1 RNGFND_TYPE 0 2 +1 1 RSSI_ANA_PIN 0 2 +1 1 RSSI_CHANNEL 0 2 +1 1 RSSI_CHAN_HIGH 2000 4 +1 1 RSSI_CHAN_LOW 1000 4 +1 1 RSSI_PIN_HIGH 5 9 +1 1 RSSI_PIN_LOW 0 9 +1 1 RSSI_TYPE 0 2 +1 1 RST_MISSION_CH 0 2 +1 1 RST_SWITCH_CH 0 2 +1 1 RTL_AUTOLAND 0 2 +1 1 RUDDER_ONLY 0 2 +1 1 SCALING_SPEED 15 9 +1 1 SCHED_DEBUG 0 2 +1 1 SERIAL0_BAUD 115 6 +1 1 SERIAL1_BAUD 57 6 +1 1 SERIAL1_PROTOCOL 1 2 +1 1 SERIAL2_BAUD 57 6 +1 1 SERIAL2_PROTOCOL 1 2 +1 1 SERIAL3_BAUD 38 6 +1 1 SERIAL3_PROTOCOL 5 2 +1 1 SERIAL4_BAUD 38 6 +1 1 SERIAL4_PROTOCOL 5 2 +1 1 SR0_EXTRA1 10 4 +1 1 SR0_EXTRA2 1 4 +1 1 SR0_EXTRA3 1 4 +1 1 SR0_EXT_STAT 2 4 +1 1 SR0_PARAMS 10 4 +1 1 SR0_POSITION 3 4 +1 1 SR0_RAW_CTRL 1 4 +1 1 SR0_RAW_SENS 2 4 +1 1 SR0_RC_CHAN 2 4 +1 1 SR1_EXTRA1 1 4 +1 1 SR1_EXTRA2 1 4 +1 1 SR1_EXTRA3 1 4 +1 1 SR1_EXT_STAT 1 4 +1 1 SR1_PARAMS 10 4 +1 1 SR1_POSITION 1 4 +1 1 SR1_RAW_CTRL 1 4 +1 1 SR1_RAW_SENS 1 4 +1 1 SR1_RC_CHAN 1 4 +1 1 SR2_EXTRA1 1 4 +1 1 SR2_EXTRA2 1 4 +1 1 SR2_EXTRA3 1 4 +1 1 SR2_EXT_STAT 1 4 +1 1 SR2_PARAMS 10 4 +1 1 SR2_POSITION 1 4 +1 1 SR2_RAW_CTRL 1 4 +1 1 SR2_RAW_SENS 1 4 +1 1 SR2_RC_CHAN 1 4 +1 1 SR3_EXTRA1 1 4 +1 1 SR3_EXTRA2 1 4 +1 1 SR3_EXTRA3 1 4 +1 1 SR3_EXT_STAT 1 4 +1 1 SR3_PARAMS 10 4 +1 1 SR3_POSITION 1 4 +1 1 SR3_RAW_CTRL 1 4 +1 1 SR3_RAW_SENS 1 4 +1 1 SR3_RC_CHAN 1 4 +1 1 STAB_PITCH_DOWN 2 9 +1 1 STALL_PREVENTION 1 2 +1 1 STEER2SRV_D 0.005 9 +1 1 STEER2SRV_FF 0 9 +1 1 STEER2SRV_I 0.2 9 +1 1 STEER2SRV_IMAX 1500 4 +1 1 STEER2SRV_MINSPD 1 9 +1 1 STEER2SRV_P 1.8 9 +1 1 STEER2SRV_TCONST 0.75 9 +1 1 STICK_MIXING 1 2 +1 1 SYSID_MYGCS 255 4 +1 1 SYSID_SW_TYPE 0 2 +1 1 SYSID_THISMAV 1 4 +1 1 SYS_NUM_RESETS 1 4 +1 1 TECS_CLMB_MAX 5 9 +1 1 TECS_HGT_OMEGA 3 9 +1 1 TECS_INTEG_GAIN 0.1 9 +1 1 TECS_LAND_ARSPD -1 9 +1 1 TECS_LAND_DAMP 0.5 9 +1 1 TECS_LAND_PMAX 10 2 +1 1 TECS_LAND_SINK 0.25 9 +1 1 TECS_LAND_SPDWGT 1 9 +1 1 TECS_LAND_TCONST 2 9 +1 1 TECS_LAND_THR -1 9 +1 1 TECS_PITCH_MAX 0 2 +1 1 TECS_PITCH_MIN 0 2 +1 1 TECS_PTCH_DAMP 0 9 +1 1 TECS_RLL2THR 10 9 +1 1 TECS_SINK_MAX 5 9 +1 1 TECS_SINK_MIN 2 9 +1 1 TECS_SPDWEIGHT 1 9 +1 1 TECS_SPD_OMEGA 2 9 +1 1 TECS_THR_DAMP 0.5 9 +1 1 TECS_TIME_CONST 5 9 +1 1 TECS_VERT_ACC 7 9 +1 1 TELEM_DELAY 0 2 +1 1 TERRAIN_ENABLE 1 2 +1 1 TERRAIN_FOLLOW 0 2 +1 1 TERRAIN_LOOKAHD 2000 4 +1 1 TERRAIN_SPACING 100 4 +1 1 THROTTLE_NUDGE 1 2 +1 1 THR_FAILSAFE 1 2 +1 1 THR_FS_VALUE 950 4 +1 1 THR_MAX 75 2 +1 1 THR_MIN 0 2 +1 1 THR_PASS_STAB 0 2 +1 1 THR_SLEWRATE 100 2 +1 1 THR_SUPP_MAN 0 2 +1 1 TKOFF_FLAP_PCNT 0 2 +1 1 TKOFF_ROTATE_SPD 0 9 +1 1 TKOFF_TDRAG_ELEV 0 2 +1 1 TKOFF_TDRAG_SPD1 0 9 +1 1 TKOFF_THR_DELAY 2 2 +1 1 TKOFF_THR_MAX 0 2 +1 1 TKOFF_THR_MINACC 0 9 +1 1 TKOFF_THR_MINSPD 0 9 +1 1 TKOFF_THR_SLEW 0 2 +1 1 TRIM_ARSPD_CM 1200 6 +1 1 TRIM_AUTO 0 2 +1 1 TRIM_PITCH_CD 0 4 +1 1 TRIM_RC_AT_START 0 2 +1 1 TRIM_THROTTLE 45 2 +1 1 VTAIL_OUTPUT 0 2 +1 1 WP_LOITER_RAD 60 4 +1 1 WP_MAX_RADIUS 0 4 +1 1 WP_RADIUS 90 4 +1 1 YAW2SRV_DAMP 0 9 +1 1 YAW2SRV_IMAX 1500 4 +1 1 YAW2SRV_INT 0 9 +1 1 YAW2SRV_RLL 1 9 +1 1 YAW2SRV_SLIP 0 9 diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index deadbd219bd1369ad452cff720f39157f7c643de..d221dc26e255633b0b062de16e496418b21b8415 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -187,6 +187,13 @@ void MAVLinkProtocol::_linkStatusChanged(LinkInterface* link, bool connected) // Use the same shared pointer as LinkManager _connectedLinks.append(LinkManager::instance()->sharedPointerForLink(link)); +#ifndef __mobile__ + if (_connectedLinks.count() == 1) { + // This is the first link, we need to start logging + _startLogging(); + } +#endif + // Send command to start MAVLink // XXX hacky but safe // Start NSH @@ -346,11 +353,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) #endif if (message.msgid == MAVLINK_MSG_ID_HEARTBEAT) { -#ifndef __mobile__ - // Start logging on first heartbeat - _startLogging(); -#endif - // Notify the vehicle manager of the heartbeat. This will create/update vehicles as needed. mavlink_heartbeat_t heartbeat; mavlink_msg_heartbeat_decode(&message, &heartbeat); @@ -644,20 +646,20 @@ bool MAVLinkProtocol::_closeLogFile(void) void MAVLinkProtocol::_startLogging(void) { - if (!_tempLogFile.isOpen()) { - if (!_logSuspendReplay) { - if (!_tempLogFile.open()) { - emit protocolStatusMessage(tr("MAVLink Protocol"), tr("Opening Flight Data file for writing failed. " - "Unable to write to %1. Please choose a different file location.").arg(_tempLogFile.fileName())); - _closeLogFile(); - _logSuspendError = true; - return; - } - - qDebug() << "Temp log" << _tempLogFile.fileName(); + Q_ASSERT(!_tempLogFile.isOpen()); - _logSuspendError = false; + if (!_logSuspendReplay) { + if (!_tempLogFile.open()) { + emit protocolStatusMessage(tr("MAVLink Protocol"), tr("Opening Flight Data file for writing failed. " + "Unable to write to %1. Please choose a different file location.").arg(_tempLogFile.fileName())); + _closeLogFile(); + _logSuspendError = true; + return; } + + qDebug() << "Temp log" << _tempLogFile.fileName(); + + _logSuspendError = false; } } diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 220d2db0bbba5d4afaae59844b70bd7d96a7eebe..b704198fa6b561bb7b7666e53125d9ae937d03ae 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -72,6 +72,7 @@ float MockLink::_vehicleLongitude = -122.08794f; float MockLink::_vehicleAltitude = 2.5f; const char* MockConfiguration::_firmwareTypeKey = "FirmwareType"; +const char* MockConfiguration::_vehicleTypeKey = "VehicleType"; const char* MockConfiguration::_sendStatusTextKey = "SendStatusText"; MockLink::MockLink(MockConfiguration* config) @@ -85,6 +86,7 @@ MockLink::MockLink(MockConfiguration* config) , _mavBaseMode(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) , _mavState(MAV_STATE_STANDBY) , _firmwareType(MAV_AUTOPILOT_PX4) + , _vehicleType(MAV_TYPE_QUADROTOR) , _fileServer(NULL) , _sendStatusText(false) , _apmSendHomePositionOnEmptyList(false) @@ -93,6 +95,7 @@ MockLink::MockLink(MockConfiguration* config) _config = config; if (_config) { _firmwareType = config->firmwareType(); + _vehicleType = config->vehicleType(); _sendStatusText = config->sendStatusText(); } @@ -113,7 +116,6 @@ MockLink::MockLink(MockConfiguration* config) MockLink::~MockLink(void) { - qDebug() << "MockLink destructor"; _disconnect(); } @@ -200,7 +202,18 @@ void MockLink::_run50HzTasks(void) void MockLink::_loadParams(void) { - QFile paramFile(":/unittest/MockLink.params"); + QFile paramFile; + + if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { + if (_vehicleType == MAV_TYPE_FIXED_WING) { + paramFile.setFileName(":/unittest/APMArduPlaneMockLink.params"); + } else { + paramFile.setFileName(":/unittest/APMArduCopterMockLink.params"); + } + } else { + paramFile.setFileName(":/unittest/PX4MockLink.params"); + } + bool success = paramFile.open(QFile::ReadOnly); Q_UNUSED(success); @@ -234,11 +247,21 @@ void MockLink::_loadParams(void) case MAV_PARAM_TYPE_INT32: paramValue = QVariant(valStr.toInt()); break; + case MAV_PARAM_TYPE_UINT16: + paramValue = QVariant((quint16)valStr.toUInt()); + break; + case MAV_PARAM_TYPE_INT16: + paramValue = QVariant((qint16)valStr.toInt()); + break; + case MAV_PARAM_TYPE_UINT8: + paramValue = QVariant((quint8)valStr.toUInt()); + break; case MAV_PARAM_TYPE_INT8: - paramValue = QVariant((unsigned char)valStr.toUInt()); + paramValue = QVariant((qint8)valStr.toUInt()); break; default: - Q_ASSERT(false); + qCritical() << "Unknown type" << paramType; + paramValue = QVariant(valStr.toInt()); break; } @@ -405,24 +428,38 @@ void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString& paramN QVariant paramVariant; switch (paramType) { - case MAV_PARAM_TYPE_INT8: - paramVariant = QVariant::fromValue(valueUnion.param_int8); - break; - - case MAV_PARAM_TYPE_INT32: - paramVariant = QVariant::fromValue(valueUnion.param_int32); - break; - - case MAV_PARAM_TYPE_UINT32: - paramVariant = QVariant::fromValue(valueUnion.param_uint32); - break; - - case MAV_PARAM_TYPE_REAL32: - paramVariant = QVariant::fromValue(valueUnion.param_float); - break; - - default: - qCritical() << "Invalid parameter type" << paramType; + case MAV_PARAM_TYPE_REAL32: + paramVariant = QVariant::fromValue(valueUnion.param_float); + break; + + case MAV_PARAM_TYPE_UINT32: + paramVariant = QVariant::fromValue(valueUnion.param_uint32); + break; + + case MAV_PARAM_TYPE_INT32: + paramVariant = QVariant::fromValue(valueUnion.param_int32); + break; + + case MAV_PARAM_TYPE_UINT16: + paramVariant = QVariant::fromValue(valueUnion.param_uint16); + break; + + case MAV_PARAM_TYPE_INT16: + paramVariant = QVariant::fromValue(valueUnion.param_int16); + break; + + case MAV_PARAM_TYPE_UINT8: + paramVariant = QVariant::fromValue(valueUnion.param_uint8); + break; + + case MAV_PARAM_TYPE_INT8: + paramVariant = QVariant::fromValue(valueUnion.param_int8); + break; + + default: + qCritical() << "Invalid parameter type" << paramType; + paramVariant = QVariant::fromValue(valueUnion.param_int32); + break; } qCDebug(MockLinkLog) << "_setParamFloatUnionIntoMap" << paramName << paramVariant; @@ -442,36 +479,65 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName) QVariant paramVar = _mapParamName2Value[componentId][paramName]; switch (paramType) { - case MAV_PARAM_TYPE_INT8: - if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { - valueUnion.param_float = (unsigned char)paramVar.toChar().toLatin1(); - } else { - valueUnion.param_int8 = (unsigned char)paramVar.toChar().toLatin1(); - } - break; + case MAV_PARAM_TYPE_REAL32: + valueUnion.param_float = paramVar.toFloat(); + break; - case MAV_PARAM_TYPE_INT32: - if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { - valueUnion.param_float = paramVar.toInt(); - } else { - valueUnion.param_int32 = paramVar.toInt(); - } - break; + case MAV_PARAM_TYPE_UINT32: + if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { + valueUnion.param_float = paramVar.toUInt(); + } else { + valueUnion.param_uint32 = paramVar.toUInt(); + } + break; - case MAV_PARAM_TYPE_UINT32: - if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { - valueUnion.param_float = paramVar.toUInt(); - } else { - valueUnion.param_uint32 = paramVar.toUInt(); - } - break; + case MAV_PARAM_TYPE_INT32: + if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { + valueUnion.param_float = paramVar.toInt(); + } else { + valueUnion.param_int32 = paramVar.toInt(); + } + break; - case MAV_PARAM_TYPE_REAL32: - valueUnion.param_float = paramVar.toFloat(); - break; + case MAV_PARAM_TYPE_UINT16: + if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { + valueUnion.param_float = paramVar.toUInt(); + } else { + valueUnion.param_uint16 = paramVar.toUInt(); + } + break; - default: - qCritical() << "Invalid parameter type" << paramType; + case MAV_PARAM_TYPE_INT16: + if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { + valueUnion.param_float = paramVar.toInt(); + } else { + valueUnion.param_int16 = paramVar.toInt(); + } + break; + + case MAV_PARAM_TYPE_UINT8: + if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { + valueUnion.param_float = paramVar.toUInt(); + } else { + valueUnion.param_uint8 = paramVar.toUInt(); + } + break; + + case MAV_PARAM_TYPE_INT8: + if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { + valueUnion.param_float = (unsigned char)paramVar.toChar().toLatin1(); + } else { + valueUnion.param_int8 = (unsigned char)paramVar.toChar().toLatin1(); + } + break; + + default: + if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { + valueUnion.param_float = paramVar.toInt(); + } else { + valueUnion.param_int32 = paramVar.toInt(); + } + qCritical() << "Invalid parameter type" << paramType; } return valueUnion.param_float; @@ -801,6 +867,7 @@ void MockLink::_sendStatusTextMessages(void) MockConfiguration::MockConfiguration(const QString& name) : LinkConfiguration(name) , _firmwareType(MAV_AUTOPILOT_PX4) + , _vehicleType(MAV_TYPE_QUADROTOR) , _sendStatusText(false) { @@ -810,6 +877,7 @@ MockConfiguration::MockConfiguration(MockConfiguration* source) : LinkConfiguration(source) { _firmwareType = source->_firmwareType; + _vehicleType = source->_vehicleType; _sendStatusText = source->_sendStatusText; } @@ -824,6 +892,7 @@ void MockConfiguration::copyFrom(LinkConfiguration *source) } _firmwareType = usource->_firmwareType; + _vehicleType = usource->_vehicleType; _sendStatusText = usource->_sendStatusText; } @@ -831,6 +900,7 @@ void MockConfiguration::saveSettings(QSettings& settings, const QString& root) { settings.beginGroup(root); settings.setValue(_firmwareTypeKey, (int)_firmwareType); + settings.setValue(_vehicleTypeKey, (int)_vehicleType); settings.setValue(_sendStatusTextKey, _sendStatusText); settings.sync(); settings.endGroup(); @@ -840,6 +910,7 @@ void MockConfiguration::loadSettings(QSettings& settings, const QString& root) { settings.beginGroup(root); _firmwareType = (MAV_AUTOPILOT)settings.value(_firmwareTypeKey, (int)MAV_AUTOPILOT_PX4).toInt(); + _vehicleType = (MAV_TYPE)settings.value(_vehicleTypeKey, (int)MAV_TYPE_QUADROTOR).toInt(); _sendStatusText = settings.value(_sendStatusTextKey, false).toBool(); settings.endGroup(); } diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index a78bbd238f10787d2a77a14acbbbaa4a92cd4d40..25fac11fb7cb24eb48960836ce5c031f04331619 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -44,6 +44,9 @@ public: MAV_AUTOPILOT firmwareType(void) { return _firmwareType; } void setFirmwareType(MAV_AUTOPILOT firmwareType) { _firmwareType = firmwareType; } + MAV_TYPE vehicleType(void) { return _vehicleType; } + void setVehicleType(MAV_TYPE vehicleType) { _vehicleType = vehicleType; } + /// @param sendStatusText true: mavlink status text messages will be sent for each severity, as well as voice output info message void setSendStatusText(bool sendStatusText) { _sendStatusText = sendStatusText; } bool sendStatusText(void) { return _sendStatusText; } @@ -57,9 +60,11 @@ public: private: MAV_AUTOPILOT _firmwareType; + MAV_TYPE _vehicleType; bool _sendStatusText; static const char* _firmwareTypeKey; + static const char* _vehicleTypeKey; static const char* _sendStatusTextKey; }; @@ -183,8 +188,9 @@ private: uint32_t _mavCustomMode; uint8_t _mavState; - MockConfiguration* _config; - MAV_AUTOPILOT _firmwareType; + MockConfiguration* _config; + MAV_AUTOPILOT _firmwareType; + MAV_TYPE _vehicleType; MockLinkFileServer* _fileServer; diff --git a/src/comm/MockLink.params b/src/comm/PX4MockLink.params similarity index 100% rename from src/comm/MockLink.params rename to src/comm/PX4MockLink.params diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index cf5e46b73c72fa3e864fc4d6dd36ac9a5b9c2889..2ec5d95fc6acdfaea2acaaabd8f82a8729fa67ad 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -84,15 +84,29 @@ This file is part of the QGROUNDCONTROL project const char* MAIN_SETTINGS_GROUP = "QGC_MAINWINDOW"; #ifndef __mobile__ -const char* MainWindow::_mavlinkDockWidgetName = "MAVLink Inspector"; -const char* MainWindow::_customCommandWidgetName = "Custom Command"; -const char* MainWindow::_filesDockWidgetName = "Onboard Files"; -const char* MainWindow::_uasStatusDetailsDockWidgetName = "Status Details"; -const char* MainWindow::_uasInfoViewDockWidgetName = "Info View"; -const char* MainWindow::_hilDockWidgetName = "HIL Config"; -const char* MainWindow::_analyzeDockWidgetName = "Analyze"; - -const char* MainWindow::_visibleWidgetsKey = "VisibleWidgets"; +enum DockWidgetTypes { + MAVLINK_INSPECTOR, + CUSTOM_COMMAND, + ONBOARD_FILES, + STATUS_DETAILS, + INFO_VIEW, + HIL_CONFIG, + ANALYZE +}; + +static const char *rgDockWidgetNames[] = { + "MAVLink Inspector", + "Custom Command", + "Onboard Files", + "Status Details", + "Info View", + "HIL Config", + "Analyze" +}; + +#define ARRAY_SIZE(ARRAY) (sizeof(ARRAY) / sizeof(ARRAY[0])) + +static const char* _visibleWidgetsKey = "VisibleWidgets"; #endif static MainWindow* _instance = NULL; ///< @brief MainWindow singleton @@ -318,27 +332,16 @@ void MainWindow::_buildCommonWidgets(void) logPlayer = new QGCMAVLinkLogPlayer(statusBar()); statusBar()->addPermanentWidget(logPlayer); - static const char* rgDockWidgetNames[] = { - _mavlinkDockWidgetName, - _customCommandWidgetName, - _filesDockWidgetName, - _uasStatusDetailsDockWidgetName, - _uasInfoViewDockWidgetName, - _hilDockWidgetName, - _analyzeDockWidgetName, - }; - static const size_t cDockWidgetNames = sizeof(rgDockWidgetNames) / sizeof(rgDockWidgetNames[0]); - - for (size_t i=0; isetCheckable(true); - action->setData(pDockWidgetName); + action->setData(i); connect(action, &QAction::triggered, this, &MainWindow::_showDockWidgetAction); _ui.menuWidgets->addAction(action); - _mapName2Action[pDockWidgetName] = action; } } @@ -365,28 +368,35 @@ void MainWindow::_showDockWidget(const QString& name, bool show) void MainWindow::_createInnerDockWidget(const QString& widgetName) { QGCDockWidget* widget = NULL; - - if (widgetName == _mavlinkDockWidgetName) { - widget = new QGCMAVLinkInspector(widgetName, _mapName2Action[widgetName], MAVLinkProtocol::instance(),this); - } else if (widgetName == _customCommandWidgetName) { - widget = new CustomCommandWidget(widgetName, _mapName2Action[widgetName], this); - } else if (widgetName == _filesDockWidgetName) { - widget = new QGCUASFileViewMulti(widgetName, _mapName2Action[widgetName], this); - } else if (widgetName == _uasStatusDetailsDockWidgetName) { - widget = new UASInfoWidget(widgetName, _mapName2Action[widgetName], this); - } else if (widgetName == _hilDockWidgetName) { - widget = new HILDockWidget(widgetName, _mapName2Action[widgetName], this); - } else if (widgetName == _analyzeDockWidgetName) { - widget = new Linecharts(widgetName, _mapName2Action[widgetName], mavlinkDecoder, this); - } else if (widgetName == _uasInfoViewDockWidgetName) { - QGCTabbedInfoView* pInfoView = new QGCTabbedInfoView(widgetName, _mapName2Action[widgetName], this); - pInfoView->addSource(mavlinkDecoder); - widget = pInfoView; - } else { - qWarning() << "Attempt to create unknown Inner Dock Widget" << widgetName; - return; + QAction *action = _mapName2Action[widgetName]; + + switch(action->data().toInt()) { + case MAVLINK_INSPECTOR: + widget = new QGCMAVLinkInspector(widgetName, action, MAVLinkProtocol::instance(),this); + break; + case CUSTOM_COMMAND: + widget = new CustomCommandWidget(widgetName, action, this); + break; + case ONBOARD_FILES: + widget = new QGCUASFileViewMulti(widgetName, action, this); + break; + case STATUS_DETAILS: + widget = new UASInfoWidget(widgetName, action, this); + break; + case HIL_CONFIG: + widget = new HILDockWidget(widgetName, action, this); + break; + case ANALYZE: + widget = new Linecharts(widgetName, action, mavlinkDecoder, this); + break; + case INFO_VIEW: + widget= new QGCTabbedInfoView(widgetName, action, this); + break; } + if(action->data().toInt() == INFO_VIEW) { + qobject_cast(widget)->addSource(mavlinkDecoder); + } _mapName2DockWidget[widgetName] = widget; } @@ -399,19 +409,23 @@ void MainWindow::_hideAllDockWidgets(void) void MainWindow::_showDockWidgetAction(bool show) { - QAction* action = dynamic_cast(QObject::sender()); + QAction* action = qobject_cast(QObject::sender()); Q_ASSERT(action); - _showDockWidget(action->text(), show); + _showDockWidget(rgDockWidgetNames[action->data().toInt()], show); } #endif void MainWindow::fullScreenActionItemCallback(bool) { + if (!_ui.actionFullscreen->isChecked()) + _ui.actionFullscreen->setChecked(true); _ui.actionNormal->setChecked(false); } void MainWindow::normalActionItemCallback(bool) { + if (!_ui.actionNormal->isChecked()) + _ui.actionNormal->setChecked(true); _ui.actionFullscreen->setChecked(false); } diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 5f767decd36b4913bf51c8f390ae47d396b5757a..6f8c9cba95cfa7263675a3ff114daedee5551d7c 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -227,15 +227,6 @@ private: QPointer _missionEditorView; #ifndef __mobile__ - // Dock widget names - static const char* _mavlinkDockWidgetName; - static const char* _customCommandWidgetName; - static const char* _filesDockWidgetName; - static const char* _uasStatusDetailsDockWidgetName; - static const char* _uasInfoViewDockWidgetName; - static const char* _hilDockWidgetName; - static const char* _analyzeDockWidgetName; - QMap _mapName2DockWidget; QMap _mapName2Action; #endif @@ -250,8 +241,6 @@ private: void _showDockWidget(const QString &name, bool show); void _loadVisibleWidgetsSettings(void); void _storeVisibleWidgetsSettings(void); - - static const char* _visibleWidgetsKey; #endif bool _autoReconnect; diff --git a/src/ui/MockLinkConfiguration.cc b/src/ui/MockLinkConfiguration.cc index 3c8e05d2642e03ce26acbce43ca9b47082225fe6..cccc75f4c7fb348292816cc930c517ab136ffa30 100644 --- a/src/ui/MockLinkConfiguration.cc +++ b/src/ui/MockLinkConfiguration.cc @@ -43,11 +43,23 @@ MockLinkConfiguration::MockLinkConfiguration(MockConfiguration *config, QWidget break; } + switch (config->vehicleType()) { + case MAV_TYPE_FIXED_WING: + _ui->apmArduPlaneRadio->setChecked(true); + break; + default: + _ui->apmArduCopterRadio->setChecked(true); + break; + } + _ui->sendStatusTextCheckBox->setChecked(config->sendStatusText()); connect(_ui->px4Radio, &QRadioButton::clicked, this, &MockLinkConfiguration::_px4RadioClicked); connect(_ui->apmRadio, &QRadioButton::clicked, this, &MockLinkConfiguration::_apmRadioClicked); connect(_ui->genericRadio, &QRadioButton::clicked, this, &MockLinkConfiguration::_genericRadioClicked); + connect(_ui->apmArduCopterRadio, &QRadioButton::clicked, this, &MockLinkConfiguration::_apmArduCopterRadioClicked); + connect(_ui->apmArduPlaneRadio, &QRadioButton::clicked, this, &MockLinkConfiguration::_apmArduPlaneRadioClicked); + connect(_ui->genericRadio, &QRadioButton::clicked, this, &MockLinkConfiguration::_genericRadioClicked); connect(_ui->sendStatusTextCheckBox, &QCheckBox::clicked, this, &MockLinkConfiguration::_sendStatusTextClicked); } @@ -77,6 +89,20 @@ void MockLinkConfiguration::_genericRadioClicked(bool checked) } } +void MockLinkConfiguration::_apmArduCopterRadioClicked(bool checked) +{ + if (checked) { + _config->setVehicleType(MAV_TYPE_QUADROTOR); + } +} + +void MockLinkConfiguration::_apmArduPlaneRadioClicked(bool checked) +{ + if (checked) { + _config->setVehicleType(MAV_TYPE_FIXED_WING); + } +} + void MockLinkConfiguration::_sendStatusTextClicked(bool checked) { _config->setSendStatusText(checked); diff --git a/src/ui/MockLinkConfiguration.h b/src/ui/MockLinkConfiguration.h index 61e55db10dcee1a6d264861200cc6ed0eb0a86dc..e60a18781d24d06bbcb2fd0bf47caa5c8f5d72de 100644 --- a/src/ui/MockLinkConfiguration.h +++ b/src/ui/MockLinkConfiguration.h @@ -44,6 +44,8 @@ private slots: void _px4RadioClicked(bool checked); void _apmRadioClicked(bool checked); void _genericRadioClicked(bool checked); + void _apmArduCopterRadioClicked(bool checked); + void _apmArduPlaneRadioClicked(bool checked); void _sendStatusTextClicked(bool checked); private: diff --git a/src/ui/MockLinkConfiguration.ui b/src/ui/MockLinkConfiguration.ui index 1519985366115e7d0c045bb3757888494da11d36..83c6e220785d68a8870d6023342401c1be563cbd 100644 --- a/src/ui/MockLinkConfiguration.ui +++ b/src/ui/MockLinkConfiguration.ui @@ -6,8 +6,8 @@ 0 0 - 187 - 116 + 238 + 188 @@ -42,6 +42,29 @@ + + + + APM vehicle type + + + + + + ArduCopter + + + + + + + ArduPlane + + + + + +