diff --git a/yaml/params_gnss_imu.yaml b/yaml/params_gnss_imu.yaml index 2a7958ac7185adcceb56ae4515000b336fadb659..61044b61539c1534dc8682db3a1c5568f9c53f6f 100644 --- a/yaml/params_gnss_imu.yaml +++ b/yaml/params_gnss_imu.yaml @@ -1,21 +1,20 @@ config: debug: profiling: true - profiling_file: "~/wolf_gnss_imu_profiling.txt" + profiling_file: "~/profiling_demo_gnss_imu.txt" print_problem: false + print_period: 1 print_depth: 4 print_constr_by: false - print_metric: false + print_metric: true print_state_blocks: false - print_period: 1 problem: + node_rate: 100 frame_structure: "POV" dimension: 3 - node_rate: 100 tree_manager: - follow: "sliding_window.yaml" - #follow: "sliding_window_dual.yaml" + follow: "sliding_window_dual.yaml" prior: mode: "fix" time_tolerance: 0.1 @@ -26,39 +25,28 @@ config: solver: follow: "solver.yaml" + + map: + type: "MapBase" + plugin: "core" sensors: - type: "SensorImu" name: "IMU_odometer" plugin: "imu" - extrinsic: - pose: [0,0,0,0,0,0,1] - a_noise: 0.53 # standard deviation of Acceleration noise (same for all the axis) in m/s2 (1/1000 max value) - w_noise: 0.011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec (1/1000 max value) - prior_bias: true - ab_initial_stdev: 0.05 # m/s2 - initial bias - wb_initial_stdev: 0.01 # rad/sec - initial bias - bias_dynamic: false - #ab_rate_stdev: 0.00001 # m/s2/sqrt(s) - #wb_rate_stdev: 0.00001 # rad/s/sqrt(s) + follow: "sensor_bno.yaml" - type: "SensorGnss" name: "gnss" plugin: "gnss" extrinsic: - pose: [0, 0, 0] - extrinsics_fixed: true - clock_bias_GPS_GLO_dynamic: false - clock_bias_GPS_GAL_dynamic: false - clock_bias_GPS_CMP_dynamic: false - ENU: - mode: "auto" - roll_fixed: true - pitch_fixed: true - yaw_fixed: true - translation_fixed: true - #ENU_latlonalt: [0.7092812341, 0.3998234412, 71.8593953932] # [40.6388212031 deg, 22.9081957302 deg, 71.8593953932 m] + pose: [0.0, 0.0, 0.28] + follow: "sensor_f9t.yaml" + - + type: "SensorModel" + name: "dynamic_model" + plugin: "core" processors: - @@ -66,69 +54,19 @@ config: name: "processor IMU" sensor_name: "IMU_odometer" plugin: "imu" - time_tolerance: 0.005 # Time tolerance for joining KFs - unmeasured_perturbation_std: 0.001 - keyframe_vote: - voting_active: false - max_time_span: 99999 # seconds - max_buff_length: 99999 # motion deltas - dist_traveled: 99999 # meters - angle_turned: 99999 # radians (1 rad approx 57 deg, approx 60 deg) - cov_det: 99999 - apply_loss_function: false - state_getter: true - state_priority: 1 + follow: "processor_imu.yaml" - type: "ProcessorTrackerGnss" name: "processor gnss" sensor_name: "gnss" plugin: "gnss" - time_tolerance: 0.1 - apply_loss_function: false - min_features_for_keyframe: 0 #4 - max_new_features: -1 #unlimited - remove_outliers: true - remove_outliers_with_fix: true - outlier_residual_th: 10 - init_frames: false - pseudo_ranges: true - enu_map_fix_dist: 5 - fix: false - keyframe_vote: - voting_active: true - max_time_span: 1 - min_features_for_keyframe: 4 - gnss: - sateph: 6 # satellite ephemeris/clock: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC) - ionoopt: 9 # ionosphere option: IONOOPT_OFF(0):correction off, IONOOPT_BRDC(1):broadcast mode, IONOOPT_SBAS(2):SBAS model, IONOOPT_IFLC(3):L1/L2 or L1/L5, IONOOPT_EST(4):estimation, IONOOPT_TEC(5):IONEX TEC model, IONOOPT_QZS(6):QZSS broadcast, IONOOPT_LEX(7):QZSS LEX ionosphere, IONOOPT_STEC(8):SLANT TEC mode, IONOOPT_SBAS2(9):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), IONOOPT_SBAS3(10):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_SBAS2), IONOOPT_SBAS4(8):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_BRDC) - tropopt: 2 # troposphere option: (0:correction off,1:Saastamoinen model,2:SBAS model,3:troposphere option: ZTD estimation,4:ZTD+grad estimation,5:ZTD correction,6:ZTD+grad correction) - sbascorr: 15 # SBAS option (1:long term correction,2:fast correction,4:ionosphere correction,8:ranging) - raim: 1 # apply RAIM - elmin: 0.26 # elevation min (rad) = 15 degrees - maxgdop: 30 # reject threshold of gdop (ground dilusion of precision) - min_sbas_sats: 5 # used in case of EPHOPT_SBAS3, EPHOPT_SBAS4, IONOOPT_SBAS3, IONOOPT_SBAS4 - constellations: - GPS: true - SBS: false - GLO: false - GAL: true - QZS: false - CMP: false - IRN: false - LEO: false - tdcp: - enabled: true - corr_iono: true - corr_tropo: true - corr_clock: true - loss_function: false - time_window: 100 # s - sigma_atm: 0.0016 # m/sqrt(s) - sigma_carrier: 0.0011 # m - multi_freq: false - remove_outliers: false - batch: false - structure: first-all + follow: "processor_gnss.yaml" + - + type: "ProcessorFixWingModel" + name: "processor fix wing" + sensor_name: "dynamic_model" + plugin: "core" + follow: "processor_fix_wing.yaml" ROS subscriber: - @@ -136,43 +74,22 @@ config: topic: "/ublox_gps/raw_data_stream" sensor_name: "gnss" package: "wolf_ros_gnss" - process_not_available: false - reset_receiver: false - publish_fix: - enabled: true - topic: "RTKLIB_fix" - gnss_options: - sateph: 6 # satellite ephemeris/clock (0:broadcast ephemeris,1:precise ephemeris,2:broadcast + SBAS,3:ephemeris option: broadcast + SSR_APC,4:broadcast + SSR_COM,5: QZSS LEX ephemeris - ionoopt: 9 # ionosphere option (0:correction off,1:broadcast mode,2:SBAS model,3:L1/L2 or L1/L5,4:estimation,5:IONEX TEC model,6:QZSS broadcast,7:QZSS LEX ionosphere,8:SLANT TEC mode) - tropopt: 2 # troposphere option: (0:correction off,1:Saastamoinen model,2:SBAS model,3:troposphere option: ZTD estimation,4:ZTD+grad estimation,5:ZTD correction,6:ZTD+grad correction) - sbascorr: 15 # SBAS option (1:long term correction,2:fast correction,4:ionosphere correction,8:ranging) - raim: 1 # apply RAIM n times - elmin: 0.26 # elevation min (rad) = 15 degrees - maxgdop: 30 # reject threshold of gdop (ground dilusion of precision) - constellations: - GPS: true - SBS: false - GLO: false - GAL: true - QZS: false - CMP: false - IRN: false - LEO: false + follow: "subscriber_gnss_ublox.yaml" - type: "SubscriberImuEnableable" topic: "/bno055_imu/imu" sensor_name: "IMU_odometer" package: "wolf_ros_imu" - topic_enable: "gnss_available" - imu_x_axis: 1 - imu_y_axis: 2 - imu_z_axis: 3 - cov_source: "sensor" - in_degrees: true - static_init_duration: 130.0 - lowpass_filter: true - lowpass_cutoff_freq: 10 - dt: 0.01 + follow: "subscriber_imu.yaml" + - + type: "SubscriberGnssFixPublisherEcef" + topic: "/ublox_gps/fix" + sensor_name: "gnss" + package: "wolf_ros_gnss" + marker_color: [0,0,1,1] + line_size: 0.2 + period: 0.2 + max_points: 10000 ROS publisher: - @@ -180,26 +97,36 @@ config: topic: "current_pose" package: "wolf_ros_node" marker_color: [1,0,0,1] - period: 0.1 - extrinsics: false + line_size: 0.2 + period: 0.2 + extrinsics: true + sensor: "gnss" frame_id: "ENU" + max_points: 10000 + - + type: "PublisherGnssTf" + topic: " " + package: "wolf_ros_gnss" + period: 0.2 + sensor_gnss_name: "gnss" - type: "PublisherTf" topic: " " package: "wolf_ros_node" - period: 0.1 + period: 0.2 map_frame_id: "map" odom_frame_id: "odom" base_frame_id: "base" publish_odom_tf: false - - - type: "PublisherGnssTf" - topic: " " - package: "wolf_ros_gnss" - period: 0.1 - sensor_gnss_name: "gnss" - type: "PublisherGraph" topic: "graph" package: "wolf_ros_node" period: 0.2 + viz_scale: 2 + - + type: "PublisherTrackerGnssInfo" + topic: "gnss_tracker" + package: "wolf_ros_gnss" + period: 0.2 + processor_gnss: "processor gnss" \ No newline at end of file diff --git a/yaml/params_gnss_imu2.yaml b/yaml/params_gnss_imu2.yaml new file mode 100644 index 0000000000000000000000000000000000000000..2a7958ac7185adcceb56ae4515000b336fadb659 --- /dev/null +++ b/yaml/params_gnss_imu2.yaml @@ -0,0 +1,205 @@ +config: + debug: + profiling: true + profiling_file: "~/wolf_gnss_imu_profiling.txt" + print_problem: false + print_depth: 4 + print_constr_by: false + print_metric: false + print_state_blocks: false + print_period: 1 + + problem: + frame_structure: "POV" + dimension: 3 + node_rate: 100 + tree_manager: + follow: "sliding_window.yaml" + #follow: "sliding_window_dual.yaml" + prior: + mode: "fix" + time_tolerance: 0.1 + $state: + P: [0,0,0] + O: [0,0,0,1] + V: [0,0,0] + + solver: + follow: "solver.yaml" + + sensors: + - + type: "SensorImu" + name: "IMU_odometer" + plugin: "imu" + extrinsic: + pose: [0,0,0,0,0,0,1] + a_noise: 0.53 # standard deviation of Acceleration noise (same for all the axis) in m/s2 (1/1000 max value) + w_noise: 0.011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec (1/1000 max value) + prior_bias: true + ab_initial_stdev: 0.05 # m/s2 - initial bias + wb_initial_stdev: 0.01 # rad/sec - initial bias + bias_dynamic: false + #ab_rate_stdev: 0.00001 # m/s2/sqrt(s) + #wb_rate_stdev: 0.00001 # rad/s/sqrt(s) + - + type: "SensorGnss" + name: "gnss" + plugin: "gnss" + extrinsic: + pose: [0, 0, 0] + extrinsics_fixed: true + clock_bias_GPS_GLO_dynamic: false + clock_bias_GPS_GAL_dynamic: false + clock_bias_GPS_CMP_dynamic: false + ENU: + mode: "auto" + roll_fixed: true + pitch_fixed: true + yaw_fixed: true + translation_fixed: true + #ENU_latlonalt: [0.7092812341, 0.3998234412, 71.8593953932] # [40.6388212031 deg, 22.9081957302 deg, 71.8593953932 m] + + processors: + - + type: "ProcessorImu" + name: "processor IMU" + sensor_name: "IMU_odometer" + plugin: "imu" + time_tolerance: 0.005 # Time tolerance for joining KFs + unmeasured_perturbation_std: 0.001 + keyframe_vote: + voting_active: false + max_time_span: 99999 # seconds + max_buff_length: 99999 # motion deltas + dist_traveled: 99999 # meters + angle_turned: 99999 # radians (1 rad approx 57 deg, approx 60 deg) + cov_det: 99999 + apply_loss_function: false + state_getter: true + state_priority: 1 + - + type: "ProcessorTrackerGnss" + name: "processor gnss" + sensor_name: "gnss" + plugin: "gnss" + time_tolerance: 0.1 + apply_loss_function: false + min_features_for_keyframe: 0 #4 + max_new_features: -1 #unlimited + remove_outliers: true + remove_outliers_with_fix: true + outlier_residual_th: 10 + init_frames: false + pseudo_ranges: true + enu_map_fix_dist: 5 + fix: false + keyframe_vote: + voting_active: true + max_time_span: 1 + min_features_for_keyframe: 4 + gnss: + sateph: 6 # satellite ephemeris/clock: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC) + ionoopt: 9 # ionosphere option: IONOOPT_OFF(0):correction off, IONOOPT_BRDC(1):broadcast mode, IONOOPT_SBAS(2):SBAS model, IONOOPT_IFLC(3):L1/L2 or L1/L5, IONOOPT_EST(4):estimation, IONOOPT_TEC(5):IONEX TEC model, IONOOPT_QZS(6):QZSS broadcast, IONOOPT_LEX(7):QZSS LEX ionosphere, IONOOPT_STEC(8):SLANT TEC mode, IONOOPT_SBAS2(9):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), IONOOPT_SBAS3(10):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_SBAS2), IONOOPT_SBAS4(8):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_BRDC) + tropopt: 2 # troposphere option: (0:correction off,1:Saastamoinen model,2:SBAS model,3:troposphere option: ZTD estimation,4:ZTD+grad estimation,5:ZTD correction,6:ZTD+grad correction) + sbascorr: 15 # SBAS option (1:long term correction,2:fast correction,4:ionosphere correction,8:ranging) + raim: 1 # apply RAIM + elmin: 0.26 # elevation min (rad) = 15 degrees + maxgdop: 30 # reject threshold of gdop (ground dilusion of precision) + min_sbas_sats: 5 # used in case of EPHOPT_SBAS3, EPHOPT_SBAS4, IONOOPT_SBAS3, IONOOPT_SBAS4 + constellations: + GPS: true + SBS: false + GLO: false + GAL: true + QZS: false + CMP: false + IRN: false + LEO: false + tdcp: + enabled: true + corr_iono: true + corr_tropo: true + corr_clock: true + loss_function: false + time_window: 100 # s + sigma_atm: 0.0016 # m/sqrt(s) + sigma_carrier: 0.0011 # m + multi_freq: false + remove_outliers: false + batch: false + structure: first-all + + ROS subscriber: + - + type: "SubscriberGnssUblox" + topic: "/ublox_gps/raw_data_stream" + sensor_name: "gnss" + package: "wolf_ros_gnss" + process_not_available: false + reset_receiver: false + publish_fix: + enabled: true + topic: "RTKLIB_fix" + gnss_options: + sateph: 6 # satellite ephemeris/clock (0:broadcast ephemeris,1:precise ephemeris,2:broadcast + SBAS,3:ephemeris option: broadcast + SSR_APC,4:broadcast + SSR_COM,5: QZSS LEX ephemeris + ionoopt: 9 # ionosphere option (0:correction off,1:broadcast mode,2:SBAS model,3:L1/L2 or L1/L5,4:estimation,5:IONEX TEC model,6:QZSS broadcast,7:QZSS LEX ionosphere,8:SLANT TEC mode) + tropopt: 2 # troposphere option: (0:correction off,1:Saastamoinen model,2:SBAS model,3:troposphere option: ZTD estimation,4:ZTD+grad estimation,5:ZTD correction,6:ZTD+grad correction) + sbascorr: 15 # SBAS option (1:long term correction,2:fast correction,4:ionosphere correction,8:ranging) + raim: 1 # apply RAIM n times + elmin: 0.26 # elevation min (rad) = 15 degrees + maxgdop: 30 # reject threshold of gdop (ground dilusion of precision) + constellations: + GPS: true + SBS: false + GLO: false + GAL: true + QZS: false + CMP: false + IRN: false + LEO: false + - + type: "SubscriberImuEnableable" + topic: "/bno055_imu/imu" + sensor_name: "IMU_odometer" + package: "wolf_ros_imu" + topic_enable: "gnss_available" + imu_x_axis: 1 + imu_y_axis: 2 + imu_z_axis: 3 + cov_source: "sensor" + in_degrees: true + static_init_duration: 130.0 + lowpass_filter: true + lowpass_cutoff_freq: 10 + dt: 0.01 + + ROS publisher: + - + type: "PublisherPose" + topic: "current_pose" + package: "wolf_ros_node" + marker_color: [1,0,0,1] + period: 0.1 + extrinsics: false + frame_id: "ENU" + - + type: "PublisherTf" + topic: " " + package: "wolf_ros_node" + period: 0.1 + map_frame_id: "map" + odom_frame_id: "odom" + base_frame_id: "base" + publish_odom_tf: false + - + type: "PublisherGnssTf" + topic: " " + package: "wolf_ros_gnss" + period: 0.1 + sensor_gnss_name: "gnss" + - + type: "PublisherGraph" + topic: "graph" + package: "wolf_ros_node" + period: 0.2 diff --git a/yaml/processor_fix_wing.yaml b/yaml/processor_fix_wing.yaml new file mode 100644 index 0000000000000000000000000000000000000000..115b1eaf1f96ae970b31d9d658f62dac4bc8ef5c --- /dev/null +++ b/yaml/processor_fix_wing.yaml @@ -0,0 +1,7 @@ +time_tolerance: 0.005 # Time tolerance for joining KFs +keyframe_vote: + voting_active: false +apply_loss_function: false +velocity_local: [1, 0, 0] +angle_stdev: 0.05 +min_vel_norm: 1 \ No newline at end of file diff --git a/yaml/processor_gnss.yaml b/yaml/processor_gnss.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f9016bd04333c4cca66e404d8bb44d88b219655d --- /dev/null +++ b/yaml/processor_gnss.yaml @@ -0,0 +1,45 @@ +time_tolerance: 0.1 +apply_loss_function: false +max_new_features: -1 #unlimited +remove_outliers: true +remove_outliers_with_fix: true +outlier_residual_th: 10 +init_frames: false +pseudo_ranges: true +fix: false +enu_map_fix_dist: 5 +keyframe_vote: + voting_active: true + max_time_span: 0.5 + min_features_for_keyframe: 0 #4 +gnss: + sateph: 6 # satellite ephemeris/clock: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC) + ionoopt: 9 # ionosphere option: IONOOPT_OFF(0):correction off, IONOOPT_BRDC(1):broadcast mode, IONOOPT_SBAS(2):SBAS model, IONOOPT_IFLC(3):L1/L2 or L1/L5, IONOOPT_EST(4):estimation, IONOOPT_TEC(5):IONEX TEC model, IONOOPT_QZS(6):QZSS broadcast, IONOOPT_LEX(7):QZSS LEX ionosphere, IONOOPT_STEC(8):SLANT TEC mode, IONOOPT_SBAS2(9):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), IONOOPT_SBAS3(10):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_SBAS2), IONOOPT_SBAS4(8):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_BRDC) + tropopt: 2 # troposphere option: (0:correction off,1:Saastamoinen model,2:SBAS model,3:troposphere option: ZTD estimation,4:ZTD+grad estimation,5:ZTD correction,6:ZTD+grad correction) + sbascorr: 15 # SBAS option (1:long term correction,2:fast correction,4:ionosphere correction,8:ranging) + raim: 1 # apply RAIM + elmin: 0.26 # elevation min (rad) = 15 degrees + maxgdop: 30 # reject threshold of gdop (ground dilusion of precision) + min_sbas_sats: 5 # used in case of EPHOPT_SBAS3, EPHOPT_SBAS4, IONOOPT_SBAS3, IONOOPT_SBAS4 + constellations: + GPS: true + SBS: false + GLO: false + GAL: true + QZS: false + CMP: false + IRN: false + LEO: false + tdcp: + enabled: true + structure: consecutive + corr_iono: true + corr_tropo: true + corr_clock: true + loss_function: false + time_window: 10 # s + sigma_atm: 0.0016 #1.6mm/sqrt(s) # m/sqrt(s) + sigma_carrier: 0.0011 #1.1mm/sqrt(s) # m + multi_freq: false + remove_outliers: false + batch: false \ No newline at end of file diff --git a/yaml/processor_imu.yaml b/yaml/processor_imu.yaml new file mode 100644 index 0000000000000000000000000000000000000000..dd32067a62379b311527d7178ef97a2e8efec44a --- /dev/null +++ b/yaml/processor_imu.yaml @@ -0,0 +1,13 @@ +time_tolerance: 0.005 # Time tolerance for joining KFs +unmeasured_perturbation_std: 0.001 +keyframe_vote: + voting_active: false + max_time_span: 99999 # seconds + max_buff_length: 99999 # motion deltas + dist_traveled: 99999 # meters + angle_turned: 99999 # radians (1 rad approx 57 deg, approx 60 deg) + cov_det: 99999 +apply_loss_function: false + +state_getter: true +state_priority: 1 \ No newline at end of file diff --git a/yaml/sensor_bno.yaml b/yaml/sensor_bno.yaml new file mode 100644 index 0000000000000000000000000000000000000000..68277838cb2d6fc76815e73abff713afe06a1a97 --- /dev/null +++ b/yaml/sensor_bno.yaml @@ -0,0 +1,10 @@ +extrinsic: + pose: [0,0,0,0,0,0,1] +a_noise: 0.53 # standard deviation of Acceleration noise (same for all the axis) in m/s2 (1/1000 max value) +w_noise: 0.011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec (1/1000 max value) +prior_bias: true +ab_initial_stdev: 0.05 # m/s2 - initial bias +wb_initial_stdev: 0.01 # rad/sec - initial bias +bias_dynamic: false +ab_rate_stdev: 0.00001 # m/s2/sqrt(s) +wb_rate_stdev: 0.00001 # rad/s/sqrt(s) diff --git a/yaml/sensor_f9t.yaml b/yaml/sensor_f9t.yaml new file mode 100644 index 0000000000000000000000000000000000000000..2917affecf2faed0c7c45be280032e0800a918a8 --- /dev/null +++ b/yaml/sensor_f9t.yaml @@ -0,0 +1,10 @@ +extrinsics_fixed: true +clock_bias_GPS_GLO_dynamic: false +clock_bias_GPS_GAL_dynamic: false +clock_bias_GPS_CMP_dynamic: false +ENU: + mode: "auto" + roll_fixed: true + pitch_fixed: true + yaw_fixed: true + translation_fixed: true \ No newline at end of file diff --git a/yaml/subscriber_gnss_ublox.yaml b/yaml/subscriber_gnss_ublox.yaml new file mode 100644 index 0000000000000000000000000000000000000000..49e802099c834055f59f085b0e1fe32dcf064266 --- /dev/null +++ b/yaml/subscriber_gnss_ublox.yaml @@ -0,0 +1,24 @@ +process_not_available: false +publish_fix: + enabled: true + topic: "RTKLIB_fix" +gnss_options: + sateph: 6 # satellite ephemeris/clock (0:broadcast ephemeris,1:precise ephemeris,2:broadcast + SBAS,3:ephemeris option: broadcast + SSR_APC,4:broadcast + SSR_COM,5: QZSS LEX ephemeris + ionoopt: 9 # ionosphere option (0:correction off,1:broadcast mode,2:SBAS model,3:L1/L2 or L1/L5,4:estimation,5:IONEX TEC model,6:QZSS broadcast,7:QZSS LEX ionosphere,8:SLANT TEC mode) + tropopt: 2 # troposphere option: (0:correction off,1:Saastamoinen model,2:SBAS model,3:troposphere option: ZTD estimation,4:ZTD+grad estimation,5:ZTD correction,6:ZTD+grad correction) + sbascorr: 15 # SBAS option (1:long term correction,2:fast correction,4:ionosphere correction,8:ranging) + raim: 1 # apply RAIM n times + elmin: 0.26 # elevation min (rad) = 15 degrees + maxgdop: 30 # reject threshold of gdop (ground dilusion of precision) + constellations: + GPS: true + SBS: false + GLO: false + GAL: true + QZS: false + CMP: false + IRN: false + LEO: false +reset_receiver: true +reset_max_topic_delay: 1 +reset_duration: 20 \ No newline at end of file diff --git a/yaml/subscriber_imu.yaml b/yaml/subscriber_imu.yaml new file mode 100644 index 0000000000000000000000000000000000000000..1866e544ea6ab1a3103ab73d3b147709e81b571b --- /dev/null +++ b/yaml/subscriber_imu.yaml @@ -0,0 +1,16 @@ +enableable: true +topic_enable: "gnss_available" + +imu_x_axis: 1 +imu_y_axis: 2 +imu_z_axis: 3 + +cov_source: "msg" + +in_degrees: true + +static_init_duration: 30.0 + +lowpass_filter: true +lowpass_cutoff_freq: 2 +dt: 0.01 \ No newline at end of file