Skip to content

[Bug] vision fuse data can not received by LOCAL_POSITION_NED #24868

Open
@1OngJ

Description

@1OngJ

Describe the bug

I attempted to use the odometry from VINS_fusion as the external positioning data for the drone and forwarded the odometry topic to /mavros/vision_pose/pose through the vins_to_mavros node. However, /mavros/local_position/pose did not correctly receive the information from the odometry topic; it still positioned based on the drone's internal data.
I used to use the version of 1.14.3 and I can't find the EV option in param tree , but I set the EKF2_AID_MASK to 24 and HGT_MODE to vision.
Now my fireware version is 1.15.4 .It still not well.

My onboard device is :

Orangepi 5b

This my param :

 Stack: PX4 Pro
Version: 1.15.4 
 Git Revision: 99c40407ff000000
 Vehicle-Id Component-Id Name Value Type
1	1	ATT_EN	0	6
1	1	BAT1_A_PER_V	59.500000000000000000	9
1	1	BAT1_CAPACITY	-1.000000000000000000	9
1	1	BAT1_I_CHANNEL	-1	6
1	1	BAT1_N_CELLS	4	6
1	1	BAT1_R_INTERNAL	0.004999999888241291	9
1	1	BAT1_SOURCE	0	6
1	1	BAT1_V_CHANNEL	-1	6
1	1	BAT1_V_CHARGED	4.050000190734863281	9
1	1	BAT1_V_DIV	11.199999809265136719	9
1	1	BAT1_V_EMPTY	3.599999904632568359	9
1	1	BAT1_V_LOAD_DROP	0.100000001490116119	9
1	1	BATMON_DRIVER_EN	0	6
1	1	BAT_AVRG_CURRENT	15.000000000000000000	9
1	1	BAT_CRIT_THR	0.070000000298023224	9
1	1	BAT_EMERGEN_THR	0.050000000745058060	9
1	1	BAT_LOW_THR	0.150000005960464478	9
1	1	BAT_N_CELLS	3	6
1	1	BAT_V_CHARGED	4.050000190734863281	9
1	1	BAT_V_EMPTY	3.599999904632568359	9
1	1	BAT_V_LOAD_DROP	0.300000011920928955	9
1	1	BAT_V_OFFS_CURR	0.000000000000000000	9
1	1	CAL_ACC0_ID	3604514	6
1	1	CAL_ACC0_PRIO	50	6
1	1	CAL_ACC0_ROT	-1	6
1	1	CAL_ACC0_XOFF	-0.300301909446716309	9
1	1	CAL_ACC0_XSCALE	1.004739999771118164	9
1	1	CAL_ACC0_YOFF	-1.320188045501708984	9
1	1	CAL_ACC0_YSCALE	0.992894232273101807	9
1	1	CAL_ACC0_ZOFF	0.070676490664482117	9
1	1	CAL_ACC0_ZSCALE	0.988390207290649414	9
1	1	CAL_ACC1_ID	0	6
1	1	CAL_ACC2_ID	0	6
1	1	CAL_ACC3_ID	0	6
1	1	CAL_AIR_CMODEL	0	6
1	1	CAL_AIR_TUBED_MM	1.500000000000000000	9
1	1	CAL_AIR_TUBELEN	0.200000002980232239	9
1	1	CAL_BARO0_ID	0	6
1	1	CAL_BARO1_ID	0	6
1	1	CAL_BARO2_ID	0	6
1	1	CAL_BARO3_ID	0	6
1	1	CAL_GYRO0_ID	3604514	6
1	1	CAL_GYRO0_PRIO	50	6
1	1	CAL_GYRO0_ROT	-1	6
1	1	CAL_GYRO0_XOFF	-0.004195139277726412	9
1	1	CAL_GYRO0_YOFF	0.004082217812538147	9
1	1	CAL_GYRO0_ZOFF	-0.000500689551699907	9
1	1	CAL_GYRO1_ID	0	6
1	1	CAL_GYRO2_ID	0	6
1	1	CAL_GYRO3_ID	0	6
1	1	CAL_MAG0_ID	0	6
1	1	CAL_MAG0_ROT	-1	6
1	1	CAL_MAG1_ID	0	6
1	1	CAL_MAG1_ROT	-1	6
1	1	CAL_MAG2_ID	0	6
1	1	CAL_MAG2_ROT	-1	6
1	1	CAL_MAG3_ID	0	6
1	1	CAL_MAG3_ROT	-1	6
1	1	CAM_CAP_FBACK	0	6
1	1	CA_AIRFRAME	0	6
1	1	CA_FAILURE_MODE	0	6
1	1	CA_METHOD	2	6
1	1	CA_R0_SLEW	0.000000000000000000	9
1	1	CA_R10_SLEW	0.000000000000000000	9
1	1	CA_R11_SLEW	0.000000000000000000	9
1	1	CA_R1_SLEW	0.000000000000000000	9
1	1	CA_R2_SLEW	0.000000000000000000	9
1	1	CA_R3_SLEW	0.000000000000000000	9
1	1	CA_R4_SLEW	0.000000000000000000	9
1	1	CA_R5_SLEW	0.000000000000000000	9
1	1	CA_R6_SLEW	0.000000000000000000	9
1	1	CA_R7_SLEW	0.000000000000000000	9
1	1	CA_R8_SLEW	0.000000000000000000	9
1	1	CA_R9_SLEW	0.000000000000000000	9
1	1	CA_ROTOR0_AX	0.000000000000000000	9
1	1	CA_ROTOR0_AY	0.000000000000000000	9
1	1	CA_ROTOR0_AZ	-1.000000000000000000	9
1	1	CA_ROTOR0_CT	6.500000000000000000	9
1	1	CA_ROTOR0_KM	0.050000000745058060	9
1	1	CA_ROTOR0_PX	1.000000000000000000	9
1	1	CA_ROTOR0_PY	1.000000000000000000	9
1	1	CA_ROTOR0_PZ	0.000000000000000000	9
1	1	CA_ROTOR10_AX	0.000000000000000000	9
1	1	CA_ROTOR10_AY	0.000000000000000000	9
1	1	CA_ROTOR10_AZ	-1.000000000000000000	9
1	1	CA_ROTOR10_CT	6.500000000000000000	9
1	1	CA_ROTOR10_KM	0.050000000745058060	9
1	1	CA_ROTOR10_PX	0.000000000000000000	9
1	1	CA_ROTOR10_PY	0.000000000000000000	9
1	1	CA_ROTOR10_PZ	0.000000000000000000	9
1	1	CA_ROTOR11_AX	0.000000000000000000	9
1	1	CA_ROTOR11_AY	0.000000000000000000	9
1	1	CA_ROTOR11_AZ	-1.000000000000000000	9
1	1	CA_ROTOR11_CT	6.500000000000000000	9
1	1	CA_ROTOR11_KM	0.050000000745058060	9
1	1	CA_ROTOR11_PX	0.000000000000000000	9
1	1	CA_ROTOR11_PY	0.000000000000000000	9
1	1	CA_ROTOR11_PZ	0.000000000000000000	9
1	1	CA_ROTOR1_AX	0.000000000000000000	9
1	1	CA_ROTOR1_AY	0.000000000000000000	9
1	1	CA_ROTOR1_AZ	-1.000000000000000000	9
1	1	CA_ROTOR1_CT	6.500000000000000000	9
1	1	CA_ROTOR1_KM	0.050000000745058060	9
1	1	CA_ROTOR1_PX	-1.000000000000000000	9
1	1	CA_ROTOR1_PY	-1.000000000000000000	9
1	1	CA_ROTOR1_PZ	0.000000000000000000	9
1	1	CA_ROTOR2_AX	0.000000000000000000	9
1	1	CA_ROTOR2_AY	0.000000000000000000	9
1	1	CA_ROTOR2_AZ	-1.000000000000000000	9
1	1	CA_ROTOR2_CT	6.500000000000000000	9
1	1	CA_ROTOR2_KM	-0.050000000745058060	9
1	1	CA_ROTOR2_PX	1.000000000000000000	9
1	1	CA_ROTOR2_PY	-1.000000000000000000	9
1	1	CA_ROTOR2_PZ	0.000000000000000000	9
1	1	CA_ROTOR3_AX	0.000000000000000000	9
1	1	CA_ROTOR3_AY	0.000000000000000000	9
1	1	CA_ROTOR3_AZ	-1.000000000000000000	9
1	1	CA_ROTOR3_CT	6.500000000000000000	9
1	1	CA_ROTOR3_KM	-0.050000000745058060	9
1	1	CA_ROTOR3_PX	-1.000000000000000000	9
1	1	CA_ROTOR3_PY	1.000000000000000000	9
1	1	CA_ROTOR3_PZ	0.000000000000000000	9
1	1	CA_ROTOR4_AX	0.000000000000000000	9
1	1	CA_ROTOR4_AY	0.000000000000000000	9
1	1	CA_ROTOR4_AZ	-1.000000000000000000	9
1	1	CA_ROTOR4_CT	6.500000000000000000	9
1	1	CA_ROTOR4_KM	0.050000000745058060	9
1	1	CA_ROTOR4_PX	0.000000000000000000	9
1	1	CA_ROTOR4_PY	0.000000000000000000	9
1	1	CA_ROTOR4_PZ	0.000000000000000000	9
1	1	CA_ROTOR5_AX	0.000000000000000000	9
1	1	CA_ROTOR5_AY	0.000000000000000000	9
1	1	CA_ROTOR5_AZ	-1.000000000000000000	9
1	1	CA_ROTOR5_CT	6.500000000000000000	9
1	1	CA_ROTOR5_KM	0.050000000745058060	9
1	1	CA_ROTOR5_PX	0.000000000000000000	9
1	1	CA_ROTOR5_PY	0.000000000000000000	9
1	1	CA_ROTOR5_PZ	0.000000000000000000	9
1	1	CA_ROTOR6_AX	0.000000000000000000	9
1	1	CA_ROTOR6_AY	0.000000000000000000	9
1	1	CA_ROTOR6_AZ	-1.000000000000000000	9
1	1	CA_ROTOR6_CT	6.500000000000000000	9
1	1	CA_ROTOR6_KM	0.050000000745058060	9
1	1	CA_ROTOR6_PX	0.000000000000000000	9
1	1	CA_ROTOR6_PY	0.000000000000000000	9
1	1	CA_ROTOR6_PZ	0.000000000000000000	9
1	1	CA_ROTOR7_AX	0.000000000000000000	9
1	1	CA_ROTOR7_AY	0.000000000000000000	9
1	1	CA_ROTOR7_AZ	-1.000000000000000000	9
1	1	CA_ROTOR7_CT	6.500000000000000000	9
1	1	CA_ROTOR7_KM	0.050000000745058060	9
1	1	CA_ROTOR7_PX	0.000000000000000000	9
1	1	CA_ROTOR7_PY	0.000000000000000000	9
1	1	CA_ROTOR7_PZ	0.000000000000000000	9
1	1	CA_ROTOR8_AX	0.000000000000000000	9
1	1	CA_ROTOR8_AY	0.000000000000000000	9
1	1	CA_ROTOR8_AZ	-1.000000000000000000	9
1	1	CA_ROTOR8_CT	6.500000000000000000	9
1	1	CA_ROTOR8_KM	0.050000000745058060	9
1	1	CA_ROTOR8_PX	0.000000000000000000	9
1	1	CA_ROTOR8_PY	0.000000000000000000	9
1	1	CA_ROTOR8_PZ	0.000000000000000000	9
1	1	CA_ROTOR9_AX	0.000000000000000000	9
1	1	CA_ROTOR9_AY	0.000000000000000000	9
1	1	CA_ROTOR9_AZ	-1.000000000000000000	9
1	1	CA_ROTOR9_CT	6.500000000000000000	9
1	1	CA_ROTOR9_KM	0.050000000745058060	9
1	1	CA_ROTOR9_PX	0.000000000000000000	9
1	1	CA_ROTOR9_PY	0.000000000000000000	9
1	1	CA_ROTOR9_PZ	0.000000000000000000	9
1	1	CA_ROTOR_COUNT	4	6
1	1	CA_R_REV	0	6
1	1	CA_SV0_SLEW	0.000000000000000000	9
1	1	CA_SV1_SLEW	0.000000000000000000	9
1	1	CA_SV2_SLEW	0.000000000000000000	9
1	1	CA_SV3_SLEW	0.000000000000000000	9
1	1	CA_SV4_SLEW	0.000000000000000000	9
1	1	CA_SV5_SLEW	0.000000000000000000	9
1	1	CA_SV6_SLEW	0.000000000000000000	9
1	1	CA_SV7_SLEW	0.000000000000000000	9
1	1	CBRK_BUZZER	782090	6
1	1	CBRK_FLIGHTTERM	121212	6
1	1	CBRK_IO_SAFETY	22027	6
1	1	CBRK_SUPPLY_CHK	894281	6
1	1	CBRK_USB_CHK	197848	6
1	1	CBRK_VTOLARMING	0	6
1	1	COM_ACT_FAIL_ACT	0	6
1	1	COM_ARMABLE	1	6
1	1	COM_ARM_AUTH_ID	10	6
1	1	COM_ARM_AUTH_MET	0	6
1	1	COM_ARM_AUTH_REQ	0	6
1	1	COM_ARM_AUTH_TO	1.000000000000000000	9
1	1	COM_ARM_BAT_MIN	0.000000000000000000	9
1	1	COM_ARM_CHK_ESCS	0	6
1	1	COM_ARM_EKF_HGT	1.000000000000000000	9
1	1	COM_ARM_EKF_POS	0.500000000000000000	9
1	1	COM_ARM_EKF_VEL	0.500000000000000000	9
1	1	COM_ARM_EKF_YAW	0.500000000000000000	9
1	1	COM_ARM_HFLT_CHK	1	6
1	1	COM_ARM_IMU_ACC	0.699999988079071045	9
1	1	COM_ARM_IMU_GYR	0.250000000000000000	9
1	1	COM_ARM_MAG_ANG	60	6
1	1	COM_ARM_MAG_STR	2	6
1	1	COM_ARM_MIS_REQ	0	6
1	1	COM_ARM_ODID	0	6
1	1	COM_ARM_SDCARD	0	6
1	1	COM_ARM_SWISBTN	0	6
1	1	COM_ARM_WO_GPS	1	6
1	1	COM_CPU_MAX	95.000000000000000000	9
1	1	COM_DISARM_LAND	2.000000000000000000	9
1	1	COM_DISARM_MAN	1	6
1	1	COM_DISARM_PRFLT	10.000000000000000000	9
1	1	COM_DL_LOSS_T	10	6
1	1	COM_FAIL_ACT_T	5.000000000000000000	9
1	1	COM_FLIGHT_UUID	453	6
1	1	COM_FLTMODE1	8	6
1	1	COM_FLTMODE2	-1	6
1	1	COM_FLTMODE3	-1	6
1	1	COM_FLTMODE4	1	6
1	1	COM_FLTMODE5	-1	6
1	1	COM_FLTMODE6	2	6
1	1	COM_FLTT_LOW_ACT	3	6
1	1	COM_FLT_PROFILE	0	6
1	1	COM_FLT_TIME_MAX	-1	6
1	1	COM_FORCE_SAFETY	0	6
1	1	COM_HLDL_LOSS_T	120	6
1	1	COM_HLDL_REG_T	0	6
1	1	COM_HOME_EN	1	6
1	1	COM_HOME_IN_AIR	0	6
1	1	COM_IMB_PROP_ACT	0	6
1	1	COM_KILL_DISARM	5.000000000000000000	9
1	1	COM_LKDOWN_TKO	3.000000000000000000	9
1	1	COM_LOW_BAT_ACT	0	6
1	1	COM_MOT_TEST_EN	1	6
1	1	COM_OBC_LOSS_T	5.000000000000000000	9
1	1	COM_OBL_RC_ACT	0	6
1	1	COM_OBS_AVOID	0	6
1	1	COM_OF_LOSS_T	5.000000000000000000	9
1	1	COM_PARACHUTE	0	6
1	1	COM_POSCTL_NAVL	0	6
1	1	COM_POS_FS_DELAY	1	6
1	1	COM_POS_FS_EPH	5.000000000000000000	9
1	1	COM_POS_LOW_EPH	-1.000000000000000000	9
1	1	COM_POWER_COUNT	1	6
1	1	COM_PREARM_MODE	0	6
1	1	COM_QC_ACT	0	6
1	1	COM_RCL_EXCEPT	4	6
1	1	COM_RC_ARM_HYST	1000	6
1	1	COM_RC_IN_MODE	3	6
1	1	COM_RC_LOSS_T	0.500000000000000000	9
1	1	COM_RC_OVERRIDE	1	6
1	1	COM_RC_STICK_OV	30.000000000000000000	9
1	1	COM_SPOOLUP_TIME	1.000000000000000000	9
1	1	COM_TAKEOFF_ACT	0	6
1	1	COM_THROW_EN	0	6
1	1	COM_THROW_SPEED	5.000000000000000000	9
1	1	COM_VEL_FS_EVH	1.000000000000000000	9
1	1	COM_WIND_MAX	-1.000000000000000000	9
1	1	COM_WIND_MAX_ACT	0	6
1	1	COM_WIND_WARN	-1.000000000000000000	9
1	1	CP_DELAY	0.400000005960464478	9
1	1	CP_DIST	-1.000000000000000000	9
1	1	CP_GO_NO_DATA	0	6
1	1	CP_GUIDE_ANG	30.000000000000000000	9
1	1	DSHOT_3D_DEAD_H	1000	6
1	1	DSHOT_3D_DEAD_L	1000	6
1	1	DSHOT_3D_ENABLE	0	6
1	1	DSHOT_MIN	0.054999999701976776	9
1	1	DSHOT_TEL_CFG	0	6
1	1	EKF2_ABIAS_INIT	0.200000002980232239	9
1	1	EKF2_ABL_ACCLIM	25.000000000000000000	9
1	1	EKF2_ABL_GYRLIM	3.000000000000000000	9
1	1	EKF2_ABL_LIM	0.400000005960464478	9
1	1	EKF2_ABL_TAU	0.500000000000000000	9
1	1	EKF2_ACC_B_NOISE	0.003000000026077032	9
1	1	EKF2_ACC_NOISE	0.349999994039535522	9
1	1	EKF2_ANGERR_INIT	0.100000001490116119	9
1	1	EKF2_BARO_CTRL	0	6
1	1	EKF2_BARO_DELAY	0.000000000000000000	9
1	1	EKF2_BARO_GATE	5.000000000000000000	9
1	1	EKF2_BARO_NOISE	3.500000000000000000	9
1	1	EKF2_DECL_TYPE	3	6
1	1	EKF2_DELAY_MAX	200.000000000000000000	9
1	1	EKF2_EN	1	6
1	1	EKF2_GBIAS_INIT	0.100000001490116119	9
1	1	EKF2_GND_EFF_DZ	4.000000000000000000	9
1	1	EKF2_GND_MAX_HGT	0.500000000000000000	9
1	1	EKF2_GPS_CHECK	245	6
1	1	EKF2_GPS_CTRL	7	6
1	1	EKF2_GPS_DELAY	110.000000000000000000	9
1	1	EKF2_GPS_POS_X	0.000000000000000000	9
1	1	EKF2_GPS_POS_Y	0.000000000000000000	9
1	1	EKF2_GPS_POS_Z	0.000000000000000000	9
1	1	EKF2_GPS_P_GATE	5.000000000000000000	9
1	1	EKF2_GPS_P_NOISE	0.500000000000000000	9
1	1	EKF2_GPS_V_GATE	5.000000000000000000	9
1	1	EKF2_GPS_V_NOISE	0.300000011920928955	9
1	1	EKF2_GRAV_NOISE	1.000000000000000000	9
1	1	EKF2_GSF_TAS	15.000000000000000000	9
1	1	EKF2_GYR_B_LIM	0.150000005960464478	9
1	1	EKF2_GYR_B_NOISE	0.001000000047497451	9
1	1	EKF2_GYR_NOISE	0.014999999664723873	9
1	1	EKF2_HDG_GATE	2.599999904632568359	9
1	1	EKF2_HEAD_NOISE	0.300000011920928955	9
1	1	EKF2_HGT_REF	3	6
1	1	EKF2_IMU_CTRL	1	6
1	1	EKF2_IMU_POS_X	0.000000000000000000	9
1	1	EKF2_IMU_POS_Y	0.000000000000000000	9
1	1	EKF2_IMU_POS_Z	0.000000000000000000	9
1	1	EKF2_MAG_ACCLIM	0.500000000000000000	9
1	1	EKF2_MAG_B_NOISE	0.000099999997473788	9
1	1	EKF2_MAG_CHECK	1	6
1	1	EKF2_MAG_CHK_INC	20.000000000000000000	9
1	1	EKF2_MAG_CHK_STR	0.200000002980232239	9
1	1	EKF2_MAG_DECL	0.000000000000000000	9
1	1	EKF2_MAG_DELAY	0.000000000000000000	9
1	1	EKF2_MAG_E_NOISE	0.001000000047497451	9
1	1	EKF2_MAG_GATE	3.000000000000000000	9
1	1	EKF2_MAG_NOISE	0.050000000745058060	9
1	1	EKF2_MAG_TYPE	5	6
1	1	EKF2_MIN_RNG	0.100000001490116119	9
1	1	EKF2_MULTI_IMU	3	6
1	1	EKF2_NOAID_NOISE	10.000000000000000000	9
1	1	EKF2_NOAID_TOUT	5000000	6
1	1	EKF2_OF_CTRL	0	6
1	1	EKF2_OF_DELAY	20.000000000000000000	9
1	1	EKF2_OF_GATE	3.000000000000000000	9
1	1	EKF2_OF_N_MAX	0.500000000000000000	9
1	1	EKF2_OF_N_MIN	0.150000005960464478	9
1	1	EKF2_OF_POS_X	0.000000000000000000	9
1	1	EKF2_OF_POS_Y	0.000000000000000000	9
1	1	EKF2_OF_POS_Z	0.000000000000000000	9
1	1	EKF2_OF_QMIN	1	6
1	1	EKF2_OF_QMIN_GND	0	6
1	1	EKF2_PREDICT_US	10000	6
1	1	EKF2_REQ_EPH	3.000000000000000000	9
1	1	EKF2_REQ_EPV	5.000000000000000000	9
1	1	EKF2_REQ_GPS_H	10.000000000000000000	9
1	1	EKF2_REQ_HDRIFT	0.100000001490116119	9
1	1	EKF2_REQ_NSATS	6	6
1	1	EKF2_REQ_PDOP	2.500000000000000000	9
1	1	EKF2_REQ_SACC	0.500000000000000000	9
1	1	EKF2_REQ_VDRIFT	0.200000002980232239	9
1	1	EKF2_RNG_A_HMAX	5.000000000000000000	9
1	1	EKF2_RNG_A_IGATE	1.000000000000000000	9
1	1	EKF2_RNG_A_VMAX	1.000000000000000000	9
1	1	EKF2_RNG_CTRL	1	6
1	1	EKF2_RNG_DELAY	5.000000000000000000	9
1	1	EKF2_RNG_GATE	5.000000000000000000	9
1	1	EKF2_RNG_K_GATE	1.000000000000000000	9
1	1	EKF2_RNG_NOISE	0.100000001490116119	9
1	1	EKF2_RNG_PITCH	0.000000000000000000	9
1	1	EKF2_RNG_POS_X	0.000000000000000000	9
1	1	EKF2_RNG_POS_Y	0.000000000000000000	9
1	1	EKF2_RNG_POS_Z	0.000000000000000000	9
1	1	EKF2_RNG_QLTY_T	1.000000000000000000	9
1	1	EKF2_RNG_SFE	0.050000000745058060	9
1	1	EKF2_SEL_ERR_RED	0.200000002980232239	9
1	1	EKF2_SEL_IMU_ACC	1.000000000000000000	9
1	1	EKF2_SEL_IMU_ANG	15.000000000000000000	9
1	1	EKF2_SEL_IMU_RAT	7.000000000000000000	9
1	1	EKF2_SEL_IMU_VEL	2.000000000000000000	9
1	1	EKF2_SYNT_MAG_Z	0	6
1	1	EKF2_TAU_POS	0.250000000000000000	9
1	1	EKF2_TAU_VEL	0.250000000000000000	9
1	1	EKF2_TERR_GRAD	0.500000000000000000	9
1	1	EKF2_TERR_MASK	3	6
1	1	EKF2_TERR_NOISE	5.000000000000000000	9
1	1	EKF2_WIND_NSD	0.050000000745058060	9
1	1	EV_TSK_RC_LOSS	0	6
1	1	EV_TSK_STAT_DIS	0	6
1	1	FD_ACT_EN	1	6
1	1	FD_ACT_MOT_C2T	2.000000000000000000	9
1	1	FD_ACT_MOT_THR	0.200000002980232239	9
1	1	FD_ACT_MOT_TOUT	100	6
1	1	FD_ESCS_EN	1	6
1	1	FD_EXT_ATS_EN	0	6
1	1	FD_EXT_ATS_TRIG	1900	6
1	1	FD_FAIL_P	60	6
1	1	FD_FAIL_P_TTRI	0.300000011920928955	9
1	1	FD_FAIL_R	60	6
1	1	FD_FAIL_R_TTRI	0.300000011920928955	9
1	1	FD_IMB_PROP_THR	30	6
1	1	FLW_TGT_ALT_M	0	6
1	1	FLW_TGT_DST	8.000000000000000000	9
1	1	FLW_TGT_FA	180.000000000000000000	9
1	1	FLW_TGT_HT	8.000000000000000000	9
1	1	FLW_TGT_MAX_VEL	5.000000000000000000	9
1	1	FLW_TGT_RS	0.100000001490116119	9
1	1	FW_AIRSPD_MAX	20.000000000000000000	9
1	1	FW_AIRSPD_TRIM	15.000000000000000000	9
1	1	FW_PSP_OFF	0.000000000000000000	9
1	1	FW_T_CLMB_R_SP	3.000000000000000000	9
1	1	FW_T_SINK_R_SP	2.000000000000000000	9
1	1	GF_ACTION	2	6
1	1	GF_MAX_HOR_DIST	0.000000000000000000	9
1	1	GF_MAX_VER_DIST	0.000000000000000000	9
1	1	GF_PREDICT	0	6
1	1	GF_SOURCE	0	6
1	1	GND_SPEED_THR_SC	1.000000000000000000	9
1	1	GPS_1_CONFIG	201	6
1	1	GPS_1_GNSS	0	6
1	1	GPS_1_PROTOCOL	1	6
1	1	GPS_2_CONFIG	0	6
1	1	GPS_DUMP_COMM	0	6
1	1	GPS_SAT_INFO	0	6
1	1	GPS_UBX_BAUD2	230400	6
1	1	GPS_UBX_CFG_INTF	0	6
1	1	GPS_UBX_DYNMODEL	6	6
1	1	GPS_UBX_MODE	0	6
1	1	GPS_YAW_OFFSET	0.000000000000000000	9
1	1	HTE_ACC_GATE	3.000000000000000000	9
1	1	HTE_HT_ERR_INIT	0.100000001490116119	9
1	1	HTE_HT_NOISE	0.003599999938160181	9
1	1	HTE_THR_RANGE	0.200000002980232239	9
1	1	HTE_VXY_THR	10.000000000000000000	9
1	1	HTE_VZ_THR	2.000000000000000000	9
1	1	IMU_ACCEL_CUTOFF	30.000000000000000000	9
1	1	IMU_DGYRO_CUTOFF	50.000000000000000000	9
1	1	IMU_GYRO_CAL_EN	1	6
1	1	IMU_GYRO_CUTOFF	90.000000000000000000	9
1	1	IMU_GYRO_DNF_BW	15.000000000000000000	9
1	1	IMU_GYRO_DNF_EN	0	6
1	1	IMU_GYRO_DNF_HMC	3	6
1	1	IMU_GYRO_DNF_MIN	25.000000000000000000	9
1	1	IMU_GYRO_FFT_EN	1	6
1	1	IMU_GYRO_FFT_LEN	512	6
1	1	IMU_GYRO_FFT_MAX	150.000000000000000000	9
1	1	IMU_GYRO_FFT_MIN	30.000000000000000000	9
1	1	IMU_GYRO_FFT_SNR	10.000000000000000000	9
1	1	IMU_GYRO_NF0_BW	20.000000000000000000	9
1	1	IMU_GYRO_NF0_FRQ	0.000000000000000000	9
1	1	IMU_GYRO_NF1_BW	20.000000000000000000	9
1	1	IMU_GYRO_NF1_FRQ	0.000000000000000000	9
1	1	IMU_GYRO_RATEMAX	2000	6
1	1	IMU_INTEG_RATE	200	6
1	1	LNDMC_ALT_GND	2.000000000000000000	9
1	1	LNDMC_ROT_MAX	20.000000000000000000	9
1	1	LNDMC_TRIG_TIME	1.000000000000000000	9
1	1	LNDMC_XY_VEL_MAX	1.500000000000000000	9
1	1	LNDMC_Z_VEL_MAX	0.250000000000000000	9
1	1	LND_FLIGHT_T_HI	0	6
1	1	LND_FLIGHT_T_LO	2022551113	6
1	1	MAN_ARM_GESTURE	0	6
1	1	MAN_KILL_GEST_T	-1.000000000000000000	9
1	1	MAV_0_CONFIG	101	6
1	1	MAV_0_FLOW_CTRL	2	6
1	1	MAV_0_FORWARD	1	6
1	1	MAV_0_MODE	0	6
1	1	MAV_0_RADIO_CTL	1	6
1	1	MAV_0_RATE	1200	6
1	1	MAV_1_CONFIG	102	6
1	1	MAV_1_FLOW_CTRL	2	6
1	1	MAV_1_FORWARD	0	6
1	1	MAV_1_MODE	2	6
1	1	MAV_1_RADIO_CTL	1	6
1	1	MAV_1_RATE	0	6
1	1	MAV_2_CONFIG	0	6
1	1	MAV_COMP_ID	1	6
1	1	MAV_FWDEXTSP	1	6
1	1	MAV_HASH_CHK_EN	1	6
1	1	MAV_HB_FORW_EN	1	6
1	1	MAV_PROTO_VER	0	6
1	1	MAV_RADIO_TOUT	5	6
1	1	MAV_SIK_RADIO_ID	0	6
1	1	MAV_SYS_ID	1	6
1	1	MAV_TYPE	2	6
1	1	MAV_USEHILGPS	0	6
1	1	MBE_ENABLE	1	6
1	1	MBE_LEARN_GAIN	18.000000000000000000	9
1	1	MC_ACRO_EXPO	0.000000000000000000	9
1	1	MC_ACRO_EXPO_Y	0.000000000000000000	9
1	1	MC_ACRO_P_MAX	100.000000000000000000	9
1	1	MC_ACRO_R_MAX	100.000000000000000000	9
1	1	MC_ACRO_SUPEXPO	0.000000000000000000	9
1	1	MC_ACRO_SUPEXPOY	0.000000000000000000	9
1	1	MC_ACRO_Y_MAX	100.000000000000000000	9
1	1	MC_AIRMODE	0	6
1	1	MC_AT_APPLY	1	6
1	1	MC_AT_EN	1	6
1	1	MC_AT_RISE_TIME	0.140000000596046448	9
1	1	MC_AT_START	0	6
1	1	MC_AT_SYSID_AMP	0.699999988079071045	9
1	1	MC_BAT_SCALE_EN	0	6
1	1	MC_MAN_TILT_TAU	0.000000000000000000	9
1	1	MC_ORBIT_RAD_MAX	1000.000000000000000000	9
1	1	MC_PITCHRATE_D	0.003000000026077032	9
1	1	MC_PITCHRATE_FF	0.000000000000000000	9
1	1	MC_PITCHRATE_I	0.100000001490116119	9
1	1	MC_PITCHRATE_K	1.000000000000000000	9
1	1	MC_PITCHRATE_MAX	220.000000000000000000	9
1	1	MC_PITCHRATE_P	0.079999998211860657	9
1	1	MC_PITCH_P	6.500000000000000000	9
1	1	MC_PR_INT_LIM	0.300000011920928955	9
1	1	MC_ROLLRATE_D	0.003000000026077032	9
1	1	MC_ROLLRATE_FF	0.000000000000000000	9
1	1	MC_ROLLRATE_I	0.100000001490116119	9
1	1	MC_ROLLRATE_K	1.000000000000000000	9
1	1	MC_ROLLRATE_MAX	220.000000000000000000	9
1	1	MC_ROLLRATE_P	0.079999998211860657	9
1	1	MC_ROLL_P	6.500000000000000000	9
1	1	MC_RR_INT_LIM	0.300000011920928955	9
1	1	MC_SLOW_DEF_HVEL	3.000000000000000000	9
1	1	MC_SLOW_DEF_VVEL	1.000000000000000000	9
1	1	MC_SLOW_DEF_YAWR	45.000000000000000000	9
1	1	MC_SLOW_MAP_HVEL	0	6
1	1	MC_SLOW_MAP_VVEL	0	6
1	1	MC_SLOW_MAP_YAWR	0	6
1	1	MC_SLOW_MIN_HVEL	0.300000011920928955	9
1	1	MC_SLOW_MIN_VVEL	0.300000011920928955	9
1	1	MC_SLOW_MIN_YAWR	3.000000000000000000	9
1	1	MC_YAWRATE_D	0.000000000000000000	9
1	1	MC_YAWRATE_FF	0.000000000000000000	9
1	1	MC_YAWRATE_I	0.100000001490116119	9
1	1	MC_YAWRATE_K	1.000000000000000000	9
1	1	MC_YAWRATE_MAX	200.000000000000000000	9
1	1	MC_YAWRATE_P	0.200000002980232239	9
1	1	MC_YAW_P	2.799999952316284180	9
1	1	MC_YAW_WEIGHT	0.400000005960464478	9
1	1	MC_YR_INT_LIM	0.300000011920928955	9
1	1	MIS_DIST_1WP	900.000000000000000000	9
1	1	MIS_LND_ABRT_ALT	30	6
1	1	MIS_MNT_YAW_CTL	0	6
1	1	MIS_PD_TO	5.000000000000000000	9
1	1	MIS_TAKEOFF_ALT	2.500000000000000000	9
1	1	MIS_YAW_ERR	12.000000000000000000	9
1	1	MIS_YAW_TMT	-1.000000000000000000	9
1	1	MNT_MODE_IN	-1	6
1	1	MOT_POLE_COUNT	14	6
1	1	MOT_SLEW_MAX	0.000000000000000000	9
1	1	MPC_ACC_DECOUPLE	1	6
1	1	MPC_ACC_DOWN_MAX	3.000000000000000000	9
1	1	MPC_ACC_HOR	3.000000000000000000	9
1	1	MPC_ACC_HOR_MAX	5.000000000000000000	9
1	1	MPC_ACC_UP_MAX	4.000000000000000000	9
1	1	MPC_ALT_MODE	2	6
1	1	MPC_HOLD_DZ	0.100000001490116119	9
1	1	MPC_HOLD_MAX_XY	0.800000011920928955	9
1	1	MPC_HOLD_MAX_Z	0.600000023841857910	9
1	1	MPC_JERK_AUTO	4.000000000000000000	9
1	1	MPC_JERK_MAX	8.000000000000000000	9
1	1	MPC_LAND_ALT1	10.000000000000000000	9
1	1	MPC_LAND_ALT2	5.000000000000000000	9
1	1	MPC_LAND_ALT3	1.000000000000000000	9
1	1	MPC_LAND_CRWL	0.300000011920928955	9
1	1	MPC_LAND_RADIUS	1000.000000000000000000	9
1	1	MPC_LAND_RC_HELP	0	6
1	1	MPC_LAND_SPEED	0.699999988079071045	9
1	1	MPC_MANTHR_MIN	0.079999998211860657	9
1	1	MPC_MAN_TILT_MAX	60.000000000000000000	9
1	1	MPC_MAN_Y_MAX	150.000000000000000000	9
1	1	MPC_MAN_Y_TAU	0.079999998211860657	9
1	1	MPC_POS_MODE	4	6
1	1	MPC_THR_CURVE	0	6
1	1	MPC_THR_HOVER	0.500000000000000000	9
1	1	MPC_THR_MAX	1.000000000000000000	9
1	1	MPC_THR_MIN	0.119999997317790985	9
1	1	MPC_THR_XY_MARG	0.300000011920928955	9
1	1	MPC_TILTMAX_AIR	45.000000000000000000	9
1	1	MPC_TILTMAX_LND	12.000000000000000000	9
1	1	MPC_TKO_RAMP_T	3.000000000000000000	9
1	1	MPC_TKO_SPEED	1.500000000000000000	9
1	1	MPC_USE_HTE	1	6
1	1	MPC_VELD_LP	5.000000000000000000	9
1	1	MPC_VEL_MANUAL	1.000000000000000000	9
1	1	MPC_VEL_MAN_BACK	-1.000000000000000000	9
1	1	MPC_VEL_MAN_SIDE	-1.000000000000000000	9
1	1	MPC_XY_CRUISE	1.000000000000000000	9
1	1	MPC_XY_ERR_MAX	2.000000000000000000	9
1	1	MPC_XY_MAN_EXPO	0.600000023841857910	9
1	1	MPC_XY_P	0.949999988079071045	9
1	1	MPC_XY_TRAJ_P	0.500000000000000000	9
1	1	MPC_XY_VEL_ALL	-10.000000000000000000	9
1	1	MPC_XY_VEL_D_ACC	0.200000002980232239	9
1	1	MPC_XY_VEL_I_ACC	0.400000005960464478	9
1	1	MPC_XY_VEL_MAX	1.000000000000000000	9
1	1	MPC_XY_VEL_P_ACC	1.799999952316284180	9
1	1	MPC_YAWRAUTO_ACC	60.000000000000000000	9
1	1	MPC_YAWRAUTO_MAX	45.000000000000000000	9
1	1	MPC_YAW_EXPO	0.600000023841857910	9
1	1	MPC_YAW_MODE	0	6
1	1	MPC_Z_MAN_EXPO	0.600000023841857910	9
1	1	MPC_Z_P	1.000000000000000000	9
1	1	MPC_Z_VEL_ALL	-3.000000000000000000	9
1	1	MPC_Z_VEL_D_ACC	0.000000000000000000	9
1	1	MPC_Z_VEL_I_ACC	2.000000000000000000	9
1	1	MPC_Z_VEL_MAX_DN	1.500000000000000000	9
1	1	MPC_Z_VEL_MAX_UP	3.000000000000000000	9
1	1	MPC_Z_VEL_P_ACC	4.000000000000000000	9
1	1	MPC_Z_V_AUTO_DN	1.500000000000000000	9
1	1	MPC_Z_V_AUTO_UP	3.000000000000000000	9
1	1	NAV_ACC_RAD	2.000000000000000000	9
1	1	NAV_DLL_ACT	0	6
1	1	NAV_FORCE_VT	1	6
1	1	NAV_FW_ALTL_RAD	5.000000000000000000	9
1	1	NAV_FW_ALT_RAD	10.000000000000000000	9
1	1	NAV_LOITER_RAD	80.000000000000000000	9
1	1	NAV_MC_ALT_RAD	0.800000011920928955	9
1	1	NAV_MIN_LTR_ALT	-1.000000000000000000	9
1	1	NAV_RCL_ACT	2	6
1	1	NAV_TRAFF_AVOID	1	6
1	1	NAV_TRAFF_A_HOR	500.000000000000000000	9
1	1	NAV_TRAFF_A_VER	500.000000000000000000	9
1	1	NAV_TRAFF_COLL_T	60	6
1	1	PLD_BTOUT	5.000000000000000000	9
1	1	PLD_FAPPR_ALT	0.100000001490116119	9
1	1	PLD_HACC_RAD	0.200000002980232239	9
1	1	PLD_MAX_SRCH	3	6
1	1	PLD_SRCH_ALT	10.000000000000000000	9
1	1	PLD_SRCH_TOUT	10.000000000000000000	9
1	1	PWM_MAIN_DIS1	1000	6
1	1	PWM_MAIN_DIS2	1000	6
1	1	PWM_MAIN_DIS3	1000	6
1	1	PWM_MAIN_DIS4	1000	6
1	1	PWM_MAIN_DIS5	1000	6
1	1	PWM_MAIN_DIS6	1000	6
1	1	PWM_MAIN_DIS7	1000	6
1	1	PWM_MAIN_DIS8	1000	6
1	1	PWM_MAIN_FAIL1	-1	6
1	1	PWM_MAIN_FAIL2	-1	6
1	1	PWM_MAIN_FAIL3	-1	6
1	1	PWM_MAIN_FAIL4	-1	6
1	1	PWM_MAIN_FAIL5	-1	6
1	1	PWM_MAIN_FAIL6	-1	6
1	1	PWM_MAIN_FAIL7	-1	6
1	1	PWM_MAIN_FAIL8	-1	6
1	1	PWM_MAIN_FUNC1	102	6
1	1	PWM_MAIN_FUNC2	103	6
1	1	PWM_MAIN_FUNC3	104	6
1	1	PWM_MAIN_FUNC4	101	6
1	1	PWM_MAIN_FUNC5	0	6
1	1	PWM_MAIN_FUNC6	0	6
1	1	PWM_MAIN_FUNC7	0	6
1	1	PWM_MAIN_FUNC8	0	6
1	1	PWM_MAIN_MAX1	1900	6
1	1	PWM_MAIN_MAX2	1900	6
1	1	PWM_MAIN_MAX3	1900	6
1	1	PWM_MAIN_MAX4	1900	6
1	1	PWM_MAIN_MAX5	2000	6
1	1	PWM_MAIN_MAX6	2000	6
1	1	PWM_MAIN_MAX7	2000	6
1	1	PWM_MAIN_MAX8	2000	6
1	1	PWM_MAIN_MIN1	1100	6
1	1	PWM_MAIN_MIN2	1100	6
1	1	PWM_MAIN_MIN3	1100	6
1	1	PWM_MAIN_MIN4	1100	6
1	1	PWM_MAIN_MIN5	1000	6
1	1	PWM_MAIN_MIN6	1000	6
1	1	PWM_MAIN_MIN7	1000	6
1	1	PWM_MAIN_MIN8	1000	6
1	1	PWM_MAIN_REV	0	6
1	1	PWM_MAIN_TIM0	-3	6
1	1	PWM_MAIN_TIM1	-3	6
1	1	PWM_MAIN_TIM2	400	6
1	1	PWM_MAIN_TIM3	400	6
1	1	RC10_DZ	0.000000000000000000	9
1	1	RC10_MAX	2000.000000000000000000	9
1	1	RC10_MIN	1000.000000000000000000	9
1	1	RC10_REV	1.000000000000000000	9
1	1	RC10_TRIM	1500.000000000000000000	9
1	1	RC11_DZ	0.000000000000000000	9
1	1	RC11_MAX	2000.000000000000000000	9
1	1	RC11_MIN	1000.000000000000000000	9
1	1	RC11_REV	1.000000000000000000	9
1	1	RC11_TRIM	1500.000000000000000000	9
1	1	RC12_DZ	0.000000000000000000	9
1	1	RC12_MAX	2000.000000000000000000	9
1	1	RC12_MIN	1000.000000000000000000	9
1	1	RC12_REV	1.000000000000000000	9
1	1	RC12_TRIM	1500.000000000000000000	9
1	1	RC13_DZ	0.000000000000000000	9
1	1	RC13_MAX	2000.000000000000000000	9
1	1	RC13_MIN	1000.000000000000000000	9
1	1	RC13_REV	1.000000000000000000	9
1	1	RC13_TRIM	1500.000000000000000000	9
1	1	RC14_DZ	0.000000000000000000	9
1	1	RC14_MAX	2000.000000000000000000	9
1	1	RC14_MIN	1000.000000000000000000	9
1	1	RC14_REV	1.000000000000000000	9
1	1	RC14_TRIM	1500.000000000000000000	9
1	1	RC15_DZ	0.000000000000000000	9
1	1	RC15_MAX	2000.000000000000000000	9
1	1	RC15_MIN	1000.000000000000000000	9
1	1	RC15_REV	1.000000000000000000	9
1	1	RC15_TRIM	1500.000000000000000000	9
1	1	RC16_DZ	0.000000000000000000	9
1	1	RC16_MAX	2000.000000000000000000	9
1	1	RC16_MIN	1000.000000000000000000	9
1	1	RC16_REV	1.000000000000000000	9
1	1	RC16_TRIM	1500.000000000000000000	9
1	1	RC17_DZ	0.000000000000000000	9
1	1	RC17_MAX	2000.000000000000000000	9
1	1	RC17_MIN	1000.000000000000000000	9
1	1	RC17_REV	1.000000000000000000	9
1	1	RC17_TRIM	1500.000000000000000000	9
1	1	RC18_DZ	0.000000000000000000	9
1	1	RC18_MAX	2000.000000000000000000	9
1	1	RC18_MIN	1000.000000000000000000	9
1	1	RC18_REV	1.000000000000000000	9
1	1	RC18_TRIM	1500.000000000000000000	9
1	1	RC1_DZ	10.000000000000000000	9
1	1	RC1_MAX	2002.000000000000000000	9
1	1	RC1_MIN	1024.000000000000000000	9
1	1	RC1_REV	1.000000000000000000	9
1	1	RC1_TRIM	1506.000000000000000000	9
1	1	RC2_DZ	10.000000000000000000	9
1	1	RC2_MAX	1993.000000000000000000	9
1	1	RC2_MIN	1032.000000000000000000	9
1	1	RC2_REV	1.000000000000000000	9
1	1	RC2_TRIM	1504.000000000000000000	9
1	1	RC3_DZ	10.000000000000000000	9
1	1	RC3_MAX	2000.000000000000000000	9
1	1	RC3_MIN	1024.000000000000000000	9
1	1	RC3_REV	1.000000000000000000	9
1	1	RC3_TRIM	1024.000000000000000000	9
1	1	RC4_DZ	10.000000000000000000	9
1	1	RC4_MAX	2002.000000000000000000	9
1	1	RC4_MIN	1024.000000000000000000	9
1	1	RC4_REV	1.000000000000000000	9
1	1	RC4_TRIM	1505.000000000000000000	9
1	1	RC5_DZ	10.000000000000000000	9
1	1	RC5_MAX	2000.000000000000000000	9
1	1	RC5_MIN	1000.000000000000000000	9
1	1	RC5_REV	1.000000000000000000	9
1	1	RC5_TRIM	1500.000000000000000000	9
1	1	RC6_DZ	10.000000000000000000	9
1	1	RC6_MAX	2000.000000000000000000	9
1	1	RC6_MIN	1000.000000000000000000	9
1	1	RC6_REV	1.000000000000000000	9
1	1	RC6_TRIM	1500.000000000000000000	9
1	1	RC7_DZ	10.000000000000000000	9
1	1	RC7_MAX	2000.000000000000000000	9
1	1	RC7_MIN	1000.000000000000000000	9
1	1	RC7_REV	1.000000000000000000	9
1	1	RC7_TRIM	1500.000000000000000000	9
1	1	RC8_DZ	10.000000000000000000	9
1	1	RC8_MAX	2000.000000000000000000	9
1	1	RC8_MIN	1000.000000000000000000	9
1	1	RC8_REV	1.000000000000000000	9
1	1	RC8_TRIM	1500.000000000000000000	9
1	1	RC9_DZ	0.000000000000000000	9
1	1	RC9_MAX	2000.000000000000000000	9
1	1	RC9_MIN	1000.000000000000000000	9
1	1	RC9_REV	1.000000000000000000	9
1	1	RC9_TRIM	1500.000000000000000000	9
1	1	RC_ARMSWITCH_TH	0.750000000000000000	9
1	1	RC_CHAN_CNT	18	6
1	1	RC_ENG_MOT_TH	0.750000000000000000	9
1	1	RC_FAILS_THR	0	6
1	1	RC_GEAR_TH	0.750000000000000000	9
1	1	RC_INPUT_PROTO	2	6
1	1	RC_KILLSWITCH_TH	0.750000000000000000	9
1	1	RC_LOITER_TH	0.750000000000000000	9
1	1	RC_MAP_ACRO_SW	0	6
1	1	RC_MAP_ARM_SW	5	6
1	1	RC_MAP_AUX1	0	6
1	1	RC_MAP_AUX2	0	6
1	1	RC_MAP_AUX3	0	6
1	1	RC_MAP_AUX4	0	6
1	1	RC_MAP_AUX5	0	6
1	1	RC_MAP_AUX6	0	6
1	1	RC_MAP_ENG_MOT	0	6
1	1	RC_MAP_FAILSAFE	0	6
1	1	RC_MAP_FLAPS	0	6
1	1	RC_MAP_FLTMODE	7	6
1	1	RC_MAP_FLTM_BTN	0	6
1	1	RC_MAP_GEAR_SW	0	6
1	1	RC_MAP_KILL_SW	6	6
1	1	RC_MAP_LOITER_SW	0	6
1	1	RC_MAP_MAN_SW	0	6
1	1	RC_MAP_MODE_SW	0	6
1	1	RC_MAP_OFFB_SW	8	6
1	1	RC_MAP_PARAM1	0	6
1	1	RC_MAP_PARAM2	0	6
1	1	RC_MAP_PARAM3	0	6
1	1	RC_MAP_PAY_SW	0	6
1	1	RC_MAP_PITCH	2	6
1	1	RC_MAP_POSCTL_SW	0	6
1	1	RC_MAP_RATT_SW	0	6
1	1	RC_MAP_RETURN_SW	0	6
1	1	RC_MAP_ROLL	1	6
1	1	RC_MAP_STAB_SW	0	6
1	1	RC_MAP_THROTTLE	3	6
1	1	RC_MAP_TRANS_SW	0	6
1	1	RC_MAP_YAW	4	6
1	1	RC_OFFB_TH	0.750000000000000000	9
1	1	RC_PAYLOAD_TH	0.750000000000000000	9
1	1	RC_PORT_CONFIG	300	6
1	1	RC_RETURN_TH	0.750000000000000000	9
1	1	RC_RSSI_PWM_CHAN	0	6
1	1	RC_RSSI_PWM_MAX	2000	6
1	1	RC_RSSI_PWM_MIN	1000	6
1	1	RC_TRANS_TH	0.750000000000000000	9
1	1	RTL_APPR_FORCE	0	6
1	1	RTL_CONE_ANG	45	6
1	1	RTL_DESCEND_ALT	10.000000000000000000	9
1	1	RTL_LAND_DELAY	0.000000000000000000	9
1	1	RTL_LOITER_RAD	80.000000000000000000	9
1	1	RTL_MIN_DIST	10.000000000000000000	9
1	1	RTL_PLD_MD	0	6
1	1	RTL_RETURN_ALT	30.000000000000000000	9
1	1	RTL_TIME_FACTOR	1.100000023841857910	9
1	1	RTL_TIME_MARGIN	100	6
1	1	RTL_TYPE	0	6
1	1	SDLOG_MODE	-1	6
1	1	SDLOG_PROFILE	19	6
1	1	SENS_BARO_QNH	1013.250000000000000000	9
1	1	SENS_BARO_RATE	20.000000000000000000	9
1	1	SENS_BOARD_ROT	0	6
1	1	SENS_BOARD_X_OFF	-11.590764999389648438	9
1	1	SENS_BOARD_Y_OFF	-1.870400905609130859	9
1	1	SENS_BOARD_Z_OFF	0.000000000000000000	9
1	1	SENS_CM8JL65_CFG	0	6
1	1	SENS_DPRES_OFF	0.000000000000000000	9
1	1	SENS_EN_BATT	0	6
1	1	SENS_EN_LL40LS	0	6
1	1	SENS_EN_MB12XX	0	6
1	1	SENS_EN_MS4525DO	0	6
1	1	SENS_EN_MS5525DS	0	6
1	1	SENS_EN_PAA3905	0	6
1	1	SENS_EN_PAW3902	0	6
1	1	SENS_EN_PMW3901	0	6
1	1	SENS_EN_PX4FLOW	0	6
1	1	SENS_EN_SDP3X	0	6
1	1	SENS_EN_SF1XX	0	6
1	1	SENS_EN_SPA06	0	6
1	1	SENS_EN_SPL06	0	6
1	1	SENS_EN_TF02PRO	0	6
1	1	SENS_EN_THERMAL	-1	6
1	1	SENS_EN_TRANGER	0	6
1	1	SENS_EN_VL53L0X	0	6
1	1	SENS_EN_VL53L1X	0	6
1	1	SENS_EXT_I2C_PRB	1	6
1	1	SENS_GPS_MASK	7	6
1	1	SENS_GPS_PRIME	0	6
1	1	SENS_GPS_TAU	10.000000000000000000	9
1	1	SENS_IMU_AUTOCAL	1	6
1	1	SENS_IMU_CLPNOTI	1	6
1	1	SENS_IMU_MODE	0	6
1	1	SENS_LEDDAR1_CFG	0	6
1	1	SENS_MAG_MODE	1	6
1	1	SENS_SF0X_CFG	0	6
1	1	SENS_TFLOW_CFG	0	6
1	1	SENS_TFMINI_CFG	0	6
1	1	SENS_ULAND_CFG	0	6
1	1	SER_GPS1_BAUD	0	6
1	1	SER_TEL1_BAUD	57600	6
1	1	SER_TEL2_BAUD	921600	6
1	1	SYS_AUTOCONFIG	0	6
1	1	SYS_AUTOSTART	4050	6
1	1	SYS_CAL_ACCEL	0	6
1	1	SYS_CAL_BARO	0	6
1	1	SYS_CAL_GYRO	0	6
1	1	SYS_CAL_TDEL	24	6
1	1	SYS_CAL_TMAX	10	6
1	1	SYS_CAL_TMIN	5	6
1	1	SYS_DM_BACKEND	1	6
1	1	SYS_FAC_CAL_MODE	0	6
1	1	SYS_FAILURE_EN	0	6
1	1	SYS_HAS_BARO	1	6
1	1	SYS_HAS_GPS	1	6
1	1	SYS_HAS_MAG	0	6
1	1	SYS_HAS_NUM_ASPD	0	6
1	1	SYS_HAS_NUM_DIST	0	6
1	1	SYS_HITL	0	6
1	1	SYS_RGB_MAXBRT	1.000000000000000000	9
1	1	SYS_STCK_EN	1	6
1	1	SYS_USB_AUTO	2	6
1	1	SYS_VEHICLE_RESP	-0.400000005960464478	9
1	1	TC_A_ENABLE	0	6
1	1	TC_B_ENABLE	0	6
1	1	TC_G_ENABLE	0	6
1	1	TC_M_ENABLE	0	6
1	1	TEL_BST_EN	0	6
1	1	TEL_FRSKY_CONFIG	0	6
1	1	TEL_HOTT_CONFIG	0	6
1	1	THR_MDL_FAC	0.300000011920928955	9
1	1	TRIG_ACT_TIME	40.000000000000000000	9
1	1	TRIG_DISTANCE	25.000000000000000000	9
1	1	TRIG_INTERFACE	4	6
1	1	TRIG_INTERVAL	40.000000000000000000	9
1	1	TRIG_MIN_INTERVA	1.000000000000000000	9
1	1	TRIG_MODE	2	6
1	1	TRIG_PWM_NEUTRAL	1500	6
1	1	TRIG_PWM_SHOOT	1900	6
1	1	USB_MAV_MODE	2	6
1	1	VT_B_DEC_MSS	2.000000000000000000	9
1	1	WV_EN	0	6
1	1	WV_GAIN	1.000000000000000000	9
1	1	WV_ROLL_MIN	1.000000000000000000	9
1	1	WV_YRATE_MAX	90.000000000000000000	9

