diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index c137d84ac2..c5ae9e2ffe 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -836,6 +836,8 @@ void statusFTDI() { // run logger sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger on"' // status commands + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "board_adc test"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander check"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander status"' @@ -846,14 +848,15 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "df"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "gps status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener adc_report"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener battery_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener cpuload"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_sensor"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_attitude"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_local_position"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_selector_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_sensor_bias"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener logger_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel_fifo"' @@ -901,6 +904,8 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1 -a"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"' // stop logger sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger off"' } @@ -909,6 +914,8 @@ void statusSEGGER() { // run logger sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger on"' // status commands + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "free"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "work_queue status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "board_adc test"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander check"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander status"' @@ -919,12 +926,13 @@ void statusSEGGER() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dataman status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "df -h"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dmesg"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ekf2 status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "free"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "gps status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener adc_report"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener battery_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener cpuload"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_attitude"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_local_position"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_selector_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_sensor_bias"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener logger_status"' @@ -974,6 +982,8 @@ void statusSEGGER() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "uorb top -1 -a"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ver all"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "work_queue status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ekf2 status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "free"' // stop logger sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger off"' } diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index e0fa783d97..340cc01901 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -141,6 +141,12 @@ then # Speedup SITL startup param set EKF2_REQ_GPS_H 0.5 + # Multi-EKF + #param set EKF2_MULTI_IMU 3 + #param set SENS_IMU_MODE 0 + #param set EKF2_MULTI_MAG 2 + #param set SENS_MAG_MODE 0 + # By default log from boot until first disarm. param set SDLOG_MODE 1 # enable default, estimator replay and vision/avoidance logging profiles diff --git a/ROMFS/px4fmu_common/init.d/rc.airship_apps b/ROMFS/px4fmu_common/init.d/rc.airship_apps index 447d9609ac..766a17347e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.airship_apps +++ b/ROMFS/px4fmu_common/init.d/rc.airship_apps @@ -40,7 +40,7 @@ else # EKF2 # param set SYS_MC_EST_GROUP 2 - ekf2 start + ekf2 start & fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index c565aebf46..95c5a365db 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -8,7 +8,7 @@ # # Start the attitude and position estimator. # -ekf2 start +ekf2 start & # # Start attitude controller. diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 32f2f18df0..d3ddd3ed87 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -40,7 +40,7 @@ else # EKF2 # param set SYS_MC_EST_GROUP 2 - ekf2 start + ekf2 start & fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_apps b/ROMFS/px4fmu_common/init.d/rc.rover_apps index dfef3e4eeb..87a77281e3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_apps @@ -8,7 +8,7 @@ # # Start the attitude and position estimator. # -ekf2 start +ekf2 start & #attitude_estimator_q start #local_position_estimator start diff --git a/ROMFS/px4fmu_common/init.d/rc.uuv_apps b/ROMFS/px4fmu_common/init.d/rc.uuv_apps index 5219f11986..1e4074b727 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uuv_apps +++ b/ROMFS/px4fmu_common/init.d/rc.uuv_apps @@ -9,7 +9,7 @@ # Begin Estimator Group Selection # ############################################################################### -ekf2 start +ekf2 start & ############################################################################### # End Estimator Group Selection # diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index 2f8b0a53d6..259dcf6594 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -199,5 +199,5 @@ fi if [ $VEHICLE_TYPE = none ] then echo "No autostart ID found" - ekf2 start + ekf2 start & fi diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 40a1e3df23..c3f0c84f8f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -9,7 +9,7 @@ # Begin Estimator group selection # ############################################################################### -ekf2 start +ekf2 start & ############################################################################### # End Estimator group selection # diff --git a/Tools/ecl_ekf/analyse_logdata_ekf.py b/Tools/ecl_ekf/analyse_logdata_ekf.py index 2693e246e8..b33fd86104 100644 --- a/Tools/ecl_ekf/analyse_logdata_ekf.py +++ b/Tools/ecl_ekf/analyse_logdata_ekf.py @@ -14,13 +14,14 @@ from analysis.checks import perform_ecl_ekf_checks from analysis.post_processing import get_estimator_check_flags def analyse_ekf( - ulog: ULog, check_levels: Dict[str, float], red_thresh: float = 1.0, - amb_thresh: float = 0.5, min_flight_duration_seconds: float = 5.0, + ulog: ULog, check_levels: Dict[str, float], multi_instance: int = 0, + red_thresh: float = 1.0, amb_thresh: float = 0.5, min_flight_duration_seconds: float = 5.0, in_air_margin_seconds: float = 5.0, pos_checks_when_sensors_not_fused: bool = False) -> \ Tuple[str, Dict[str, str], Dict[str, float], Dict[str, float]]: """ :param ulog: :param check_levels: + :param multi_instance: :param red_thresh: :param amb_thresh: :param min_flight_duration_seconds: @@ -30,22 +31,19 @@ def analyse_ekf( """ try: - estimator_states = ulog.get_dataset('estimator_states').data - print('found estimator_states data') + estimator_states = ulog.get_dataset('estimator_states', multi_instance).data except: - raise PreconditionError('could not find estimator_states data') + raise PreconditionError('could not find estimator_states instance', multi_instance) try: - estimator_status = ulog.get_dataset('estimator_status').data - print('found estimator_status data') + estimator_status = ulog.get_dataset('estimator_status', multi_instance).data except: - raise PreconditionError('could not find estimator_status data') + raise PreconditionError('could not find estimator_status instance', multi_instance) try: - _ = ulog.get_dataset('estimator_innovations').data - print('found estimator_innovations data') + _ = ulog.get_dataset('estimator_innovations', multi_instance).data except: - raise PreconditionError('could not find estimator_innovations data') + raise PreconditionError('could not find estimator_innovations instance', multi_instance) try: in_air = InAirDetector( @@ -71,7 +69,7 @@ def analyse_ekf( metrics = calculate_ecl_ekf_metrics( ulog, innov_flags, innov_fail_checks, sensor_checks, in_air, in_air_no_ground_effects, - red_thresh=red_thresh, amb_thresh=amb_thresh) + multi_instance, red_thresh=red_thresh, amb_thresh=amb_thresh) check_status, master_status = perform_ecl_ekf_checks( metrics, sensor_checks, innov_fail_checks, check_levels) diff --git a/Tools/ecl_ekf/analysis/metrics.py b/Tools/ecl_ekf/analysis/metrics.py index 597593ea32..22b6a61931 100644 --- a/Tools/ecl_ekf/analysis/metrics.py +++ b/Tools/ecl_ekf/analysis/metrics.py @@ -13,7 +13,7 @@ from analysis.detectors import InAirDetector def calculate_ecl_ekf_metrics( ulog: ULog, innov_flags: Dict[str, float], innov_fail_checks: List[str], sensor_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector, - red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]: + multi_instance: int = 0, red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]: sensor_metrics = calculate_sensor_metrics( ulog, sensor_checks, in_air, in_air_no_ground_effects, @@ -22,9 +22,9 @@ def calculate_ecl_ekf_metrics( innov_fail_metrics = calculate_innov_fail_metrics( innov_flags, innov_fail_checks, in_air, in_air_no_ground_effects) - imu_metrics = calculate_imu_metrics(ulog, in_air_no_ground_effects) + imu_metrics = calculate_imu_metrics(ulog, multi_instance, in_air_no_ground_effects) - estimator_status_data = ulog.get_dataset('estimator_status').data + estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data # Check for internal filter nummerical faults ekf_metrics = {'filter_faults_max': np.amax(estimator_status_data['filter_fault_flags'])} @@ -44,10 +44,10 @@ def calculate_ecl_ekf_metrics( def calculate_sensor_metrics( ulog: ULog, sensor_checks: List[str], in_air: InAirDetector, - in_air_no_ground_effects: InAirDetector, red_thresh: float = 1.0, - amb_thresh: float = 0.5) -> Dict[str, float]: + in_air_no_ground_effects: InAirDetector, multi_instance: int = 0, + red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Dict[str, float]: - estimator_status_data = ulog.get_dataset('estimator_status').data + estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data sensor_metrics = dict() @@ -131,10 +131,9 @@ def calculate_innov_fail_metrics( return innov_fail_metrics -def calculate_imu_metrics( - ulog: ULog, in_air_no_ground_effects: InAirDetector) -> dict: +def calculate_imu_metrics(ulog: ULog, multi_instance, in_air_no_ground_effects: InAirDetector) -> dict: - estimator_status_data = ulog.get_dataset('estimator_status').data + estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data imu_metrics = dict() @@ -158,7 +157,7 @@ def calculate_imu_metrics( in_air_no_ground_effects, np.mean) # IMU bias checks - estimator_states_data = ulog.get_dataset('estimator_states').data + estimator_states_data = ulog.get_dataset('estimator_states', multi_instance).data imu_metrics['imu_dang_bias_median'] = np.sqrt(np.sum([np.square(calculate_stat_from_signal( estimator_states_data, 'estimator_states', signal, in_air_no_ground_effects, np.median)) diff --git a/Tools/ecl_ekf/plotting/pdf_report.py b/Tools/ecl_ekf/plotting/pdf_report.py index 7e7041cb37..b4c10ac03f 100644 --- a/Tools/ecl_ekf/plotting/pdf_report.py +++ b/Tools/ecl_ekf/plotting/pdf_report.py @@ -17,7 +17,7 @@ from plotting.data_plots import TimeSeriesPlot, InnovationPlot, ControlModeSumma from analysis.detectors import PreconditionError import analysis.data_version_handler as dvh -def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: +def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str) -> None: """ creates a pdf report of the ekf analysis. :param ulog: @@ -29,20 +29,18 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: # save the plots to PDF try: - estimator_status = ulog.get_dataset('estimator_status').data - print('found estimator_status data') + estimator_status = ulog.get_dataset('estimator_status', multi_instance).data except: - raise PreconditionError('could not find estimator_status data') + raise PreconditionError('could not find estimator_status instance', multi_instance) try: - estimator_states = ulog.get_dataset('estimator_states').data - print('found estimator_states data') + estimator_states = ulog.get_dataset('estimator_states', multi_instance).data except: - raise PreconditionError('could not find estimator_states data') + raise PreconditionError('could not find estimator_states instance', multi_instance) try: - estimator_innovations = ulog.get_dataset('estimator_innovations').data - estimator_innovation_variances = ulog.get_dataset('estimator_innovation_variances').data + estimator_innovations = ulog.get_dataset('estimator_innovations', multi_instance).data + estimator_innovation_variances = ulog.get_dataset('estimator_innovation_variances', multi_instance).data innovation_data = estimator_innovations for key in estimator_innovation_variances: # append 'var' to the field name such that we can distingush between innov and innov_var @@ -65,7 +63,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: for key in innovation_data: innovation_data[key] = innovation_data[key][0:innovation_data_min_length] - print('found innovation data (merged estimator_innovations + estimator_innovation_variances)') + print('found innovation data (merged estimator_innovations + estimator_innovation_variances) instance', multi_instance) except: raise PreconditionError('could not find innovation data') diff --git a/Tools/ecl_ekf/process_logdata_ekf.py b/Tools/ecl_ekf/process_logdata_ekf.py index 14c6df2167..d533070e9b 100755 --- a/Tools/ecl_ekf/process_logdata_ekf.py +++ b/Tools/ecl_ekf/process_logdata_ekf.py @@ -90,6 +90,21 @@ def process_logdata_ekf( except: raise PreconditionError('could not open {:s}'.format(filename)) + ekf_instances = 1 + + try: + estimator_selector_status = ulog.get_dataset('estimator_selector_status',).data + print('found estimator_selector_status (multi-ekf) data') + + for instances_available in estimator_selector_status['instances_available']: + if instances_available > ekf_instances: + ekf_instances = instances_available + + print(ekf_instances, 'ekf instances') + + except: + print('could not find estimator_selector_status data') + try: # get the dictionary of fail and warning test thresholds from a csv file with open(check_level_dict_filename, 'r') as file: @@ -100,30 +115,35 @@ def process_logdata_ekf( raise PreconditionError('could not find {:s}'.format(check_level_dict_filename)) in_air_margin = 5.0 if sensor_safety_margins else 0.0 - # perform the ekf analysis - master_status, check_status, metrics, airtime_info = analyse_ekf( - ulog, check_levels, red_thresh=1.0, amb_thresh=0.5, min_flight_duration_seconds=5.0, - in_air_margin_seconds=in_air_margin) - - test_results = create_results_table( - check_table_filename, master_status, check_status, metrics, airtime_info) - - # write metadata to a .csv file - with open('{:s}.mdat.csv'.format(filename), "w") as file: - - file.write("name,value,description\n") - - # loop through the test results dictionary and write each entry on a separate row, with data comma separated - # save data in alphabetical order - key_list = list(test_results.keys()) - key_list.sort() - for key in key_list: - file.write(key + "," + str(test_results[key][0]) + "," + test_results[key][1] + "\n") - print('Test results written to {:s}.mdat.csv'.format(filename)) - - if plot: - create_pdf_report(ulog, '{:s}.pdf'.format(filename)) - print('Plots saved to {:s}.pdf'.format(filename)) + + for multi_instance in range(ekf_instances): + + print('\nestimator instance:', multi_instance) + + # perform the ekf analysis + master_status, check_status, metrics, airtime_info = analyse_ekf( + ulog, check_levels, multi_instance, red_thresh=1.0, amb_thresh=0.5, min_flight_duration_seconds=5.0, + in_air_margin_seconds=in_air_margin) + + test_results = create_results_table( + check_table_filename, master_status, check_status, metrics, airtime_info) + + # write metadata to a .csv file + with open('{:s}-{:d}.mdat.csv'.format(filename, multi_instance), "w") as file: + + file.write("name,value,description\n") + + # loop through the test results dictionary and write each entry on a separate row, with data comma separated + # save data in alphabetical order + key_list = list(test_results.keys()) + key_list.sort() + for key in key_list: + file.write(key + "," + str(test_results[key][0]) + "," + test_results[key][1] + "\n") + print('Test results written to {:s}-{:d}.mdat.csv'.format(filename, multi_instance)) + + if plot: + create_pdf_report(ulog, multi_instance, '{:s}-{:d}.pdf'.format(filename, multi_instance)) + print('Plots saved to {:s}-{:d}.pdf'.format(filename, multi_instance)) return test_results diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 9b563274e4..55cc6c5b90 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -57,6 +57,7 @@ set(msg_files esc_status.msg estimator_innovations.msg estimator_optical_flow_vel.msg + estimator_selector_status.msg estimator_sensor_bias.msg estimator_states.msg estimator_status.msg diff --git a/msg/estimator_selector_status.msg b/msg/estimator_selector_status.msg new file mode 100644 index 0000000000..52f8085901 --- /dev/null +++ b/msg/estimator_selector_status.msg @@ -0,0 +1,22 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 primary_instance + +uint8 instances_available + +uint32 instance_changed_count +uint64 last_instance_change + +uint32 accel_device_id +uint32 baro_device_id +uint32 gyro_device_id +uint32 mag_device_id + +float32[9] combined_test_ratio +float32[9] relative_test_ratio +bool[9] healthy + +float32[4] accumulated_gyro_error +float32[4] accumulated_accel_error +bool gyro_fault_detected +bool accel_fault_detected diff --git a/msg/sensors_status_imu.msg b/msg/sensors_status_imu.msg index 769e7205e3..6185e6cabc 100644 --- a/msg/sensors_status_imu.msg +++ b/msg/sensors_status_imu.msg @@ -5,13 +5,13 @@ uint64 timestamp # time since system start (microseconds) uint32 accel_device_id_primary # current primary accel device id for reference -uint32[3] accel_device_ids -float32[3] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and primary in (m/s/s). -bool[3] accel_healthy +uint32[4] accel_device_ids +float32[4] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and mean in (m/s/s). +bool[4] accel_healthy uint32 gyro_device_id_primary # current primary gyro device id for reference -uint32[3] gyro_device_ids -float32[3] gyro_inconsistency_rad_s # magnitude of angular rate difference between IMU instance and primary in (rad/s). -bool[3] gyro_healthy +uint32[4] gyro_device_ids +float32[4] gyro_inconsistency_rad_s # magnitude of angular rate difference between IMU instance and mean in (rad/s). +bool[4] gyro_healthy diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index a382012e54..80c3f962bc 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -287,6 +287,8 @@ rtps: id: 137 - msg: estimator_optical_flow_vel id: 138 + - msg: estimator_selector_status + id: 139 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index 8fbc874374..3c24cfb45c 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -9,3 +9,4 @@ float32[4] delta_q_reset # Amount by which quaternion has changed during last r uint8 quat_reset_counter # Quaternion reset counter # TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude +# TOPICS estimator_attitude diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index 3da984de66..ce16daecc8 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -26,3 +26,4 @@ bool terrain_alt_valid # Terrain altitude estimate is valid bool dead_reckoning # True if this position is estimated through dead-reckoning # TOPICS vehicle_global_position vehicle_global_position_groundtruth +# TOPICS estimator_global_position diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index bc393b5f62..259f05fbdb 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -65,3 +65,4 @@ float32 hagl_min # minimum height above ground level - set to 0 when limiting float32 hagl_max # maximum height above ground level - set to 0 when limiting not required (meters) # TOPICS vehicle_local_position vehicle_local_position_groundtruth +# TOPICS estimator_local_position diff --git a/msg/vehicle_odometry.msg b/msg/vehicle_odometry.msg index bd100a4eee..4a9dce823f 100644 --- a/msg/vehicle_odometry.msg +++ b/msg/vehicle_odometry.msg @@ -63,3 +63,4 @@ float32 yawspeed # Angular velocity about Z body axis float32[21] velocity_covariance # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry vehicle_visual_odometry_aligned +# TOPICS estimator_odometry estimator_visual_odometry_aligned diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 41edbd228f..7d6930c0d7 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -65,23 +65,27 @@ static constexpr wq_config_t I2C3{"wq:I2C3", 1472, -11}; static constexpr wq_config_t I2C4{"wq:I2C4", 1472, -12}; // PX4 att/pos controllers, highest priority after sensors. -static constexpr wq_config_t attitude_ctrl{"wq:attitude_ctrl", 1672, -13}; -static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 7200, -14}; +static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 1728, -13}; -static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -15}; +static constexpr wq_config_t INS0{"wq:INS0", 7200, -14}; +static constexpr wq_config_t INS1{"wq:INS1", 7200, -15}; +static constexpr wq_config_t INS2{"wq:INS2", 7200, -16}; +static constexpr wq_config_t INS3{"wq:INS3", 7200, -17}; -static constexpr wq_config_t uavcan{"wq:uavcan", 3000, -16}; +static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18}; -static constexpr wq_config_t UART0{"wq:UART0", 1400, -17}; -static constexpr wq_config_t UART1{"wq:UART1", 1400, -18}; -static constexpr wq_config_t UART2{"wq:UART2", 1400, -19}; -static constexpr wq_config_t UART3{"wq:UART3", 1400, -20}; -static constexpr wq_config_t UART4{"wq:UART4", 1400, -21}; -static constexpr wq_config_t UART5{"wq:UART5", 1400, -22}; -static constexpr wq_config_t UART6{"wq:UART6", 1400, -23}; -static constexpr wq_config_t UART7{"wq:UART7", 1400, -24}; -static constexpr wq_config_t UART8{"wq:UART8", 1400, -25}; -static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -26}; +static constexpr wq_config_t uavcan{"wq:uavcan", 3000, -19}; + +static constexpr wq_config_t UART0{"wq:UART0", 1400, -21}; +static constexpr wq_config_t UART1{"wq:UART1", 1400, -22}; +static constexpr wq_config_t UART2{"wq:UART2", 1400, -23}; +static constexpr wq_config_t UART3{"wq:UART3", 1400, -24}; +static constexpr wq_config_t UART4{"wq:UART4", 1400, -25}; +static constexpr wq_config_t UART5{"wq:UART5", 1400, -26}; +static constexpr wq_config_t UART6{"wq:UART6", 1400, -27}; +static constexpr wq_config_t UART7{"wq:UART7", 1400, -28}; +static constexpr wq_config_t UART8{"wq:UART8", 1400, -29}; +static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -30}; static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50}; @@ -129,5 +133,7 @@ const wq_config_t &device_bus_to_wq(uint32_t device_id); */ const wq_config_t &serial_port_to_wq(const char *serial); +const wq_config_t &ins_instance_to_wq(uint8_t instance); + } // namespace px4 diff --git a/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp index efbabe0a9a..7486e0bee4 100644 --- a/platforms/common/px4_work_queue/WorkQueueManager.cpp +++ b/platforms/common/px4_work_queue/WorkQueueManager.cpp @@ -201,6 +201,23 @@ serial_port_to_wq(const char *serial) return wq_configurations::UART_UNKNOWN; } +const wq_config_t &ins_instance_to_wq(uint8_t instance) +{ + switch (instance) { + case 0: return wq_configurations::INS0; + + case 1: return wq_configurations::INS1; + + case 2: return wq_configurations::INS2; + + case 3: return wq_configurations::INS3; + } + + PX4_WARN("no INS%d wq configuration, using INS0", instance); + + return wq_configurations::INS0; +} + static void * WorkQueueRunner(void *context) { diff --git a/src/lib/cdev/posix/cdev_platform.cpp b/src/lib/cdev/posix/cdev_platform.cpp index 0cad37ad3a..30b59279b2 100644 --- a/src/lib/cdev/posix/cdev_platform.cpp +++ b/src/lib/cdev/posix/cdev_platform.cpp @@ -62,9 +62,9 @@ private: px4_dev_t() = default; }; -static px4_dev_t *devmap[128] {}; +static px4_dev_t *devmap[256] {}; -#define PX4_MAX_FD 128 +#define PX4_MAX_FD 256 static cdev::file_t filemap[PX4_MAX_FD] {}; class VFile : public cdev::CDev diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 4e0a303660..cc7f8b9f2a 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -105,6 +106,7 @@ private: uORB::PublicationMulti _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */ orb_advert_t _mavlink_log_pub {nullptr}; /**< mavlink log topic*/ + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; uORB::Subscription _param_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; @@ -430,6 +432,17 @@ void AirspeedModule::update_params() void AirspeedModule::poll_topics() { + // use primary estimator_status + if (_estimator_selector_status_sub.updated()) { + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { + if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) { + _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance); + } + } + } + _estimator_status_sub.update(&_estimator_status); _vehicle_acceleration_sub.update(&_accel); _vehicle_air_data_sub.update(&_vehicle_air_data); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index 953b32f02e..de72b5f5f1 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -66,7 +67,8 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & bool gps_present = true; // Get estimator status data if available and exit with a fail recorded if not - uORB::SubscriptionData status_sub{ORB_ID(estimator_status)}; + uORB::SubscriptionData estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; + uORB::SubscriptionData status_sub{ORB_ID(estimator_status), estimator_selector_status_sub.get().primary_instance}; status_sub.update(); const estimator_status_s &status = status_sub.get(); @@ -238,7 +240,9 @@ bool PreFlightCheck::ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool r param_get(param_find("COM_ARM_EKF_GB"), &ekf_gb_test_limit); // Get estimator states data if available and exit with a fail recorded if not - uORB::Subscription states_sub{ORB_ID(estimator_states)}; + uORB::SubscriptionData estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; + uORB::Subscription states_sub{ORB_ID(estimator_states), estimator_selector_status_sub.get().primary_instance}; + estimator_states_s states; if (states_sub.copy(&states)) { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 64a90636cc..9e5b86f617 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -4055,6 +4055,17 @@ void Commander::estimator_check(const vehicle_status_flags_s &vstatus_flags) const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)); + // use primary estimator_status + if (_estimator_selector_status_sub.updated()) { + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { + if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) { + _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance); + } + } + } + if (_estimator_status_sub.update()) { const estimator_status_s &estimator_status = _estimator_status_sub.get(); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index a80596d9cd..46a8c48775 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -384,6 +385,7 @@ private: uORB::Subscription _cmd_sub {ORB_ID(vehicle_command)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)}; uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 6292baf227..c26f6727ea 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -41,10 +41,13 @@ px4_add_module( SRCS EKF2.cpp EKF2.hpp + EKF2Selector.cpp + EKF2Selector.hpp DEPENDS git_ecl ecl_EKF ecl_geo perf EKF2Utility + px4_work_queue ) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 154175f3e5..f354f09ef4 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -37,11 +37,25 @@ using namespace time_literals; using math::constrain; -EKF2::EKF2(bool replay_mode): +pthread_mutex_t ekf2_module_mutex = PTHREAD_MUTEX_INITIALIZER; +static px4::atomic _objects[EKF2_MAX_INSTANCES] {}; +#if !defined(CONSTRAINED_FLASH) +static px4::atomic _ekf2_selector {nullptr}; +#endif // !CONSTRAINED_FLASH + +EKF2::EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool replay_mode): ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), - _replay_mode(replay_mode), + ScheduledWorkItem(MODULE_NAME, config), + _replay_mode(replay_mode && instance < 0), + _multi_mode(instance >= 0), + _instance(math::constrain(instance, 0, EKF2_MAX_INSTANCES - 1)), _ekf_update_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": update")), + _attitude_pub(_multi_mode ? ORB_ID(estimator_attitude) : ORB_ID(vehicle_attitude)), + _local_position_pub(_multi_mode ? ORB_ID(estimator_local_position) : ORB_ID(vehicle_local_position)), + _global_position_pub(_multi_mode ? ORB_ID(estimator_global_position) : ORB_ID(vehicle_global_position)), + _odometry_pub(_multi_mode ? ORB_ID(estimator_odometry) : ORB_ID(vehicle_odometry)), + _visual_odometry_aligned_pub(_multi_mode ? ORB_ID(estimator_visual_odometry_aligned) : + ORB_ID(vehicle_visual_odometry_aligned)), _params(_ekf.getParamHandle()), _param_ekf2_min_obs_dt(_params->sensor_interval_min_ms), _param_ekf2_mag_delay(_params->mag_delay_ms), @@ -153,7 +167,12 @@ EKF2::EKF2(bool replay_mode): _ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s); // advertise immediately to ensure consistent uORB instance numbering - _att_pub.advertise(); + _attitude_pub.advertise(); + _local_position_pub.advertise(); + _global_position_pub.advertise(); + _odometry_pub.advertise(); + _visual_odometry_aligned_pub.advertise(); + _ekf2_timestamps_pub.advertise(); _ekf_gps_drift_pub.advertise(); _estimator_innovation_test_ratios_pub.advertise(); @@ -163,65 +182,29 @@ EKF2::EKF2(bool replay_mode): _estimator_sensor_bias_pub.advertise(); _estimator_states_pub.advertise(); _estimator_status_pub.advertise(); - _vehicle_global_position_pub.advertise(); - _vehicle_local_position_pub.advertise(); - _vehicle_odometry_pub.advertise(); - _vehicle_visual_odometry_aligned_pub.advertise(); _wind_pub.advertise(); _yaw_est_pub.advertise(); -} -EKF2::~EKF2() -{ - px4_lockstep_unregister_component(_lockstep_component); - perf_free(_ekf_update_perf); + if (_multi_mode) { + _vehicle_imu_sub.ChangeInstance(imu); + _magnetometer_sub.ChangeInstance(mag); + } } -bool EKF2::init() +EKF2::~EKF2() { - const uint32_t device_id = _param_ekf2_imu_id.get(); - - // if EKF2_IMU_ID is non-zero we use the corresponding IMU, otherwise the voted primary (sensor_combined) - if (device_id != 0) { - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - vehicle_imu_s imu; - - if (_vehicle_imu_sub.ChangeInstance(i) && _vehicle_imu_sub.copy(&imu)) { - if ((imu.accel_device_id > 0) && (imu.accel_device_id == device_id)) { - if (_vehicle_imu_sub.registerCallback()) { - PX4_INFO("subscribed to vehicle_imu:%d (%d)", i, device_id); - _imu_sub_index = i; - _callback_registered = true; - return true; - } - } - } - } - - } else { - _imu_sub_index = -1; - - if (_sensor_combined_sub.registerCallback()) { - _callback_registered = true; - return true; - } + if (!_multi_mode) { + px4_lockstep_unregister_component(_lockstep_component); } - PX4_WARN("failed to register callback, retrying in 1 second"); - ScheduleDelayed(1_s); // retry in 1 second - - return true; + perf_free(_ekf_update_perf); } int EKF2::print_status() { - PX4_INFO("local position: %s", (_ekf.local_position_is_valid()) ? "valid" : "invalid"); - PX4_INFO("global position: %s", (_ekf.global_position_is_valid()) ? "valid" : "invalid"); - - PX4_INFO("time slip: %" PRId64 " us", _last_time_slip_us); - + PX4_INFO_RAW("ekf2:%d attitude: %d, local position: %d, global position: %d\n", _instance, _ekf.attitude_valid(), + _ekf.local_position_is_valid(), _ekf.global_position_is_valid()); perf_print_counter(_ekf_update_perf); - return 0; } @@ -238,7 +221,11 @@ void EKF2::update_mag_bias(Param &mag_bias_param, int axis_index) _last_valid_mag_cal[axis_index] = weighting * _last_valid_mag_cal[axis_index] + mag_bias_saved; mag_bias_param.set(_last_valid_mag_cal[axis_index]); - mag_bias_param.commit_no_notification(); + + // save new parameters unless in multi-instance mode + if (!_multi_mode) { + mag_bias_param.commit_no_notification(); + } _valid_cal_available[axis_index] = false; } @@ -252,7 +239,11 @@ bool EKF2::update_mag_decl(Param &mag_decl_param) if (_ekf.get_mag_decl_deg(&declination_deg)) { mag_decl_param.set(declination_deg); - mag_decl_param.commit_no_notification(); + + if (!_multi_mode) { + mag_decl_param.commit_no_notification(); + } + return true; } @@ -265,13 +256,22 @@ void EKF2::Run() _sensor_combined_sub.unregisterCallback(); _vehicle_imu_sub.unregisterCallback(); - exit_and_cleanup(); return; } if (!_callback_registered) { - init(); - return; + if (_multi_mode) { + _callback_registered = _vehicle_imu_sub.registerCallback(); + + } else { + _callback_registered = _sensor_combined_sub.registerCallback(); + } + + if (!_callback_registered) { + PX4_WARN("%d failed to register callback, retrying", _instance); + ScheduleDelayed(1_s); + return; + } } bool updated = false; @@ -279,7 +279,7 @@ void EKF2::Run() hrt_abstime imu_dt = 0; // for tracking time slip later - if (_imu_sub_index >= 0) { + if (_multi_mode) { vehicle_imu_s imu; updated = _vehicle_imu_sub.update(&imu); @@ -357,20 +357,18 @@ void EKF2::Run() } // Always update sensor selction first time through if time stamp is non zero - if (_sensor_selection_sub.updated() || (_sensor_selection.timestamp == 0)) { + if (!_multi_mode && (_sensor_selection_sub.updated() || (_sensor_selection.timestamp == 0))) { const sensor_selection_s sensor_selection_prev = _sensor_selection; if (_sensor_selection_sub.copy(&_sensor_selection)) { if ((sensor_selection_prev.timestamp > 0) && (_sensor_selection.timestamp > sensor_selection_prev.timestamp)) { - if (_imu_sub_index < 0) { - if (_sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) { - _imu_bias_reset_request = true; - } + if (_sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) { + _imu_bias_reset_request = true; + } - if (_sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) { - _imu_bias_reset_request = true; - } + if (_sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) { + _imu_bias_reset_request = true; } } @@ -417,22 +415,27 @@ void EKF2::Run() if ((_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) && (_invalid_mag_id_count > 100)) { // the sensor ID used for the last saved mag bias is not confirmed to be the same as the current sensor ID // this means we need to reset the learned bias values to zero - _param_ekf2_magbias_x.reset(); - _param_ekf2_magbias_y.reset(); - _param_ekf2_magbias_z.reset(); + _param_ekf2_magbias_x.set(0.f); + _param_ekf2_magbias_y.set(0.f); + _param_ekf2_magbias_z.set(0.f); _param_ekf2_magbias_id.set(magnetometer.device_id); - _param_ekf2_magbias_id.commit(); - _invalid_mag_id_count = 0; + if (!_multi_mode) { + _param_ekf2_magbias_x.reset(); + _param_ekf2_magbias_y.reset(); + _param_ekf2_magbias_z.reset(); + _param_ekf2_magbias_id.commit(); + PX4_INFO("Mag sensor ID changed to %i", _param_ekf2_magbias_id.get()); + } - PX4_INFO("Mag sensor ID changed to %i", _param_ekf2_magbias_id.get()); + _invalid_mag_id_count = 0; } magSample mag_sample {}; mag_sample.mag(0) = magnetometer.magnetometer_ga[0] - _param_ekf2_magbias_x.get(); mag_sample.mag(1) = magnetometer.magnetometer_ga[1] - _param_ekf2_magbias_y.get(); mag_sample.mag(2) = magnetometer.magnetometer_ga[2] - _param_ekf2_magbias_z.get(); - mag_sample.time_us = magnetometer.timestamp; + mag_sample.time_us = magnetometer.timestamp_sample; _ekf.setMagData(mag_sample); ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 - @@ -681,15 +684,15 @@ void EKF2::Run() // only publish position after successful alignment if (control_status.flags.tilt_align) { // generate vehicle local position data - vehicle_local_position_s &lpos = _vehicle_local_position_pub.get(); + vehicle_local_position_s &lpos = _local_position_pub.get(); lpos.timestamp_sample = imu_sample_new.time_us; // Position of body origin in local NED frame - Vector3f position = _ekf.getPosition(); + const Vector3f position = _ekf.getPosition(); const float lpos_x_prev = lpos.x; const float lpos_y_prev = lpos.y; - lpos.x = (_ekf.local_position_is_valid()) ? position(0) : 0.0f; - lpos.y = (_ekf.local_position_is_valid()) ? position(1) : 0.0f; + lpos.x = position(0); + lpos.y = position(1); lpos.z = position(2); // Velocity of body origin in local NED frame (m/s) @@ -801,7 +804,7 @@ void EKF2::Run() // publish vehicle local position data lpos.timestamp = _replay_mode ? now : hrt_absolute_time(); - _vehicle_local_position_pub.update(); + _local_position_pub.update(); // publish vehicle_odometry publish_odometry(now, imu_sample_new, lpos); @@ -848,12 +851,12 @@ void EKF2::Run() ev_quat_aligned.copyTo(aligned_ev_odom.q); quat_ev2ekf.copyTo(aligned_ev_odom.q_offset); - _vehicle_visual_odometry_aligned_pub.publish(aligned_ev_odom); + _visual_odometry_aligned_pub.publish(aligned_ev_odom); } if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) { // generate and publish global position data - vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get(); + vehicle_global_position_s &global_pos = _global_position_pub.get(); global_pos.timestamp_sample = imu_sample_new.time_us; if (fabsf(lpos_x_prev - lpos.x) > FLT_EPSILON || fabsf(lpos_y_prev - lpos.y) > FLT_EPSILON) { @@ -881,7 +884,7 @@ void EKF2::Run() global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning global_pos.timestamp = _replay_mode ? now : hrt_absolute_time(); - _vehicle_global_position_pub.update(); + _global_position_pub.update(); } } @@ -909,8 +912,8 @@ void EKF2::Run() status.hgt_test_ratio, status.tas_test_ratio, status.hagl_test_ratio, status.beta_test_ratio); - status.pos_horiz_accuracy = _vehicle_local_position_pub.get().eph; - status.pos_vert_accuracy = _vehicle_local_position_pub.get().epv; + status.pos_horiz_accuracy = _local_position_pub.get().eph; + status.pos_vert_accuracy = _local_position_pub.get().epv; _ekf.get_ekf_soln_status(&status.solution_status_flags); _ekf.getImuVibrationMetrics().copyTo(status.vibe); status.time_slip = _last_time_slip_us * 1e-6f; @@ -922,7 +925,8 @@ void EKF2::Run() status.timestamp = _replay_mode ? now : hrt_absolute_time(); _estimator_status_pub.update(); - { + // estimator_sensor_bias + if (status.filter_fault_flags == 0) { // publish all corrected sensor readings and bias estimates after mag calibration is updated above estimator_sensor_bias_s bias; bias.timestamp_sample = imu_sample_new.time_us; @@ -1143,11 +1147,13 @@ void EKF2::Run() // publish ekf2_timestamps _ekf2_timestamps_pub.publish(ekf2_timestamps); - if (_lockstep_component == -1) { - _lockstep_component = px4_lockstep_register_component(); - } + if (!_multi_mode) { + if (_lockstep_component == -1) { + _lockstep_component = px4_lockstep_register_component(); + } - px4_lockstep_progress(_lockstep_component); + px4_lockstep_progress(_lockstep_component); + } } } @@ -1221,13 +1227,13 @@ void EKF2::publish_attitude(const hrt_abstime ×tamp) _ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter); att.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); - _att_pub.publish(att); + _attitude_pub.publish(att); } else if (_replay_mode) { // in replay mode we have to tell the replay module not to wait for an update // we do this by publishing an attitude with zero timestamp vehicle_attitude_s att{}; - _att_pub.publish(att); + _attitude_pub.publish(att); } } @@ -1293,7 +1299,7 @@ void EKF2::publish_odometry(const hrt_abstime ×tamp, const imuSample &imu, // publish vehicle odometry data odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); - _vehicle_odometry_pub.publish(odom); + _odometry_pub.publish(odom); } void EKF2::publish_yaw_estimator_status(const hrt_abstime ×tamp) @@ -1383,6 +1389,7 @@ int EKF2::custom_command(int argc, char *argv[]) int EKF2::task_spawn(int argc, char *argv[]) { + bool success = false; bool replay_mode = false; if (argc > 1 && !strcmp(argv[1], "-r")) { @@ -1390,25 +1397,140 @@ int EKF2::task_spawn(int argc, char *argv[]) replay_mode = true; } - EKF2 *instance = new EKF2(replay_mode); +#if !defined(CONSTRAINED_FLASH) + bool multi_mode = false; + int32_t imu_instances = 0; + int32_t mag_instances = 0; - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + int32_t sens_imu_mode = 1; + param_get(param_find("SENS_IMU_MODE"), &sens_imu_mode); - if (instance->init()) { - return PX4_OK; + if (sens_imu_mode == 0) { + // ekf selector requires SENS_IMU_MODE = 0 + multi_mode = true; + + // IMUs (1 - 3 supported) + param_get(param_find("EKF2_MULTI_IMU"), &imu_instances); + + if (imu_instances < 1 || imu_instances > 3) { + const int32_t imu_instances_limited = math::constrain(imu_instances, 1, 3); + PX4_WARN("EKF2_MULTI_IMU limited %d -> %d", imu_instances, imu_instances_limited); + param_set_no_notification(param_find("EKF2_MULTI_IMU"), &imu_instances_limited); + imu_instances = imu_instances_limited; } - } else { - PX4_ERR("alloc failed"); + int32_t sens_mag_mode = 1; + param_get(param_find("SENS_MAG_MODE"), &sens_mag_mode); + + if (sens_mag_mode == 0) { + param_get(param_find("EKF2_MULTI_MAG"), &mag_instances); + + // Mags (1 - 4 supported) + if (mag_instances < 1 || mag_instances > 4) { + const int32_t mag_instances_limited = math::constrain(mag_instances, 1, 4); + PX4_WARN("EKF2_MULTI_MAG limited %d -> %d", mag_instances, mag_instances_limited); + param_set_no_notification(param_find("EKF2_MULTI_MAG"), &mag_instances_limited); + mag_instances = mag_instances_limited; + } + + } else { + mag_instances = 1; + } } - delete instance; - _object.store(nullptr); - _task_id = -1; + if (multi_mode) { + // Start EKF2Selector if it's not already running + if (_ekf2_selector.load() == nullptr) { + EKF2Selector *inst = new EKF2Selector(); + + if (inst) { + _ekf2_selector.store(inst); + inst->Start(); + + } else { + PX4_ERR("Failed to start EKF2 selector"); + } + } + + const hrt_abstime time_started = hrt_absolute_time(); + const int multi_instances = math::min(imu_instances * mag_instances, (int)EKF2_MAX_INSTANCES); + int multi_instances_allocated = 0; + + // allocate EKF2 instances until all found or arming + uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; + + while ((multi_instances_allocated < multi_instances) + && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) + && (hrt_elapsed_time(&time_started) < 30_s)) { + + vehicle_status_sub.update(); + + for (uint8_t mag = 0; mag < mag_instances; mag++) { + uORB::SubscriptionData vehicle_mag_sub{ORB_ID(vehicle_magnetometer), mag}; - return PX4_ERROR; + for (uint8_t imu = 0; imu < imu_instances; imu++) { + + uORB::SubscriptionData vehicle_imu_sub{ORB_ID(vehicle_imu), imu}; + vehicle_mag_sub.update(); + + // Mag & IMU data must be valid, first mag can be ignored initially + if ((vehicle_mag_sub.get().device_id != 0 || mag == 0) + && (vehicle_imu_sub.get().accel_device_id != 0) + && (vehicle_imu_sub.get().gyro_device_id != 0)) { + + const int instance = imu + mag * imu_instances; + + if (_objects[instance].load() == nullptr) { + EKF2 *ekf2_inst = new EKF2(instance, px4::ins_instance_to_wq(imu), imu, mag, false); + + if (ekf2_inst) { + PX4_INFO("starting instance %d, IMU:%d (%d), MAG:%d (%d)", instance, + imu, vehicle_imu_sub.get().accel_device_id, + mag, vehicle_mag_sub.get().device_id); + + _objects[instance].store(ekf2_inst); + ekf2_inst->ScheduleNow(); + success = true; + multi_instances_allocated++; + + } else { + PX4_ERR("instance %d alloc failed", instance); + px4_usleep(1000000); + break; + } + } + + } else { + px4_usleep(50000); // give the sensors extra time to start + continue; + } + } + } + + if (multi_instances_allocated < multi_instances) { + px4_usleep(100000); + } + } + + } + +#endif // !CONSTRAINED_FLASH + + else { + // otherwise launch regular + int instance = -1; + int imu = 0; + int mag = 0; + EKF2 *ekf2_inst = new EKF2(instance, px4::wq_configurations::INS0, imu, mag, replay_mode); + + if (ekf2_inst) { + _objects[0].store(ekf2_inst); + ekf2_inst->ScheduleNow(); + success = true; + } + } + + return success ? PX4_OK : PX4_ERROR; } int EKF2::print_usage(const char *reason) @@ -1439,5 +1561,104 @@ timestamps from the sensor topics. extern "C" __EXPORT int ekf2_main(int argc, char *argv[]) { - return EKF2::main(argc, argv); + if (argc <= 1 || strcmp(argv[1], "-h") == 0) { + return EKF2::print_usage(); + } + + if (strcmp(argv[1], "start") == 0) { + int ret = 0; + EKF2::lock_module(); + + ret = EKF2::task_spawn(argc - 1, argv + 1); + + if (ret < 0) { + PX4_ERR("start failed (%i)", ret); + } + + EKF2::unlock_module(); + return ret; + + } else if (strcmp(argv[1], "status") == 0) { + if (EKF2::trylock_module()) { +#if !defined(CONSTRAINED_FLASH) + if (_ekf2_selector.load()) { + _ekf2_selector.load()->PrintStatus(); + } +#endif // !CONSTRAINED_FLASH + + for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { + if (_objects[i].load()) { + PX4_INFO_RAW("\n"); + _objects[i].load()->print_status(); + } + } + + EKF2::unlock_module(); + + } else { + PX4_WARN("module locked, try again later"); + } + + return 0; + + } else if (strcmp(argv[1], "stop") == 0) { + EKF2::lock_module(); + + if (argc > 2) { + int instance = atoi(argv[2]); + + PX4_INFO("stopping %d", instance); + + if (instance > 0 && instance < EKF2_MAX_INSTANCES) { + EKF2 *inst = _objects[instance].load(); + + if (inst) { + inst->request_stop(); + px4_usleep(20000); // 20 ms + delete inst; + _objects[instance].store(nullptr); + } + } + + } else { + // otherwise stop everything + bool was_running = false; + +#if !defined(CONSTRAINED_FLASH) + if (_ekf2_selector.load()) { + PX4_INFO("stopping ekf2 selector"); + _ekf2_selector.load()->Stop(); + delete _ekf2_selector.load(); + _ekf2_selector.store(nullptr); + was_running = true; + } +#endif // !CONSTRAINED_FLASH + + for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { + EKF2 *inst = _objects[i].load(); + + if (inst) { + PX4_INFO("stopping ekf2 instance %d", i); + was_running = true; + inst->request_stop(); + px4_usleep(20000); // 20 ms + delete inst; + _objects[i].store(nullptr); + } + } + + if (!was_running) { + PX4_WARN("not running"); + } + } + + EKF2::unlock_module(); + return PX4_OK; + } + + EKF2::lock_module(); // Lock here, as the method could access _object. + int ret = EKF2::custom_command(argc - 1, argv + 1); + EKF2::unlock_module(); + + return ret; } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 7b4e9833e3..963b74f15c 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -39,8 +39,12 @@ */ #pragma once + +#include "EKF2Selector.hpp" + #include +#include #include #include #include @@ -61,6 +65,7 @@ #include #include #include +#include #include #include #include @@ -81,14 +86,16 @@ #include #include #include -#include #include "Utility/PreFlightChecker.hpp" -class EKF2 final : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +extern pthread_mutex_t ekf2_module_mutex; + +class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem { public: - explicit EKF2(bool replay_mode = false); + EKF2() = delete; + EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool replay_mode); ~EKF2() override; /** @see ModuleBase */ @@ -100,9 +107,15 @@ public: /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); - bool init(); + int print_status(); + + bool should_exit() const { return _task_should_exit.load(); } + + void request_stop() { _task_should_exit.store(true); } - int print_status() override; + static void lock_module() { pthread_mutex_lock(&ekf2_module_mutex); } + static bool trylock_module() { return (pthread_mutex_trylock(&ekf2_module_mutex) == 0); } + static void unlock_module() { pthread_mutex_unlock(&ekf2_module_mutex); } private: void Run() override; @@ -123,19 +136,23 @@ private: bool update_mag_decl(Param &mag_decl_param); void publish_attitude(const hrt_abstime ×tamp); + void publish_estimator_optical_flow_vel(const hrt_abstime ×tamp); void publish_odometry(const hrt_abstime ×tamp, const imuSample &imu, const vehicle_local_position_s &lpos); void publish_wind_estimate(const hrt_abstime ×tamp); void publish_yaw_estimator_status(const hrt_abstime ×tamp); - void publish_estimator_optical_flow_vel(const hrt_abstime ×tamp); /* * Calculate filtered WGS84 height from estimated AMSL height */ float filter_altitude_ellipsoid(float amsl_hgt); - inline float sq(float x) { return x * x; }; + static constexpr float sq(float x) { return x * x; }; + + const bool _replay_mode{false}; ///< true when we use replay data from a log + const bool _multi_mode; + const int _instance; - const bool _replay_mode; ///< true when we use replay data from a log + px4::atomic_bool _task_should_exit{false}; // time slip monitoring uint64_t _integrated_time_us = 0; ///< integral of gyro delta time from start (uSec) @@ -186,9 +203,8 @@ private: uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; - static constexpr int MAX_SENSOR_COUNT = 3; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; - int _imu_sub_index{-1}; + bool _callback_registered{false}; int _lockstep_component{-1}; @@ -200,23 +216,25 @@ private: vehicle_land_detected_s _vehicle_land_detected{}; vehicle_status_s _vehicle_status{}; - uORB::Publication _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; - uORB::Publication _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; - uORB::Publication _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; - uORB::Publication _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; - uORB::Publication _estimator_innovations_pub{ORB_ID(estimator_innovations)}; - uORB::Publication _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; - uORB::Publication _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; - uORB::Publication _estimator_states_pub{ORB_ID(estimator_states)}; - uORB::PublicationData _estimator_status_pub{ORB_ID(estimator_status)}; - uORB::Publication _att_pub{ORB_ID(vehicle_attitude)}; - uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; - uORB::Publication _yaw_est_pub{ORB_ID(yaw_estimator_status)}; - uORB::PublicationData _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; - uORB::PublicationData _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; - uORB::PublicationData _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)}; + uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; + uORB::PublicationMulti _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; + uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; + uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; + uORB::PublicationMulti _estimator_innovations_pub{ORB_ID(estimator_innovations)}; + uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; + uORB::PublicationMulti _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; + uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; + uORB::PublicationMultiData _estimator_status_pub{ORB_ID(estimator_status)}; + uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; uORB::PublicationMulti _wind_pub{ORB_ID(wind_estimate)}; + // publications with topic dependent on multi-mode + uORB::PublicationMulti _attitude_pub; + uORB::PublicationMultiData _local_position_pub; + uORB::PublicationMultiData _global_position_pub; + uORB::PublicationMulti _odometry_pub; + uORB::PublicationMultiData _visual_odometry_aligned_pub; + Ekf _ekf; parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance) @@ -368,8 +386,6 @@ private: (ParamExtFloat) _param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD) - (ParamInt) _param_ekf2_imu_id, - // sensor positions in body frame (ParamExtFloat) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m) (ParamExtFloat) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m) diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp new file mode 100644 index 0000000000..cb802dd519 --- /dev/null +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -0,0 +1,628 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "EKF2Selector.hpp" + +using namespace time_literals; +using matrix::Quatf; +using matrix::Vector2f; +using math::constrain; +using math::max; +using math::radians; + +EKF2Selector::EKF2Selector() : + ModuleParams(nullptr), + ScheduledWorkItem("ekf2_selector", px4::wq_configurations::nav_and_controllers) +{ +} + +EKF2Selector::~EKF2Selector() +{ + Stop(); + + px4_lockstep_unregister_component(_lockstep_component); +} + +bool EKF2Selector::Start() +{ + // default to first instance + _selected_instance = 0; + _instance[0].estimator_status_sub.registerCallback(); + _instance[0].estimator_attitude_sub.registerCallback(); + + return true; +} + +void EKF2Selector::Stop() +{ + for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { + _instance[i].estimator_attitude_sub.unregisterCallback(); + _instance[i].estimator_status_sub.unregisterCallback(); + } + + ScheduleClear(); +} + +void EKF2Selector::SelectInstance(uint8_t ekf_instance) +{ + if (ekf_instance != _selected_instance) { + + // update sensor_selection immediately + sensor_selection_s sensor_selection{}; + sensor_selection.accel_device_id = _instance[ekf_instance].estimator_status.accel_device_id; + sensor_selection.gyro_device_id = _instance[ekf_instance].estimator_status.gyro_device_id; + sensor_selection.timestamp = hrt_absolute_time(); + _sensor_selection_pub.publish(sensor_selection); + + if (_selected_instance != INVALID_INSTANCE) { + // switch callback registration + _instance[_selected_instance].estimator_attitude_sub.unregisterCallback(); + _instance[_selected_instance].estimator_status_sub.unregisterCallback(); + + if (!_instance[_selected_instance].healthy) { + PX4_WARN("primary EKF changed %d (unhealthy) -> %d", _selected_instance, ekf_instance); + } + } + + _instance[ekf_instance].estimator_attitude_sub.registerCallback(); + _instance[ekf_instance].estimator_status_sub.registerCallback(); + + _selected_instance = ekf_instance; + _instance_changed_count++; + _last_instance_change = sensor_selection.timestamp; + _instance[ekf_instance].time_last_selected = _last_instance_change; + + // reset all relative test ratios + for (auto &inst : _instance) { + inst.relative_test_ratio = 0; + } + + // publish new data immediately with resets + PublishVehicleAttitude(true); + PublishVehicleLocalPosition(true); + PublishVehicleGlobalPosition(true); + } +} + +bool EKF2Selector::UpdateErrorScores() +{ + bool updated = false; + + // first check imu inconsistencies + _gyro_fault_detected = false; + uint32_t faulty_gyro_id = 0; + _accel_fault_detected = false; + uint32_t faulty_accel_id = 0; + + if (_sensors_status_imu.updated()) { + sensors_status_imu_s sensors_status_imu; + + if (_sensors_status_imu.copy(&sensors_status_imu)) { + const float angle_rate_threshold = radians(_param_ekf2_sel_imu_angle_rate.get()); + const float angle_threshold = radians(_param_ekf2_sel_imu_angle.get()); + const float accel_threshold = _param_ekf2_sel_imu_accel.get(); + const float velocity_threshold = _param_ekf2_sel_imu_velocity.get(); + const float time_step_s = constrain((sensors_status_imu.timestamp - _last_update_us) * 1e-6f, 0.f, 0.02f); + _last_update_us = sensors_status_imu.timestamp; + + uint8_t n_gyros = 0; + uint8_t n_accels = 0; + uint8_t n_gyro_exceedances = 0; + uint8_t n_accel_exceedances = 0; + float largest_accumulated_gyro_error = 0.0f; + float largest_accumulated_accel_error = 0.0f; + uint8_t largest_gyro_error_index = 0; + uint8_t largest_accel_error_index = 0; + + for (unsigned i = 0; i < IMU_STATUS_SIZE; i++) { + // check for gyros with excessive difference to mean using accumulated error + if (sensors_status_imu.gyro_device_ids[i] != 0) { + n_gyros++; + _accumulated_gyro_error[i] += (sensors_status_imu.gyro_inconsistency_rad_s[i] - angle_rate_threshold) * time_step_s; + _accumulated_gyro_error[i] = fmaxf(_accumulated_gyro_error[i], 0.f); + + if (_accumulated_gyro_error[i] > angle_threshold) { + n_gyro_exceedances++; + } + + if (_accumulated_gyro_error[i] > largest_accumulated_gyro_error) { + largest_accumulated_gyro_error = _accumulated_gyro_error[i]; + largest_gyro_error_index = i; + } + + } else { + // no sensor + _accumulated_gyro_error[i] = 0.f; + } + + // check for accelerometers with excessive difference to mean using accumulated error + if (sensors_status_imu.accel_device_ids[i] != 0) { + n_accels++; + _accumulated_accel_error[i] += (sensors_status_imu.accel_inconsistency_m_s_s[i] - accel_threshold) * time_step_s; + _accumulated_accel_error[i] = fmaxf(_accumulated_accel_error[i], 0.f); + + if (_accumulated_accel_error[i] > velocity_threshold) { + n_accel_exceedances++; + } + + if (_accumulated_accel_error[i] > largest_accumulated_accel_error) { + largest_accumulated_accel_error = _accumulated_accel_error[i]; + largest_accel_error_index = i; + } + + } else { + // no sensor + _accumulated_accel_error[i] = 0.f; + } + } + + if (n_gyro_exceedances > 0) { + if (n_gyros >= 3) { + // If there are 3 or more sensors, the one with the largest accumulated error is faulty + _gyro_fault_detected = true; + faulty_gyro_id = _instance[largest_gyro_error_index].estimator_status.gyro_device_id; + + } else if (n_gyros == 2) { + // A fault is present, but the faulty sensor identity cannot be determined + _gyro_fault_detected = true; + } + } + + if (n_accel_exceedances > 0) { + if (n_accels >= 3) { + // If there are 3 or more sensors, the one with the largest accumulated error is faulty + _accel_fault_detected = true; + faulty_accel_id = _instance[largest_accel_error_index].estimator_status.accel_device_id; + + } else if (n_accels == 2) { + // A fault is present, but the faulty sensor identity cannot be determined + _accel_fault_detected = true; + } + } + } + } + + + bool primary_updated = false; + + // calculate individual error scores + for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) { + const bool prev_healthy = _instance[i].healthy; + + if (_instance[i].estimator_status_sub.update(&_instance[i].estimator_status)) { + + if ((i + 1) > _available_instances) { + _available_instances = i + 1; + updated = true; + } + + if (i == _selected_instance) { + primary_updated = true; + } + + const estimator_status_s &status = _instance[i].estimator_status; + + const bool tilt_align = status.control_mode_flags & (1 << estimator_status_s::CS_TILT_ALIGN); + const bool yaw_align = status.control_mode_flags & (1 << estimator_status_s::CS_YAW_ALIGN); + + _instance[i].combined_test_ratio = 0.0f; + + if (tilt_align && yaw_align) { + _instance[i].combined_test_ratio = fmaxf(_instance[i].combined_test_ratio, + 0.5f * (status.vel_test_ratio + status.pos_test_ratio)); + _instance[i].combined_test_ratio = fmaxf(_instance[i].combined_test_ratio, status.hgt_test_ratio); + } + + _instance[i].healthy = tilt_align && yaw_align && (status.filter_fault_flags == 0); + + } else if (hrt_elapsed_time(&_instance[i].estimator_status.timestamp) > 20_ms) { + _instance[i].healthy = false; + } + + // if the gyro used by the EKF is faulty, declare the EKF unhealthy without delay + if (_gyro_fault_detected && _instance[i].estimator_status.gyro_device_id == faulty_gyro_id) { + _instance[i].healthy = false; + } + + // if the accelerometer used by the EKF is faulty, declare the EKF unhealthy without delay + if (_accel_fault_detected && _instance[i].estimator_status.accel_device_id == faulty_accel_id) { + _instance[i].healthy = false; + } + + if (prev_healthy != _instance[i].healthy) { + updated = true; + } + } + + // update relative test ratios if primary has updated + if (primary_updated) { + for (uint8_t i = 0; i < _available_instances; i++) { + if (i != _selected_instance) { + + const float error_delta = _instance[i].combined_test_ratio - _instance[_selected_instance].combined_test_ratio; + + // reduce error only if its better than the primary instance by at least EKF2_SEL_ERR_RED to prevent unnecessary selection changes + const float threshold = _gyro_fault_detected ? fmaxf(_param_ekf2_sel_err_red.get(), 0.05f) : 0.0f; + + if (error_delta > 0 || error_delta < -threshold) { + _instance[i].relative_test_ratio += error_delta; + _instance[i].relative_test_ratio = constrain(_instance[i].relative_test_ratio, -_rel_err_score_lim, _rel_err_score_lim); + } + } + } + } + + return (primary_updated || updated); +} + +void EKF2Selector::PublishVehicleAttitude(bool reset) +{ + vehicle_attitude_s attitude; + + if (_instance[_selected_instance].estimator_attitude_sub.copy(&attitude)) { + if (reset) { + // on reset compute deltas from last published data + ++_quat_reset_counter; + + _delta_q_reset = (Quatf(attitude.q) * Quatf(_attitude_last.q).inversed()).normalized(); + + // ensure monotonically increasing timestamp_sample through reset + attitude.timestamp_sample = max(attitude.timestamp_sample, _attitude_last.timestamp_sample); + + } else { + // otherwise propogate deltas from estimator data while maintaining the overall reset counts + if (attitude.quat_reset_counter > _attitude_last.quat_reset_counter) { + ++_quat_reset_counter; + _delta_q_reset = Quatf{attitude.delta_q_reset}; + } + } + + // save last primary estimator_attitude + _attitude_last = attitude; + + // republish with total reset count and current timestamp + attitude.quat_reset_counter = _quat_reset_counter; + _delta_q_reset.copyTo(attitude.delta_q_reset); + + attitude.timestamp = hrt_absolute_time(); + _vehicle_attitude_pub.publish(attitude); + } +} + +void EKF2Selector::PublishVehicleLocalPosition(bool reset) +{ + // vehicle_local_position + vehicle_local_position_s local_position; + + if (_instance[_selected_instance].estimator_local_position_sub.copy(&local_position)) { + if (reset) { + // on reset compute deltas from last published data + ++_xy_reset_counter; + ++_z_reset_counter; + ++_vxy_reset_counter; + ++_vz_reset_counter; + ++_heading_reset_counter; + + _delta_xy_reset = Vector2f{local_position.x, local_position.y} - Vector2f{_local_position_last.x, _local_position_last.y}; + _delta_z_reset = local_position.z - _local_position_last.z; + _delta_vxy_reset = Vector2f{local_position.vx, local_position.vy} - Vector2f{_local_position_last.vx, _local_position_last.vy}; + _delta_vz_reset = local_position.vz - _local_position_last.vz; + _delta_heading_reset = matrix::wrap_2pi(local_position.heading - _local_position_last.heading); + + // ensure monotonically increasing timestamp_sample through reset + local_position.timestamp_sample = max(local_position.timestamp_sample, _local_position_last.timestamp_sample); + + } else { + // otherwise propogate deltas from estimator data while maintaining the overall reset counts + + // XY reset + if (local_position.xy_reset_counter > _local_position_last.xy_reset_counter) { + ++_xy_reset_counter; + _delta_xy_reset = Vector2f{local_position.delta_xy}; + } + + // Z reset + if (local_position.z_reset_counter > _local_position_last.z_reset_counter) { + ++_z_reset_counter; + _delta_z_reset = local_position.delta_z; + } + + // VXY reset + if (local_position.vxy_reset_counter > _local_position_last.vxy_reset_counter) { + ++_vxy_reset_counter; + _delta_vxy_reset = Vector2f{local_position.delta_vxy}; + } + + // VZ reset + if (local_position.vz_reset_counter > _local_position_last.vz_reset_counter) { + ++_vz_reset_counter; + _delta_z_reset = local_position.delta_vz; + } + + // heading reset + if (local_position.heading_reset_counter > _local_position_last.heading_reset_counter) { + ++_heading_reset_counter; + _delta_heading_reset = local_position.delta_heading; + } + } + + // save last primary estimator_local_position + _local_position_last = local_position; + + // publish estimator's local position for system (vehicle_local_position) unless it's stale + if (local_position.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) { + // republish with total reset count and current timestamp + local_position.xy_reset_counter = _xy_reset_counter; + local_position.z_reset_counter = _z_reset_counter; + local_position.vxy_reset_counter = _vxy_reset_counter; + local_position.vz_reset_counter = _vz_reset_counter; + local_position.heading_reset_counter = _heading_reset_counter; + + _delta_xy_reset.copyTo(local_position.delta_xy); + local_position.delta_z = _delta_z_reset; + _delta_vxy_reset.copyTo(local_position.delta_vxy); + local_position.delta_vz = _delta_vz_reset; + local_position.delta_heading = _delta_heading_reset; + + local_position.timestamp = hrt_absolute_time(); + _vehicle_local_position_pub.publish(local_position); + } + } +} + +void EKF2Selector::PublishVehicleGlobalPosition(bool reset) +{ + vehicle_global_position_s global_position; + + if (_instance[_selected_instance].estimator_global_position_sub.copy(&global_position)) { + if (reset) { + // on reset compute deltas from last published data + ++_lat_lon_reset_counter; + ++_alt_reset_counter; + + _delta_lat_reset = global_position.lat - _global_position_last.lat; + _delta_lon_reset = global_position.lon - _global_position_last.lon; + _delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt; + + // ensure monotonically increasing timestamp_sample through reset + global_position.timestamp_sample = max(global_position.timestamp_sample, _global_position_last.timestamp_sample); + + } else { + // otherwise propogate deltas from estimator data while maintaining the overall reset counts + + // lat/lon reset + if (global_position.lat_lon_reset_counter > _global_position_last.lat_lon_reset_counter) { + ++_lat_lon_reset_counter; + + // TODO: delta latitude/longitude + //_delta_lat_reset = global_position.delta_lat; + //_delta_lon_reset = global_position.delta_lon; + } + + // alt reset + if (global_position.alt_reset_counter > _global_position_last.alt_reset_counter) { + ++_alt_reset_counter; + _delta_alt_reset = global_position.delta_alt; + } + } + + // save last primary estimator_global_position + _global_position_last = global_position; + + // publish estimator's global position for system (vehicle_global_position) unless it's stale + if (global_position.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) { + // republish with total reset count and current timestamp + global_position.lat_lon_reset_counter = _lat_lon_reset_counter; + global_position.alt_reset_counter = _alt_reset_counter; + global_position.delta_alt = _delta_alt_reset; + + global_position.timestamp = hrt_absolute_time(); + _vehicle_global_position_pub.publish(global_position); + } + } +} + +void EKF2Selector::Run() +{ + // re-schedule as backup timeout + ScheduleDelayed(10_ms); + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + } + + // update combined test ratio for all estimators + const bool updated = UpdateErrorScores(); + + if (updated) { + bool lower_error_available = false; + float alternative_error = 0.f; // looking for instances that have error lower than the current primary + uint8_t best_ekf_instance = _selected_instance; + + // loop through all available instances to find if an alternative is available + for (int i = 0; i < _available_instances; i++) { + // Use an alternative instance if - + // (healthy and has updated recently) + // AND + // (has relative error less than selected instance and has not been the selected instance for at least 10 seconds + // OR + // selected instance has stopped updating + if (_instance[i].healthy && (i != _selected_instance)) { + const float relative_error = _instance[i].relative_test_ratio; + + if (relative_error < alternative_error) { + + alternative_error = relative_error; + best_ekf_instance = i; + + // relative error less than selected instance and has not been the selected instance for at least 10 seconds + if ((relative_error <= -_rel_err_thresh) && hrt_elapsed_time(&_instance[i].time_last_selected) > 10_s) { + lower_error_available = true; + } + } + } + } + + if ((_selected_instance == INVALID_INSTANCE) + || (!_instance[_selected_instance].healthy && (best_ekf_instance == _selected_instance))) { + + // force initial selection + uint8_t best_instance = _selected_instance; + float best_test_ratio = FLT_MAX; + + for (int i = 0; i < _available_instances; i++) { + if (_instance[i].healthy) { + const float test_ratio = _instance[i].combined_test_ratio; + + if ((test_ratio > 0) && (test_ratio < best_test_ratio)) { + best_instance = i; + best_test_ratio = test_ratio; + } + } + } + + SelectInstance(best_instance); + + } else if (best_ekf_instance != _selected_instance) { + // if this instance has a significantly lower relative error to the active primary, we consider it as a + // better instance and would like to switch to it even if the current primary is healthy + // switch immediately if the current selected is no longer healthy + if ((lower_error_available && hrt_elapsed_time(&_last_instance_change) > 10_s) + || !_instance[_selected_instance].healthy) { + + SelectInstance(best_ekf_instance); + } + } + + // publish selector status + estimator_selector_status_s selector_status{}; + selector_status.primary_instance = _selected_instance; + selector_status.instances_available = _available_instances; + selector_status.instance_changed_count = _instance_changed_count; + selector_status.last_instance_change = _last_instance_change; + selector_status.accel_device_id = _instance[_selected_instance].estimator_status.accel_device_id; + selector_status.baro_device_id = _instance[_selected_instance].estimator_status.baro_device_id; + selector_status.gyro_device_id = _instance[_selected_instance].estimator_status.gyro_device_id; + selector_status.mag_device_id = _instance[_selected_instance].estimator_status.mag_device_id; + selector_status.gyro_fault_detected = _gyro_fault_detected; + selector_status.accel_fault_detected = _accel_fault_detected; + + for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { + selector_status.combined_test_ratio[i] = _instance[i].combined_test_ratio; + selector_status.relative_test_ratio[i] = _instance[i].relative_test_ratio; + selector_status.healthy[i] = _instance[i].healthy; + } + + for (int i = 0; i < IMU_STATUS_SIZE; i++) { + selector_status.accumulated_gyro_error[i] = _accumulated_gyro_error[i]; + selector_status.accumulated_accel_error[i] = _accumulated_accel_error[i]; + } + + selector_status.timestamp = hrt_absolute_time(); + _estimator_selector_status_pub.publish(selector_status); + } + + // republish selected estimator data for system + if (_selected_instance != INVALID_INSTANCE) { + // selected estimator_attitude -> vehicle_attitude + if (_instance[_selected_instance].estimator_attitude_sub.updated()) { + PublishVehicleAttitude(); + } + + // selected estimator_local_position -> vehicle_local_position + if (_instance[_selected_instance].estimator_local_position_sub.updated()) { + PublishVehicleLocalPosition(); + } + + // selected estimator_global_position -> vehicle_global_position + if (_instance[_selected_instance].estimator_global_position_sub.updated()) { + PublishVehicleGlobalPosition(); + } + + // selected estimator_odometry -> vehicle_odometry + if (_instance[_selected_instance].estimator_odometry_sub.updated()) { + vehicle_odometry_s vehicle_odometry; + + if (_instance[_selected_instance].estimator_odometry_sub.update(&vehicle_odometry)) { + if (vehicle_odometry.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) { + vehicle_odometry.timestamp = hrt_absolute_time(); + _vehicle_odometry_pub.publish(vehicle_odometry); + } + } + } + + // selected estimator_visual_odometry_aligned -> vehicle_visual_odometry_aligned + if (_instance[_selected_instance].estimator_visual_odometry_aligned_sub.updated()) { + vehicle_odometry_s vehicle_visual_odometry_aligned; + + if (_instance[_selected_instance].estimator_visual_odometry_aligned_sub.update(&vehicle_visual_odometry_aligned)) { + if (vehicle_visual_odometry_aligned.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) { + vehicle_visual_odometry_aligned.timestamp = hrt_absolute_time(); + _vehicle_visual_odometry_aligned_pub.publish(vehicle_visual_odometry_aligned); + } + } + } + } + + if (_lockstep_component == -1) { + _lockstep_component = px4_lockstep_register_component(); + } + + px4_lockstep_progress(_lockstep_component); +} + +void EKF2Selector::PrintStatus() +{ + PX4_INFO("available instances: %d", _available_instances); + + if (_selected_instance == INVALID_INSTANCE) { + PX4_WARN("selected instance: None"); + } + + for (int i = 0; i < _available_instances; i++) { + const EstimatorInstance &inst = _instance[i]; + + PX4_INFO("%d: ACC: %d, GYRO: %d, MAG: %d, %s, test ratio: %.7f (%.5f) %s", + inst.instance, inst.estimator_status.accel_device_id, inst.estimator_status.gyro_device_id, + inst.estimator_status.mag_device_id, + inst.healthy ? "healthy" : "unhealthy", + (double)inst.combined_test_ratio, (double)inst.relative_test_ratio, + (_selected_instance == i) ? "*" : ""); + } +} diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp new file mode 100644 index 0000000000..07fca36889 --- /dev/null +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -0,0 +1,199 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static constexpr uint8_t EKF2_MAX_INSTANCES{9}; +static_assert(EKF2_MAX_INSTANCES <= ORB_MULTI_MAX_INSTANCES, "EKF2_MAX_INSTANCES must be <= ORB_MULTI_MAX_INSTANCES"); + +class EKF2Selector : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + EKF2Selector(); + ~EKF2Selector() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + static constexpr uint8_t INVALID_INSTANCE = UINT8_MAX; + void Run() override; + void PublishVehicleAttitude(bool reset = false); + void PublishVehicleLocalPosition(bool reset = false); + void PublishVehicleGlobalPosition(bool reset = false); + void SelectInstance(uint8_t instance); + + // Update the error scores for all available instances + bool UpdateErrorScores(); + + // Subscriptions (per estimator instance) + struct EstimatorInstance { + EstimatorInstance(EKF2Selector *selector, uint8_t i) : + estimator_attitude_sub{selector, ORB_ID(estimator_attitude), i}, + estimator_status_sub{selector, ORB_ID(estimator_status), i}, + estimator_local_position_sub{ORB_ID(estimator_local_position), i}, + estimator_global_position_sub{ORB_ID(estimator_global_position), i}, + estimator_odometry_sub{ORB_ID(estimator_odometry), i}, + estimator_visual_odometry_aligned_sub{ORB_ID(estimator_visual_odometry_aligned), i}, + instance(i) + {} + + uORB::SubscriptionCallbackWorkItem estimator_attitude_sub; + uORB::SubscriptionCallbackWorkItem estimator_status_sub; + + uORB::Subscription estimator_local_position_sub; + uORB::Subscription estimator_global_position_sub; + uORB::Subscription estimator_odometry_sub; + uORB::Subscription estimator_visual_odometry_aligned_sub; + + estimator_status_s estimator_status{}; + + hrt_abstime time_last_selected{0}; + + float combined_test_ratio{0.f}; + float relative_test_ratio{0.f}; + + bool healthy{false}; + + const uint8_t instance; + }; + + static constexpr float _rel_err_score_lim{1.0f}; // +- limit applied to the relative error score + static constexpr float _rel_err_thresh{0.5f}; // the relative score difference needs to be greater than this to switch from an otherwise healthy instance + + EstimatorInstance _instance[EKF2_MAX_INSTANCES] { + {this, 0}, + {this, 1}, + {this, 2}, + {this, 3}, + {this, 4}, + {this, 5}, + {this, 6}, + {this, 7}, + {this, 8}, + }; + + static constexpr uint8_t IMU_STATUS_SIZE = (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof( + sensors_status_imu_s::gyro_inconsistency_rad_s[0])); + static_assert(IMU_STATUS_SIZE == sizeof(estimator_selector_status_s::accumulated_gyro_error) / sizeof( + estimator_selector_status_s::accumulated_gyro_error[0]), + "increase estimator_selector_status_s::accumulated_gyro_error size"); + static_assert(IMU_STATUS_SIZE == sizeof(estimator_selector_status_s::accumulated_accel_error) / sizeof( + estimator_selector_status_s::accumulated_accel_error[0]), + "increase estimator_selector_status_s::accumulated_accel_error size"); + static_assert(EKF2_MAX_INSTANCES == sizeof(estimator_selector_status_s::combined_test_ratio) / sizeof( + estimator_selector_status_s::combined_test_ratio[0]), + "increase estimator_selector_status_s::combined_test_ratio size"); + + float _accumulated_gyro_error[IMU_STATUS_SIZE] {}; + float _accumulated_accel_error[IMU_STATUS_SIZE] {}; + hrt_abstime _last_update_us{0}; + bool _gyro_fault_detected{false}; + bool _accel_fault_detected{false}; + + uint8_t _available_instances{0}; + uint8_t _selected_instance{INVALID_INSTANCE}; + + uint32_t _instance_changed_count{0}; + hrt_abstime _last_instance_change{0}; + + // vehicle_attitude: reset counters + vehicle_attitude_s _attitude_last{}; + matrix::Quatf _delta_q_reset{}; + uint8_t _quat_reset_counter{0}; + + // vehicle_local_position: reset counters + vehicle_local_position_s _local_position_last{}; + matrix::Vector2f _delta_xy_reset{}; + float _delta_z_reset{0.f}; + matrix::Vector2f _delta_vxy_reset{}; + float _delta_vz_reset{0.f}; + float _delta_heading_reset{0}; + uint8_t _xy_reset_counter{0}; + uint8_t _z_reset_counter{0}; + uint8_t _vxy_reset_counter{0}; + uint8_t _vz_reset_counter{0}; + uint8_t _heading_reset_counter{0}; + + // vehicle_global_position: reset counters + vehicle_global_position_s _global_position_last{}; + double _delta_lat_reset{0}; + double _delta_lon_reset{0}; + float _delta_alt_reset{0.f}; + uint8_t _lat_lon_reset_counter{0}; + uint8_t _alt_reset_counter{0}; + + int _lockstep_component{-1}; + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)}; + + // Publications + uORB::Publication _estimator_selector_status_pub{ORB_ID(estimator_selector_status)}; + uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; + uORB::Publication _vehicle_attitude_pub{ORB_ID(vehicle_attitude)}; + uORB::Publication _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; + uORB::Publication _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; + uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; + uORB::Publication _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ekf2_sel_err_red, + (ParamFloat) _param_ekf2_sel_imu_angle_rate, + (ParamFloat) _param_ekf2_sel_imu_angle, + (ParamFloat) _param_ekf2_sel_imu_accel, + (ParamFloat) _param_ekf2_sel_imu_velocity + ) +}; diff --git a/src/modules/ekf2/ekf2_multi_params.c b/src/modules/ekf2/ekf2_multi_params.c new file mode 100644 index 0000000000..7e2db48baa --- /dev/null +++ b/src/modules/ekf2/ekf2_multi_params.c @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Multi-EKF IMUs + * + * Maximum number of IMUs to use for Multi-EKF. Set 0 to disable. + * Requires SENS_IMU_MODE 0. + * + * @group EKF2 + * @reboot_required true + * @min 0 + * @max 3 + */ +PARAM_DEFINE_INT32(EKF2_MULTI_IMU, 0); + +/** + * Multi-EKF Magnetometers. + * + * Maximum number of magnetometers to use for Multi-EKF. Set 0 to disable. + * Requires SENS_MAG_MODE 0. + * + * @group EKF2 + * @reboot_required true + * @min 0 + * @max 4 + */ +PARAM_DEFINE_INT32(EKF2_MULTI_MAG, 0); diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index b1a4c36344..98e2c8f1e3 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -820,19 +820,6 @@ PARAM_DEFINE_FLOAT(EKF2_TERR_NOISE, 5.0f); */ PARAM_DEFINE_FLOAT(EKF2_TERR_GRAD, 0.5f); -/** - * Device id of IMU - * - * Set to 0 to use system selected (sensor_combined) IMU, - * otherwise set to the device id of the desired IMU (vehicle_imu). - * - * @group EKF2 - * @value 0 System Primary - * @category Developer - * - */ -PARAM_DEFINE_INT32(EKF2_IMU_ID, 0); - /** * X position of IMU in body frame (forward axis with origin relative to vehicle centre of gravity) * diff --git a/src/modules/ekf2/ekf2_selector_params.c b/src/modules/ekf2/ekf2_selector_params.c new file mode 100644 index 0000000000..43a630fe8f --- /dev/null +++ b/src/modules/ekf2/ekf2_selector_params.c @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Selector error reduce threshold + * + * EKF2 instances have to be better than the selected by at least this amount before their relative score can be reduced. + * + * @group EKF2 + */ +PARAM_DEFINE_FLOAT(EKF2_SEL_ERR_RED, 0.2f); + +/** + * Selector angular rate threshold + * + * EKF2 selector angular rate error threshold for comparing gyros. Angular rate vector differences larger than this will result in accumulated angular error. + * + * @group EKF2 + * @unit deg/s + */ +PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_RAT, 7.0f); + +/** + * Selector angular threshold. + * + * EKF2 selector maximum accumulated angular error threshold for comparing gyros. Accumulated angular error larger than this will result in the sensor being declared faulty. + * + * @group EKF2 + * @unit deg + */ +PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_ANG, 15.0f); + +/** + * Selector acceleration threshold + * + * EKF2 selector acceleration error threshold for comparing accelerometers. Acceleration vector differences larger than this will result in accumulated velocity error. + * + * @group EKF2 + * @unit m/s^2 + */ +PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_ACC, 1.0f); + +/** + * Selector angular threshold. + * + * EKF2 selector maximum accumulated velocity threshold for comparing accelerometers. Accumulated velocity error larger than this will result in the sensor being declared faulty. + * + * @group EKF2 + * @unit m/s + */ +PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_VEL, 2.0f); diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 8e344b046b..6498b97e1c 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -42,7 +42,7 @@ using math::radians; FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::attitude_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), _actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)), _attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 1e594fcaea..41b8897ae0 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -55,15 +55,7 @@ void LoggedTopics::add_default_topics() add_topic("cellular_status", 200); add_topic("commander_state"); add_topic("cpuload"); - add_topic("ekf_gps_drift"); add_topic("esc_status", 250); - add_topic("estimator_innovation_test_ratios", 200); - add_topic("estimator_innovation_variances", 200); - add_topic("estimator_innovations", 200); - add_topic("estimator_optical_flow_vel", 200); - add_topic("estimator_sensor_bias", 1000); - add_topic("estimator_states", 1000); - add_topic("estimator_status", 200); add_topic("generator_status"); add_topic("home_position"); add_topic("hover_thrust_estimate", 100); @@ -109,24 +101,39 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_status"); add_topic("vehicle_status_flags"); add_topic("vtol_vehicle_status", 200); - add_topic("yaw_estimator_status", 200); // multi topics add_topic_multi("actuator_outputs", 100, 2); add_topic_multi("logger_status", 0, 2); add_topic_multi("multirotor_motor_limits", 1000, 2); add_topic_multi("rate_ctrl_status", 200, 2); - add_topic_multi("telemetry_status", 1000); - add_topic_multi("wind_estimate", 1000); + add_topic_multi("telemetry_status", 1000, 4); + + // EKF multi topics (currently max 9 estimators) + static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 4; + add_topic("estimator_selector_status", 200); + add_topic_multi("ekf_gps_drift", 1000, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_sensor_bias", 1000, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_status", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("wind_estimate", 1000); // published by both ekf2 and airspeed_selector // log all raw sensors at minimal rate (at least 1 Hz) - add_topic_multi("battery_status", 300, 4); - add_topic_multi("differential_pressure", 1000, 3); + add_topic_multi("battery_status", 300, 2); + add_topic_multi("differential_pressure", 1000, 2); add_topic_multi("distance_sensor", 1000); - add_topic_multi("optical_flow", 1000, 2); + add_topic_multi("optical_flow", 1000, 1); add_topic_multi("sensor_accel", 1000, 4); add_topic_multi("sensor_baro", 1000, 4); - add_topic_multi("sensor_gps", 1000, 3); + add_topic_multi("sensor_gps", 1000, 2); add_topic_multi("sensor_gyro", 1000, 4); add_topic_multi("sensor_mag", 1000, 4); add_topic_multi("vehicle_imu", 500, 4); @@ -374,7 +381,6 @@ bool LoggedTopics::add_topic_multi(const char *name, uint16_t interval_ms, uint8 return true; } - bool LoggedTopics::initialize_logged_topics(SDLogProfileMask profile) { int ntopics = add_topics_from_file(PX4_STORAGEDIR "/etc/logging/logger_topics.txt"); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6cf6ecbbfc..6913ec794a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -67,6 +67,7 @@ #include #include #include +#include #include #include #include @@ -969,8 +970,9 @@ public: private: uORB::SubscriptionMultiArray _vehicle_imu_subs{ORB_ID::vehicle_imu}; + uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; - uORB::Subscription _bias_sub{ORB_ID(estimator_sensor_bias)}; uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)}; @@ -1010,13 +1012,60 @@ protected: vehicle_magnetometer_s magnetometer{}; if (_magnetometer_sub.update(&magnetometer)) { - /* mark third group dimensions as changed */ + // mark third group dimensions as changed fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); } else { _magnetometer_sub.copy(&magnetometer); } + // find corresponding estimated sensor bias + if (_estimator_selector_status_sub.updated()) { + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { + _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance); + } + } + + Vector3f accel_bias{0.f, 0.f, 0.f}; + Vector3f gyro_bias{0.f, 0.f, 0.f}; + Vector3f mag_bias{0.f, 0.f, 0.f}; + + { + estimator_sensor_bias_s bias; + + if (_estimator_sensor_bias_sub.copy(&bias)) { + if ((bias.accel_device_id != 0) && (bias.accel_device_id == imu.accel_device_id)) { + accel_bias = Vector3f{bias.accel_bias}; + } + + if ((bias.gyro_device_id != 0) && (bias.gyro_device_id == imu.gyro_device_id)) { + gyro_bias = Vector3f{bias.gyro_bias}; + } + + if ((bias.mag_device_id != 0) && (bias.mag_device_id == magnetometer.device_id)) { + mag_bias = Vector3f{bias.mag_bias}; + + } else { + // find primary mag + uORB::SubscriptionMultiArray mag_subs{ORB_ID::vehicle_magnetometer}; + + for (int i = 0; i < mag_subs.size(); i++) { + if (mag_subs[i].advertised() && mag_subs[i].copy(&magnetometer)) { + if (magnetometer.device_id == bias.mag_device_id) { + _magnetometer_sub.ChangeInstance(i); + break; + } + } + + } + } + } + } + + const Vector3f mag = Vector3f{magnetometer.magnetometer_ga} - mag_bias; + vehicle_air_data_s air_data{}; if (_air_data_sub.update(&air_data)) { @@ -1037,14 +1086,11 @@ protected: _differential_pressure_sub.copy(&differential_pressure); } - estimator_sensor_bias_s bias{}; - _bias_sub.copy(&bias); - const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; - Vector3f accel = (Vector3f{imu.delta_velocity} * accel_dt_inv) - Vector3f{bias.accel_bias}; + const Vector3f accel = (Vector3f{imu.delta_velocity} * accel_dt_inv) - accel_bias; const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt; - Vector3f gyro = (Vector3f{imu.delta_angle} * gyro_dt_inv) - Vector3f{bias.gyro_bias}; + const Vector3f gyro = (Vector3f{imu.delta_angle} * gyro_dt_inv) - gyro_bias; mavlink_highres_imu_t msg{}; @@ -1055,9 +1101,9 @@ protected: msg.xgyro = gyro(0); msg.ygyro = gyro(1); msg.zgyro = gyro(2); - msg.xmag = magnetometer.magnetometer_ga[0] - bias.mag_bias[0]; - msg.ymag = magnetometer.magnetometer_ga[1] - bias.mag_bias[1]; - msg.zmag = magnetometer.magnetometer_ga[2] - bias.mag_bias[2]; + msg.xmag = mag(0); + msg.ymag = mag(1); + msg.zmag = mag(2); msg.abs_pressure = air_data.baro_pressure_pa; msg.diff_pressure = differential_pressure.differential_pressure_raw_pa; msg.pressure_alt = air_data.baro_alt_meter; @@ -2916,11 +2962,12 @@ public: unsigned get_size() override { - return _est_sub.advertised() ? MAVLINK_MSG_ID_ESTIMATOR_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _estimator_status_sub.advertised() ? MAVLINK_MSG_ID_ESTIMATOR_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: - uORB::Subscription _est_sub{ORB_ID(estimator_status)}; + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; + uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; /* do not allow top copying this class */ MavlinkStreamEstimatorStatus(MavlinkStreamEstimatorStatus &) = delete; @@ -2932,9 +2979,20 @@ protected: bool send() override { + // use primary estimator_status + if (_estimator_selector_status_sub.updated()) { + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { + if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) { + _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance); + } + } + } + estimator_status_s est; - if (_est_sub.update(&est)) { + if (_estimator_status_sub.update(&est)) { mavlink_estimator_status_t est_msg{}; est_msg.time_usec = est.timestamp; est_msg.vel_ratio = est.vel_test_ratio; diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 626fa2c9be..cf7b6f7915 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -285,6 +286,17 @@ private: bool write_estimator_status(mavlink_high_latency2_t *msg) { + // use primary estimator_status + if (_estimator_selector_status_sub.updated()) { + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { + if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) { + _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance); + } + } + } + estimator_status_s estimator_status; if (_estimator_status_sub.update(&estimator_status)) { @@ -618,6 +630,7 @@ private: uORB::Subscription _actuator_1_sub{ORB_ID(actuator_controls_1)}; uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; uORB::Subscription _attitude_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; uORB::Subscription _pos_ctrl_status_sub{ORB_ID(position_controller_status)}; uORB::Subscription _geofence_sub{ORB_ID(geofence_result)}; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 6392da26a7..08ab125356 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -53,7 +53,7 @@ using namespace matrix; MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::attitude_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), _vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _vtol(vtol) diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 09b128791d..7efa2b471d 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -69,7 +69,6 @@ #include #include #include -#include #include using matrix::Dcmf; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 318e3a5e37..6282ddc5c6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -536,7 +536,8 @@ void Sensors::InitializeVehicleIMU() if (accel.device_id > 0 && gyro.device_id > 0) { // if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleIMU in the same WQ // otherwise each VehicleIMU runs in a corresponding INSx WQ - const px4::wq_config_t &wq_config = px4::wq_configurations::nav_and_controllers; + const bool multi_mode = (_param_sens_imu_mode.get() == 0); + const px4::wq_config_t &wq_config = multi_mode ? px4::ins_instance_to_wq(i) : px4::wq_configurations::INS0; VehicleIMU *imu = new VehicleIMU(i, i, i, wq_config); diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 6d57cd9ca4..8500083585 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -65,7 +65,8 @@ bool VehicleAcceleration::Start() } if (!SensorSelectionUpdate(true)) { - ScheduleDelayed(10_ms); + _selected_sensor_sub_index = 0; + _sensor_sub.registerCallback(); } return true; @@ -137,6 +138,15 @@ void VehicleAcceleration::CheckFilters() void VehicleAcceleration::SensorBiasUpdate(bool force) { + // find corresponding estimated sensor bias + if (_estimator_selector_status_sub.updated()) { + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { + _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance); + } + } + if (_estimator_sensor_bias_sub.updated() || force) { estimator_sensor_bias_s bias; diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index 755ebc7ab1..9bb4e39aee 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -56,7 +57,6 @@ namespace sensors class VehicleAcceleration : public ModuleParams, public px4::ScheduledWorkItem { public: - VehicleAcceleration(); ~VehicleAcceleration() override; @@ -77,6 +77,7 @@ private: uORB::Publication _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)}; + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; uORB::Subscription _params_sub{ORB_ID(parameter_update)}; diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index c712a708ec..66e4d32ecf 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -137,7 +137,7 @@ void VehicleAirData::Run() if (!_advertised[uorb_index]) { // use data's timestamp to throttle advertisement checks - if (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s) { + if ((_last_data[uorb_index].timestamp == 0) || (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s)) { if (_sensor_sub[uorb_index].advertised()) { if (uorb_index > 0) { /* the first always exists, but for each further sensor, add a new validator */ @@ -151,12 +151,17 @@ void VehicleAirData::Run() // force temperature correction update SensorCorrectionsUpdate(true); + if (_selected_sensor_sub_index < 0) { + _sensor_sub[uorb_index].registerCallback(); + } + } else { _last_data[uorb_index].timestamp = hrt_absolute_time(); } } + } - } else { + if (_advertised[uorb_index]) { updated[uorb_index] = _sensor_sub[uorb_index].update(&_last_data[uorb_index]); if (updated[uorb_index]) { @@ -204,7 +209,8 @@ void VehicleAirData::Run() _baro_sum_count++; if ((_param_sens_baro_rate.get() > 0) - && hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_baro_rate.get())) { + && ((_last_publication_timestamp == 0) + || (hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_baro_rate.get())))) { const float pressure = _baro_sum / _baro_sum_count; const hrt_abstime timestamp_sample = _baro_timestamp_sum / _baro_sum_count; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 6c5cd40973..13988bd7f5 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -68,7 +68,8 @@ bool VehicleAngularVelocity::Start() } if (!SensorSelectionUpdate(true)) { - ScheduleDelayed(10_ms); + _selected_sensor_sub_index = 0; + _sensor_sub.registerCallback(); } return true; @@ -157,6 +158,15 @@ void VehicleAngularVelocity::CheckFilters() void VehicleAngularVelocity::SensorBiasUpdate(bool force) { + // find corresponding estimated sensor bias + if (_estimator_selector_status_sub.updated()) { + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { + _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance); + } + } + if (_estimator_sensor_bias_sub.updated() || force) { estimator_sensor_bias_s bias; @@ -321,6 +331,7 @@ void VehicleAngularVelocity::PrintStatus() PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz", _selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz); PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2)); + _calibration.PrintStatus(); } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 1841d252a6..30152b7262 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -58,7 +59,6 @@ namespace sensors class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkItem { public: - VehicleAngularVelocity(); ~VehicleAngularVelocity() override; @@ -80,6 +80,7 @@ private: uORB::Publication _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)}; uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; uORB::Subscription _params_sub{ORB_ID(parameter_update)}; diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 120266566a..5db4b94227 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -199,7 +199,7 @@ void VehicleMagnetometer::Run() if (!_advertised[uorb_index]) { // use data's timestamp to throttle advertisement checks - if (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s) { + if ((_last_data[uorb_index].timestamp == 0) || (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s)) { if (_sensor_sub[uorb_index].advertised()) { if (uorb_index > 0) { /* the first always exists, but for each further sensor, add a new validator */ @@ -217,12 +217,18 @@ void VehicleMagnetometer::Run() } } + if (_selected_sensor_sub_index < 0) { + _sensor_sub[uorb_index].registerCallback(); + } + } else { _last_data[uorb_index].timestamp = hrt_absolute_time(); } } - } else { + } + + if (_advertised[uorb_index]) { sensor_mag_s report; while (_sensor_sub[uorb_index].update(&report)) { @@ -339,8 +345,8 @@ void VehicleMagnetometer::Run() void VehicleMagnetometer::Publish(uint8_t instance, bool multi) { - if ((_param_sens_mag_rate.get() > 0) - && hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_mag_rate.get())) { + if ((_param_sens_mag_rate.get() > 0) && (_last_publication_timestamp[instance] || + (hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_mag_rate.get())))) { const Vector3f magnetometer_data = _mag_sum[instance] / _mag_sum_count[instance]; const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _mag_sum_count[instance]; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 69409983a3..f1563f02e7 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -374,14 +374,19 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw) status.accel_device_id_primary = _selection.accel_device_id; status.gyro_device_id_primary = _selection.gyro_device_id; + static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::accel_inconsistency_m_s_s) / sizeof( + sensors_status_imu_s::accel_inconsistency_m_s_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size"); + static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof( + sensors_status_imu_s::gyro_inconsistency_rad_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size"); + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - if (_accel_device_id[i] != 0) { + if ((_accel_device_id[i] != 0) && (_accel.priority[i] > 0)) { status.accel_device_ids[i] = _accel_device_id[i]; status.accel_inconsistency_m_s_s[i] = _accel_diff[i].norm(); status.accel_healthy[i] = (_accel.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR); } - if (_gyro_device_id[i] != 0) { + if ((_gyro_device_id[i] != 0) && (_gyro.priority[i] > 0)) { status.gyro_device_ids[i] = _gyro_device_id[i]; status.gyro_inconsistency_rad_s[i] = _gyro_diff[i].norm(); status.gyro_healthy[i] = (_gyro.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR); @@ -403,32 +408,50 @@ void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw) void VotedSensorsUpdate::calcAccelInconsistency() { - const Vector3f primary_accel{_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2}; + Vector3f accel_mean{}; + Vector3f accel_all[MAX_SENSOR_COUNT] {}; + uint8_t accel_count = 0; - // Check each sensor against the primary for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - // check that the sensor we are checking against is not the same as the primary - if (_accel.advertised[sensor_index] && (_accel.priority[sensor_index] > 0) - && (_accel_device_id[sensor_index] != _selection.accel_device_id)) { + if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) { + accel_count++; + accel_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].accelerometer_m_s2}; + accel_mean += accel_all[sensor_index]; + } + } - const Vector3f current_accel{_last_sensor_data[sensor_index].accelerometer_m_s2}; - _accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (primary_accel - current_accel); + if (accel_count > 0) { + accel_mean /= accel_count; + + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) { + _accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (accel_all[sensor_index] - accel_mean); + } } } } void VotedSensorsUpdate::calcGyroInconsistency() { - const Vector3f primary_gyro{_last_sensor_data[_gyro.last_best_vote].gyro_rad}; + Vector3f gyro_mean{}; + Vector3f gyro_all[MAX_SENSOR_COUNT] {}; + uint8_t gyro_count = 0; - // Check each sensor against the primary for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - // check that the sensor we are checking against is not the same as the primary - if (_gyro.advertised[sensor_index] && (_gyro.priority[sensor_index] > 0) - && (_gyro_device_id[sensor_index] != _selection.gyro_device_id)) { + if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) { + gyro_count++; + gyro_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].gyro_rad}; + gyro_mean += gyro_all[sensor_index]; + } + } - const Vector3f current_gyro{_last_sensor_data[sensor_index].gyro_rad}; - _gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (primary_gyro - current_gyro); + if (gyro_count > 0) { + gyro_mean /= gyro_count; + + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) { + _gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (gyro_all[sensor_index] - gyro_mean); + } } } } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 59eaf69c5b..750a3d5a2c 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -142,12 +142,12 @@ private: bool checkFailover(SensorData &sensor, const char *sensor_name); /** - * Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor + * Calculates the magnitude in m/s/s of the largest difference between each accelerometer vector and the mean of all vectors */ void calcAccelInconsistency(); /** - * Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor + * Calculates the magnitude in rad/s of the largest difference between each gyro vector and the mean of all vectors */ void calcGyroInconsistency(); diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 1ecd1f1c07..f2b0308c56 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -143,6 +143,8 @@ private: delete _dist_pubs[i]; } + px4_lockstep_unregister_component(_lockstep_component); + _instance = nullptr; } @@ -288,6 +290,8 @@ private: px4::atomic _has_initialized {false}; #endif + int _lockstep_component{-1}; + DEFINE_PARAMETERS( (ParamInt) _param_mav_type, (ParamInt) _param_mav_sys_id, diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 7951b1a8a5..b416240ef1 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -368,6 +368,10 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg) void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) { + if (_lockstep_component == -1) { + _lockstep_component = px4_lockstep_register_component(); + } + mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); @@ -399,6 +403,8 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) } #endif + + px4_lockstep_progress(_lockstep_component); } void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg) @@ -627,9 +633,11 @@ void Simulator::send() parameters_update(false); check_failure_injections(); _vehicle_status_sub.update(&_vehicle_status); - send_controls(); + // Wait for other modules, such as logger or ekf2 px4_lockstep_wait_for_components(); + + send_controls(); } } } diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp index 8d1c3e15a5..dfd93722fb 100644 --- a/src/modules/uuv_att_control/uuv_att_control.cpp +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -59,7 +59,7 @@ extern "C" __EXPORT int uuv_att_control_main(int argc, char *argv[]); UUVAttitudeControl::UUVAttitudeControl(): ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::attitude_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { diff --git a/src/modules/uuv_att_control/uuv_att_control.hpp b/src/modules/uuv_att_control/uuv_att_control.hpp index 6f3a880e57..648cfa46cc 100644 --- a/src/modules/uuv_att_control/uuv_att_control.hpp +++ b/src/modules/uuv_att_control/uuv_att_control.hpp @@ -71,7 +71,6 @@ #include #include #include -#include #include using matrix::Eulerf; diff --git a/src/systemcmds/top/CMakeLists.txt b/src/systemcmds/top/CMakeLists.txt index 351ef8a9e6..e5ab7671b9 100644 --- a/src/systemcmds/top/CMakeLists.txt +++ b/src/systemcmds/top/CMakeLists.txt @@ -35,7 +35,7 @@ px4_add_module( MAIN top STACK_MAIN 4096 PRIORITY - "SCHED_PRIORITY_MAX - 16" # max priority below high priority WQ threads + "SCHED_PRIORITY_MAX - 18" # max priority below high priority WQ threads SRCS top.cpp DEPENDS