and this is how I transform vins to mavros:

void vins_callback(const nav_msgs::Odometry::ConstPtr &msg)

{
    if(msg->header.frame_id == "world")
    {
        p_mav = Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
        q_mav = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
    }
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "vins_to_mavros");
    ros::NodeHandle nh("~");
    ros::Subscriber slam_sub = nh.subscribe<nav_msgs::Odometry>("/vins_estimator/imu_propagate", 100, 
    vins_callback);
    ros::Publisher vision_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 10);
     ros::Rate rate(20.0);
 ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        geometry_msgs::PoseStamped vision;
 
        vision.pose.position.x = p_mav[0];
        vision.pose.position.y = p_mav[1];
        vision.pose.position.z = p_mav[2];
 
        vision.pose.orientation.x = q_mav.x();
        vision.pose.orientation.y = q_mav.y();
        vision.pose.orientation.z = q_mav.z();
        vision.pose.orientation.w = q_mav.w();
 
        vision.header.stamp = ros::Time::now();
        vision_pub.publish(vision);ek
 
        ROS_INFO("\nposition:\n   x: %.18f\n   y: %.18f\n   z: %.18f\norientation:\n   x: %.18f\n   y: %.18f\n   z: %.18f\n   w: %.18f", \
        p_mav[0],p_mav[1],p_mav[2],q_mav.x(),q_mav.y(),q_mav.z(),q_mav.w());
 
        ros::spinOnce();
        rate.sleep();
    }
 
    return 0;

To Reproduce

No response

Expected behavior

No response

Screenshot / Media

top: /mavros/local_position/pose
below: /mavros/vision_pose/pose

Image

Image

and I have an error on screen ,I don't konw what it mean and I can't find any explain about that

Image

Flight Log

No response

Software Version

No response

Flight controller

Holybro KakuteH7

Vehicle type

Multicopter

How are the different components wired up (including port information)

No response

Additional context

No response

Metadata

Metadata

Assignees

No one assigned

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions