Création d'un enregistreur de données personnalisé avec Raspberry Pi pour les véhicules : intégration des capteurs BNO et ELM

Résumé

En tant qu'ingénieur logiciel travaillant dans l'industrie automobile, je m'intéresse vivement aux véhicules autonomes, aux techniques de mesure des données et aux méthodes d'analyse. Dans cet article, je décrirai un système de mesure sur mesure, détaillant le processus à partir de zéro, et présenterai quelques résultats expérimentaux. Mon enregistreur de données se compose d'un Raspberry Pi 3, d'un capteur BNO055 et d'un adaptateur ELM327 OBD-II, qui sont respectivement responsables du calcul, de la collecte des données d'accélération et de vitesse de rotation et de la récupération des informations sur le moteur d'un véhicule réel.

Table des matières

1.Contexte
2.Architecture
3.Informations sur le capteur
4.Informations sur le système de coordonnées
5. Comparaison des données
6.Logiques de base
7.Conclusion
8.Référence

1. Contexte

Ces dernières années, l'essor de l'IoT, de l'intelligence artificielle et de l'informatique de pointe a apporté des progrès significatifs à diverses industries, dont le secteur automobile. Les véhicules modernes sont équipés d'une multitude de capteurs qui fonctionnent ensemble pour créer des systèmes sophistiqués, permettant tout, des fonctionnalités de sécurité améliorées aux capacités de conduite autonome.

En règle générale, la surveillance de l’état d’un véhicule nécessite l’utilisation de capteurs avancés et coûteux pour obtenir des mesures précises et fiables. Cependant, pour certaines situations, comme l'obtention de données sur des circuits pour une compréhension générale de la dynamique du véhicule, ces capteurs haut de gamme peuvent ne pas être strictement nécessaires. Conscient de cela, j'ai décidé de créer un système rentable capable de mesurer l'état physique d'un véhicule à l'aide de composants plus abordables.

J'ai décidé de me concentrer sur la mesure des données liées à la dynamique du véhicule plutôt que sur le confort de conduite. En effet, l'analyse du confort de conduite nécessite de capturer davantage de signaux haute fréquence avec une précision appropriée, ce qui nécessite de définir une fréquence d'échantillonnage élevée pendant la mesure.

Plus précisément, dans les caractéristiques de réponse en fréquence de l'accélération latérale par rapport à l'angle de braquage, la fréquence de résonance apparaît généralement inférieure à 5 Hz, bien qu'elle dépende de la vitesse du véhicule. En revanche, dans l'analyse du confort de conduite, les discussions s'étendent souvent à plusieurs dizaines de Hz.

Par conséquent, l'objectif principal du développement du système de mesure est de créer un système d'enregistrement de données à faible coût qui facilite l'analyse des données dans le domaine de la dynamique des véhicules.

2. Architecture

Le système de mesure se compose du matériel suivant :

• Framboise Pi 3
• BNO055
• Scanner ELM327 OBD2
• Onduleur
• Moniteur
• clavier/souris (si besoin)

Le Raspberry Pi fait office d'ordinateur principal, collectant les données du BNO055 et de l'ELM327. Le Raspberry Pi communique avec le BNO055 via I2C et avec l'ELM327 via Bluetooth (voir Figure 1).

Figure 1 : Architecture matérielle

3. Informations sur le capteur

3-1. Adafruits BNO055

Le capteur BNO055 est capable de mesurer différents types de mouvements et de données :★Adafruit BNO055 Aperçu[1]

• Orientation absolue (vecteur d'Euler, 100 Hz) : fournit des données d'orientation sur trois axes basées sur une sphère à 360°.

• Orientation absolue (Quaternion, 100 Hz) : offre une sortie quaternion à quatre points pour une manipulation plus précise des données.

• Vecteur de vitesse angulaire (100 Hz) : mesure la vitesse de rotation en radians par seconde (rad/s) sur trois axes.

• Vecteur d'accélération (100 Hz) : capture l'accélération, y compris la gravité et le mouvement linéaire, en mètres par seconde carrée (m/s²) sur trois axes.

• Vecteur d'intensité de champ magnétique (20 Hz) : détecte l'intensité du champ magnétique en microteslas (µT) sur trois axes.

• Vecteur d'accélération linéaire (100 Hz) : enregistre les données d'accélération linéaire (hors gravité) en mètres par seconde carrée (m/s²) sur trois axes.

• Vecteur de gravité (100 Hz) : mesure l'accélération gravitationnelle (à l'exclusion de tout mouvement) en mètres par seconde carrée (m/s²) sur trois axes.

• Température (1 Hz) : fournit la température ambiante en degrés Celsius.

Le BNO055 est compact et pris en charge par la bibliothèque Python d'Adafruit appelée Adafruit_CircuitPython. De plus, une bibliothèque en langage C est disponible auprès d'Adafruit.

★Adafruit_CircuitPython_BNO055[2]

★Adafruit_BNO055[3]

Pour une démonstration, vous pouvez visionner la vidéo ici :
Démo Adafruit BNO055
★Démo Adafruit BNO055[4]

3-2. ELM327

The ELM327 is a widely used OBD-II (On-Board Diagnostics) adapter that allows access to vehicle engine data through a standard interface. It acts as a bridge between the vehicle's ECU (Electronic Control Unit) and external devices such as computers or smartphones, enabling the retrieval of diagnostic and performance data. Key features and capabilities of the ELM327 include:★ELM327 information[5]

• OBD-II Protocol Support: The ELM327 supports various OBD-II protocols, including ISO 9141, ISO 14230 (KWP2000), ISO 15765 (CAN), and more, making it compatible with a broad range of vehicles.

• Diagnostic Trouble Codes (DTCs): It can read and clear diagnostic trouble codes from the vehicle's ECU, assisting in identifying and troubleshooting issues.

• Live Data Streaming: Provides real-time data from the vehicle's sensors, such as engine RPM, vehicle speed, coolant temperature, and fuel level.

• Freeze Frame Data: Captures and stores data at the moment a fault is detected, which helps in diagnosing intermittent problems.

• Vehicle Information: Retrieves details about the vehicle, including the VIN (Vehicle Identification Number), calibration IDs, and more.

• Compatibility: Available in various forms, including Bluetooth, USB, and Wi-Fi versions, allowing compatibility with different devices and platforms.

Typically, the ELM327 is used to diagnose vehicle status, particularly engine states. However, it can also be employed to collect a wide range of data from the vehicle. The ELM327 is available for purchase on platforms like Amazon, with prices ranging from approximately $20 to $80.

Using the python-OBD library, you can create queries to gather data via the ELM327 adapter, allowing for customizable and detailed data collection

4. Coordinate system information

Since this system interacts with multiple sensors and an actual vehicle, it must understand each coordinate system.

4-1. BNO055 coordinate system for data logger

The image below shows the BNO055 coordinate system for my data measurement system. My system additionally calculates rotational angles using quaternions. The sensor coordinate system needs to match the vehicle coordinate system.

Figure 2: BNO055 coordinate system

4-2. Vehicle coordinate system(ISO 8855)

The measurement system operates based on the vehicle coordinate system.

Figure 3: Vehicle coordinate system [6] Source: MathWorks, Coordinate Systems in Automated Driving Toolbox.

4-3. iPhone Coordinate System

I used an iPhone application that can measure multiple physical data points, including acceleration, gyro, and more, for validation tasks. The purpose of using the iPhone application is to confirm the validity of my data measurement system with data from this application. Figure 4 is cited from Apple's developer site.

Figure 4: iPhone coordinate system [7] Source: Apple, Getting raw gyroscope events.

5. Data Comparison

In this section, I present some experimental results. As the first step, I conducted sine wave tasks to confirm whether my measurement system outputs readings without errors, such as unit mistakes. This part is basic but crucial to ensure the correct way to gather data.
Next, I measured some physical values on an actual vehicle while driving.
Then, I checked ELM327's performance and sampling accuracy of each sensor.

5-1. Sine wave tests

I conducted sine wave tests, and in this section, I present the results. The objective of these tests is to confirm the following:

• Whether the coordinate system of the measurement system is functioning properly
• To validate the accuracy of the measurement app on the iPhone

To achieve this, I used "phyphox" ★phyphox[8], a smartphone app that can measure various physical values. I mounted my BNO055 sensor on the iPhone as shown below [Figure 5].

Figure 5: Image of the Sensor and iPhone Mounting Method for Conducting Sine Wave Tests

Figure 6 shows the sine wave test results with a sampling frequency of 10 Hz. The legend "VDDM" represents data collected by a measurement system I developed, while "iPhone" indicates data collected using "phyphox".

Figure 6: Sine Wave Tests Results

• The data from both VDDM and the iPhone show similar trends across all axes, indicating that both applications are capturing similar motion patterns.

• The VDDM data appears to be slightly more variable, particularly in the acceleration graphs, which might suggest higher sensitivity or a different filtering approach compared to the iPhone.

• Both data generally agree well, but the differences in amplitude suggest that further calibration or adjustment may be needed depending on the accuracy requirements.

• The VDDM might be capturing additional noise or higher frequency components, especially in the acceleration graphs, which are not present in the iPhone data. This could either be beneficial for detailed analysis or require further filtering.

5-2. Vehicle Dynamics Data Evaluation

Figure 7 shows the results of an experiment focusing on data from the BNO055 in an actual vehicle. An iPhone application called "phyphox" was used as a reference. The sampling frequency was 50Hz. I observed that the acceleration and rotational signals measured by the data logging system I developed on the vehicle contained noise, outliers, and NaN values. Therefore, I applied post-processing functions, including an IQR method and a Butterworth low-pass filter. The term "fc" in the legend of each graph represents the cutoff frequency. For example, fc_19.5 indicates a cutoff frequency of 19.5Hz. Note that this experiment was conducted using only the BNO055 sensor, and the ELM327 function was disabled.

Figure 7: Actual vehicle test result

• The data from VDDM (raw data) and the iPhone show similar trends across all axes. This suggests that both systems are capturing the same motion patterns.

• The data from VDDM appears to have greater variation compared to the iPhone data. This is particularly noticeable in the acceleration graphs and may indicate that VDDM captures data with higher sensitivity or uses a different filtering approach.

• Filtered data from VDDM (with fc 19.5Hz and 10Hz) shows reduced variation compared to the raw data and is closer to the iPhone data. This suggests that VDDM (raw data) might include more noise or high-frequency components.

• Raw data from VDDM contained outliers and missing values, which were mitigated by applying filters. This is particularly evident in the roll rate and pitch rate graphs.

• VDDM appears to include more high-frequency components and noise compared to the iPhone. While this can be advantageous for detailed analysis, additional filtering or hardware-level adjustments might be needed if noise reduction is critical.

5-3. Vehicle Speed Evaluation

Figure 8 shows data regarding engine performance. These data were collected using the ELM327 scanner with a sampling frequency of 4 Hz. The BNO055 function was disabled during data collection.

Figure 8: Vehicle speed and Engine data

• Vehicle speed data from VDDM (raw data) and the iPhone exhibit similar trends.
• The ELM327 can collect information on engine RPM and throttle position. These data are useful for understanding general throttle pedal operation. Note that the test vehicle is HEV.

5-4. Evaluation of the accuracy of the sampling frequency

During my experiments with the BNO055 and ELM327, I observed delays in sampling. To identify the issues, I implemented measurements by adjusting the sampling frequency and by activating and deactivating the ELM327 and BNO055.
Initially, I present the time difference corresponding to the sampling frequency. This test involves the use of both the BNO055 and the ELM327 sensors

Figure 9: Time difference corresponding to the sampling frequency

Figure 9 shows that when the sampling frequency is set to 10Hz, the precision of the sampling time significantly decreases.
The ELM327 cannot achieve sampling frequencies as high as the BNO055, which may negatively impact the overall system performance. Nevertheless, to better understand the performance of each sensor, I conducted individual measurements.

Figures 10 and 11 illustrate the relationship between sampling frequencies and the sampling timing precision. The sampling timing precision is represented as:

Figure 10: BNO055_Sampling Test Result

According to Figure 10, the BNO055 can respond to sampling frequencies up to 50 Hz. However, beyond 60 Hz, the sampling timing precision drops dramatically.

Figure 11: BNO055 Sampling Test Result

Figure 11 shows that the ELM327 has lower responsivity compared to the BNO055. The ELM327 maintains good sampling timing precision only up to 5 Hz. This is because the ELM327 communicates with a vehicle via OBD, and the vehicle's ECU may not support high-frequency responses to OBD requests, resulting in low-frequency communication.

Additionally, the ELM327 is the cheapest and lowest-quality option among sensors that can retrieve data from multiple ECUs in a vehicle via OBD-II.
This post★Challenging your car's trip computer fuel efficiency figures[9] says that

One important thing to consider with the Elm327 is that response times are far from being negligible: the device may take .1 to .3 seconds to answer to a message request, which means that adding too many signals rapidly ends up reducing the sampling rate to a point where it gets useles

For more detailed advice, visit this page:★Choosing OBDII adapter[10]

As for the causes of the low performance, the following reasons could be considered:

• Vehicle ECU's specifications
• Too many requests to the OBD scanner
• Requests for multiple signals from multiple ECUs
• Issues with software
• Network problems
• OBD-II protocol overhead
• Scanner hardware limitations
• Vehicle communication speed
• Firmware and driver issues
• Environmental factors

6. Core Logics

Measurement software that I have been developing assumes that use cases that multiple sensors need. Additionally, a user can change measurement settings on the yaml file. A user can operate the measurement application on a simple GUI. You can see source code here: ★VDDM[11]

The structure of the measurement system is below:

VDDM/
│
├── fusion/
│   ├── __init__.py
│   ├── sensor_fusion.py   # Handles data fusion, collects data from sensors
│   └── sensors/
│       ├── __init__.py
│       ├── bno055_measurement.py   # Script for BNO055 sensor data collection
│       └── elm327_measurement.py   # Script for ELM327 OBD2 data collection
│
├── signalprocessing/
│   ├── __init__.py
│   └── filter.py           # Contains filtering algorithms (e.g., Butterworth filter)
│
├── config/
│   ├── config_manager.py    # Loads configuration from YAML
│   └── measurement_system_config.yaml   # Configuration file for sensors and system settings
│
├── utils/
│   ├── tools.py             # Utility functions (e.g., wait functions)
│   └── visualize_data.py    # Functions to format and display sensor data
│
├── measurement/
│   ├── __init__.py
│   └── measurement_control.py   # Controls the measurement process
│
├── gui/
│   ├── __init__.py
│   └── main_gui.py          # GUI setup for starting/stopping measurement
│
├── main.py                  # Entry point for starting the system
└── requirements.txt         # Project dependencies

Below diaglam indicates the flow of VDDM.

main.py
  └── main_gui.py
        ├── Starts GUI interface and buttons
        └── measurement_control.py
              ├── Initializes sensor measurements
              ├── Controls start/stop of measurements
              └── sensor_fusion.py
                    ├── Manages sensor data collection and fusion
                    ├── Uses:
                    │    ├── bno055_measurement.py  (BNO055 sensor data collection)
                    │    └── elm327_measurement.py  (ELM327 OBD2 data collection)
                    └── Processes data with:
                         ├── filter.py  (Applies Butterworth low-pass filter)
                         └── visualize_data.py  (Formats and visualizes sensor data)

VDDM's main components are VDDM/fusion/sensor_fusion.py, VDDM/fusion/sensors/bno055_measurement.py, and VDDM/fusion/sensors/elm327_measurement.py.

# sensor_fusion.py
import os
import sys
import importlib
from time import perf_counter
from collections import defaultdict
import pandas as pd
import datetime
import asyncio
import numpy as np



parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))
sys.path.append(parent_dir)

from config import config_manager
from utils.tools import wait_process
from utils.visualize_data import format_sensor_fusion_data
from signalprocessing.filter import butterlowpass

config_path = os.path.join(parent_dir, 'config', 'measurement_system_config.yaml')


class SensorFactory:
    @staticmethod
    def create_sensor(sensor_type, config):
        try:
            # Import the appropriate sensor module based on sensor_type
            module = importlib.import_module(f"fusion.sensors.{sensor_type}_measurement")
            # Get a sensor class
            sensor_class = getattr(module, f"{sensor_type.upper()}")
            # Create a sensor instance and return
            return sensor_class(config)
        except (ImportError, AttributeError) as e:
            print(f"Error creating sensor {sensor_type}: {e}")
            return None


class Sensors:
    def __init__(self, config):
        self.config = config_manager.load_config(config_path)
        self.sensor_list = tuple(self.config.sensors.keys())
        self.sensor_instances = {}
        self.is_running = False


        self.SAMPLING_FREQUENCY_HZ = config.sampling_frequency_hz
        self.SAMPLING_TIME = 1 / self.SAMPLING_FREQUENCY_HZ
        self.SAVE_DATA_DIR = config.save_data_dir
        self.SAVE_BUF_CSVDATA_PATH = self.SAVE_DATA_DIR + "/" + "measurement_raw_data.csv"
        self.SEQUENCE_LENGTH = config.sequence_length # Windows size [s]
        # Buffer size is determined by the relation of sequence length and sampling frequency
        # Buffer secures data for SEQUENCE_LENGTH[s]
        self.MAX_DATA_BUF_LEN = self.SEQUENCE_LENGTH * self.SAMPLING_FREQUENCY_HZ
        self.FPASS = config.filter_params.fpass
        self.FSTOP = config.filter_params.fstop
        self.GPASS = config.filter_params.gpass
        self.GSTOP = config.filter_params.gstop
        self.is_filter = config.filter_params.is_filter
        self.is_show_real_time_data = config.is_show_real_time_data
        self.TIMEZONE = config.timezone
        self.all_data_columns_list = ()
        for sensor_name in self.sensor_list:
            self.all_data_columns_list += tuple(self.config["sensors"][sensor_name]["data_columns"])            




        self.data_buffer = pd.DataFrame()  # data buffer

        for sensor_type in self.sensor_list:
            sensor_config = self.config.sensors[sensor_type]
            sensor_instance = SensorFactory.create_sensor(sensor_type, sensor_config)
            if sensor_instance:
                self.sensor_instances[sensor_type] = sensor_instance


        if os.path.exists(self.SAVE_BUF_CSVDATA_PATH):
            os.remove(self.SAVE_BUF_CSVDATA_PATH)
            print(f"File  '{self.SAVE_BUF_CSVDATA_PATH}' was deleted for initialization")                


    def get_sensor(self, sensor_type):
        """
        Retrieve the sensor instance corresponding to the specified sensor type.

        Args:
            sensor_type (str): The type of the sensor to retrieve.

        Returns:
            object: The sensor instance corresponding to the specified sensor type.
                    Returns None if the sensor type does not exist.
        """
        return self.sensor_instances.get(sensor_type)

    def collect_data(self):
        """
        Collect data from all sensors.

        This method iterates over all sensor instances and collects data from each sensor.
        The collected data is stored in a dictionary where the keys are sensor types and
        the values are the data collected from the corresponding sensors.

        Returns:
            dict: A dictionary containing the collected data from all sensors.
                The keys are sensor types and the values are the data from each sensor.

        Raises:
            Exception: If an error occurs while collecting data from any sensor, the exception
                    is caught and printed.
        """
        data = {}
        try:
            for sensor_type, sensor in self.sensor_instances.items():
                # get data from sensors
                data[sensor_type] = sensor.get_data_from_sensor()
            return data
        except Exception as e:
            print(e)



    def on_change_start_measurement(self):
        """
        Start the measurement process.

        This method sets the is_running flag to True, indicating that the measurement
        process should start.
        """
        self.is_running = True

    def on_change_stop_measurement(self):
        """
        Stop the measurement process.

        This method sets the is_running flag to False, indicating that the measurement
        process should stop.
        """
        self.is_running = False


    def filtering(self, df, labellist):
        """
        Apply a low-pass filter to the specified columns in the DataFrame.

        This method applies a Butterworth low-pass filter to each column specified
        in the labellist. The "Time" column should be excluded from the labellist
        as it is not needed for the computation.

        Args:
            df (pd.DataFrame): The input DataFrame containing the data to be filtered.
            labellist (list of str): A list of column names to be filtered. The "Time"
                                     column should not be included in this list.

        Returns:
            pd.DataFrame: A new DataFrame with the filtered data.
        """
        filtered_df = df.copy()
        for labelname in labellist:
            # Ensure the column is converted to a numpy array
            x = df[labelname].to_numpy()
            filtered_df[labelname] = butterlowpass(
                x=x,  # Correctly pass the numpy array as 'x'
                fpass=self.FPASS,
                fstop=self.FSTOP,
                gpass=self.GPASS,
                gstop=self.GSTOP,
                fs=self.SAMPLING_FREQUENCY_HZ,
                dt=self.SAMPLING_TIME,
                checkflag=False,
                labelname=labelname
            )
        return filtered_df

    def convert_dictdata(self, current_time, sensor_data_dict):
        """
        Convert nested dictionary data from multiple sensors into a single DataFrame.

        This method converts nested dictionary data obtained from multiple sensors
        into a single dictionary and then converts it into a pandas DataFrame. The
        current_time information is associated with the data.

        Args:
            current_time (float): The current time at which the data was obtained.
            sensor_data_dict (dict): A nested dictionary containing data from multiple sensors.

        Returns:
            pd.DataFrame: A DataFrame containing the converted data with the current time information.
        """
        converted_data = {'Time': current_time}
        for sensor, data in sensor_data_dict.items():
            converted_data.update(data)

        converted_data = pd.DataFrame([converted_data])

        return converted_data



    async def update_data_buffer(self, dict_data):
        """
        Add data from sensors to the buffer and save it if necessary.

        This method adds the provided sensor data to the internal buffer. If the buffer
        exceeds the specified maximum length, the oldest data is saved to a CSV file
        and removed from the buffer.

        Args:
            dict_data (dict): The data from sensors to be added to the buffer.
        """

        # Add data to the buffer
        self.data_buffer = pd.concat([self.data_buffer, dict_data], ignore_index=True)

        # If the buffer exceeds the specified length, save the oldest data
        if len(self.data_buffer) > self.MAX_DATA_BUF_LEN:
            # Save the oldest data to a CSV file
            old_data = self.data_buffer.head(self.MAX_DATA_BUF_LEN)

            await self.save_data(old_data, self.SAVE_BUF_CSVDATA_PATH)

            # Update the buffer
            self.data_buffer = self.data_buffer.tail(len(self.data_buffer) - self.MAX_DATA_BUF_LEN)        



    async def save_data_async(self, df, path):
        """
        Save the DataFrame to a CSV file asynchronously.

        This method uses asyncio.to_thread to run the synchronous to_csv method
        in a separate thread, allowing it to be handled asynchronously.

        Args:
            df (pd.DataFrame): The DataFrame to be saved.
            path (str): The file path where the DataFrame should be saved.
        """
        if not os.path.isfile(path):
            await asyncio.to_thread(df.to_csv, path, sep=',', encoding='utf-8', index=False, header=True, mode='w')
        else:
            await asyncio.to_thread(df.to_csv, path, sep=',', encoding='utf-8', index=False, header=False, mode='a')

    async def save_data(self, df, path):
        """
        Save the DataFrame to a CSV file asynchronously.

        This method calls save_data_async to save the DataFrame to a CSV file
        asynchronously.

        Args:
            df (pd.DataFrame): The DataFrame to be saved.
            path (str): The file path where the DataFrame should be saved.
        """
        await self.save_data_async(df, path)



    async def finish_measurement_and_save_data(self):
        """
        Finish the measurement process and save the data.

        This method finalizes the measurement process by saving the buffered data
        to a CSV file. It also applies filtering if specified and saves the filtered
        data to a separate CSV file. The method handles time zone settings and
        generates a timestamp for the file names.

        The buffered data is saved to a temporary CSV file, which is then read back
        and saved to a final file path with a timestamp. If filtering is enabled,
        the filtered data is also saved. The temporary CSV file is deleted after
        the data is saved.

        Raises:
            Exception: If an error occurs during the file operations.
        """
        t_delta = datetime.timedelta(hours=9)
        TIMEZONE = datetime.timezone(t_delta, self.TIMEZONE)# You have to set your timezone
        now = datetime.datetime.now(TIMEZONE)
        timestamp = now.strftime('%Y%m%d%H%M%S')
        final_file_path = self.SAVE_BUF_CSVDATA_PATH.replace(self.SAVE_BUF_CSVDATA_PATH.split('/')[-1], 
                                                   timestamp + "/" + timestamp + '_' + 
                                                   self.SAVE_BUF_CSVDATA_PATH.split('/')[-1])
        await self.save_data_async(self.data_buffer, self.SAVE_BUF_CSVDATA_PATH)
        raw_df = pd.read_csv(self.SAVE_BUF_CSVDATA_PATH, header=0)
        os.makedirs(self.SAVE_DATA_DIR + "/" + timestamp, exist_ok=True)
        raw_df.to_csv(final_file_path, sep=',', encoding='utf-8', index=False, header=True)


        if self.is_filter:
            filt_df = self.filtering(df=raw_df, labellist=raw_df.columns[1:])
            filt_df.to_csv(final_file_path.replace('_raw_data.csv', '_filt_data.csv'), sep=',', encoding='utf-8', index=False, header=True)

        if os.path.exists(self.SAVE_BUF_CSVDATA_PATH):
            os.remove(self.SAVE_BUF_CSVDATA_PATH)
            print(f"File  '{self.SAVE_BUF_CSVDATA_PATH}' was deleted")
        else:
            print(f"File '{self.SAVE_BUF_CSVDATA_PATH}' is not existed")






async def sensor_fusion_main():
    """
    Main function for sensor fusion.

    This function initializes the sensor fusion process, starts the measurement loop,
    collects data from multiple sensors, updates the data buffer, and handles real-time
    data display. It also calculates and prints the sampling delay and reliability rate
    upon termination.

    The main loop runs until the measurement process is stopped, either by an exception
    or a keyboard interrupt.

    Raises:
        Exception: If an error occurs during the measurement process, it is caught and printed.
        KeyboardInterrupt: If a keyboard interrupt occurs, the measurement process is stopped
                           and the data is saved.

    """
    print("Start sensor fusion main")
    config = config_manager.load_config(config_path)
    sensors = Sensors(config["master"])
    print("Called an instance of Sensors class")

    # sensors.start_all_measurements()
    sampling_counter = 0
    current_time = 0
    #sensors.is_running = True
    sensors.on_change_start_measurement()

    try:
        main_loop_start_time = None
        while sensors.is_running:
            iteration_start_time = perf_counter() # Start time of each iteration

            if main_loop_start_time is None:
                    main_loop_start_time = iteration_start_time  # initialize main loop start time

            current_time = perf_counter() - main_loop_start_time # Current time

            data = sensors.collect_data() # Get data from sensors                                        
            sampling_counter += 1 # Num of sampling

            converted_data = sensors.convert_dictdata(current_time, data) # Convert data to dataframe format
            # Update the data buffer. If it reaches the buffer limit, write the data to a CSV file.
            await sensors.update_data_buffer(converted_data)
            # Display data in real time. This process is executed on additional thread.
            if sensors.is_show_real_time_data:
                formatted_data = format_sensor_fusion_data(data, sensors.all_data_columns_list)    
                print("--------------------------------------------------------------------")
                print("Current Time is: {:.3f}".format(current_time))
                print(formatted_data)

            # Wait based on the sampling interval and execution time to maintain the sampling frequency.
            iteration_end_time = perf_counter()
            iteration_duration = iteration_end_time - iteration_start_time 
            print("Iteration duration is: {0} [s]".format(iteration_duration))
            sleep_time = max(0, sensors.SAMPLING_TIME - iteration_duration)
            if sleep_time > 0:
                wait_process(sleep_time)

    except Exception as e:
        print(e)

    except KeyboardInterrupt:
        sensors.on_change_stop_measurement()
        print("KeyboardInterrupt")
        await sensors.finish_measurement_and_save_data()

    finally:
        print("finish")
         # Compute delay of sampling
        main_loop_end_time = perf_counter() - main_loop_start_time
        print("Program terminated")
        print("main loop is ended. current time is: {:.3f}".format(current_time))
        print("main loop is ended. end time is: {:.3f}".format(main_loop_end_time))
        print("sampling num is: {}".format(sampling_counter))



        # Compute ideal sampliing time
        ideal_time = ((sampling_counter - 1) / sensors.SAMPLING_FREQUENCY_HZ)
        # Cpmpute a delay
        delay_time = current_time - ideal_time
        # reliability rate
        sampling_reliability_rate = (delay_time / (sampling_counter / sensors.SAMPLING_FREQUENCY_HZ)) * 100


        print("sampling delay is: {:.3f} s".format(delay_time))
        print("sampling delay rate is: {:.3f} %".format(sampling_reliability_rate))


if __name__ == '__main__':
    asyncio.run(sensor_fusion_main())

sensor_fusion.py manages sensor data collection and processing using asynchronous operations. It dynamically creates sensor instances, collects data, applies filters, and saves the data to CSV files. The script also displays real-time data if needed and monitors performance metrics like sampling delay and reliability. The main function runs a loop that handles data collection, buffer updates, and performance reporting. It uses asynchronous operations to efficiently manage data saving and ensure smooth performance.

# bno055_measurement.py
import time
import numpy as np
import adafruit_bno055
import board
import os
import sys

parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', '..'))
sys.path.append(parent_dir)

from config.config_manager import load_config
config_path = os.path.join(parent_dir, 'config', 'measurement_system_config.yaml')


class BNO055:
    def __init__(self, config):
        self.COLUMNS = config.data_columns    
        self.SAMPLING_FREQUENCY_HZ = config.sampling_frequency_hz
        self.SAMPLING_TIME = 1 / self.SAMPLING_FREQUENCY_HZ
        self.SAVE_DATA_DIR = config.save_data_dir
        self.SEQUENCE_LENGTH = config.sequence_length
        self.FPASS = config.filter_params.fpass
        self.FSTOP = config.filter_params.fstop
        self.GPASS = config.filter_params.gpass
        self.GSTOP = config.filter_params.gstop
        self.Isfilter = config.filter_params.is_filter

        self.IsStart = False
        self.IsStop = True
        self.Is_show_real_time_data = config.is_show_real_time_data 

        i2c_instance = board.I2C() # Create i2c instance
        self.bno055_sensor = adafruit_bno055.BNO055_I2C(i2c_instance) # create BNO055_I2C instance

    def calibration(self):
        print("Start calibration!")
        while not self.bno055_sensor.calibrated:
            print('SYS: {0}, Gyro: {1}, Accel: {2}, Mag: {3}'.format(*(self.bno055_sensor.calibration_status)))
            time.sleep(1)


    def calcEulerfromQuaternion(self, _w, _x, _y, _z):
        """
        Calculate Euler angles (roll, pitch, yaw) from quaternion components.

        This method converts quaternion components (_w, _x, _y, _z) into Euler angles
        (roll, pitch, yaw) in degrees. If any of the quaternion components are None,
        it returns (0.0, 0.0, 0.0) and prints an error message.

        Args:
            _w (float): The w component of the quaternion.
            _x (float): The x component of the quaternion.
            _y (float): The y component of the quaternion.
            _z (float): The z component of the quaternion.

        Returns:
            tuple: A tuple containing the roll, pitch, and yaw angles in degrees.
                If an error occurs, it returns (0.0, 0.0, 0.0) and prints an error message.
        """
        if None in (_w, _x, _y, _z):
            print(f"Error: One or more quaternion values are None: {_w}, {_x}, {_y}, {_z}")
            return 0.0, 0.0, 0.0

        try:
            sqw = _w ** 2
            sqx = _x ** 2
            sqy = _y ** 2
            sqz = _z ** 2
            COEF_EULER2DEG = 57.2957795131

            # Yaw
            term1 = 2.0 * (_x * _y + _z * _w)
            term2 = sqx - sqy - sqz + sqw
            yaw = np.arctan2(term1, term2)

            # Pitch
            term1 = -2.0 * (_x * _z - _y * _w)
            term2 = sqx + sqy + sqz + sqw
            pitch = np.arcsin(term1 / term2) if -1 <= term1 / term2 <= 1 else 0.0

            # Roll
            term1 = 2.0 * (_y * _z + _x * _w)
            term2 = -sqx - sqy + sqz + sqw
            roll = np.arctan2(term1, term2)

            return COEF_EULER2DEG * roll, COEF_EULER2DEG * pitch, COEF_EULER2DEG * yaw

        except Exception as e:
            print(f"Error in calcEulerfromQuaternion: {e}")
            return 0.0, 0.0, 0.0

    def get_data_from_sensor(self):
        """
        Retrieve data from the BNO055 sensor and return it as a dictionary.

        This method collects various sensor readings from the BNO055 sensor, including
        Euler angles, gyroscope data, linear acceleration, quaternion, magnetic field,
        and calibration status. It then constructs a dictionary with these values and
        returns only the columns specified in self.COLUMNS.

        Returns:
            dict: A dictionary containing the sensor data. Only the columns specified
                in self.COLUMNS are included in the returned dictionary.
        """
        # Get data
        euler_z, euler_y, euler_x = [val for val in self.bno055_sensor.euler]  # X: yaw, Y: pitch, Z: roll
        gyro_x, gyro_y, gyro_z = [val for val in self.bno055_sensor.gyro]  # Gyro[rad/s]
        linear_accel_x, linear_accel_y, linear_accel_z = [val for val in self.bno055_sensor.linear_acceleration]  # Linear acceleration[m/s^2]
        quaternion_1, quaternion_2, quaternion_3, quaternion_4 = [val for val in self.bno055_sensor.quaternion]  # Quaternion
        quat_roll, quat_pitch, quat_yaw = self.calcEulerfromQuaternion(quaternion_1, quaternion_2, quaternion_3, quaternion_4)  # Cal Euler angle from quaternion
        magnetic_x, magnetic_y, magnetic_z = [val for val in self.bno055_sensor.magnetic]  # Magnetic field
        calibstat_sys, calibstat_gyro, calibstat_accel, calibstat_mag = [val for val in self.bno055_sensor.calibration_status]  # Status of calibration


        data_dict = {
            "linear_accel_x": linear_accel_x,
            "linear_accel_y": linear_accel_y,
            "linear_accel_z": linear_accel_z,
            "gyro_x": gyro_x,
            "gyro_y": gyro_y,
            "gyro_z": gyro_z,
            "euler_x": euler_x,
            "euler_y": euler_y,
            "euler_z": euler_z,
            "quat_roll": quat_roll,
            "quat_pitch": quat_pitch,
            "quat_yaw": quat_yaw,
            "quaternion_1": quaternion_1,
            "quaternion_2": quaternion_2,
            "quaternion_3": quaternion_3,
            "quaternion_4": quaternion_4,
            "magnetic_x": magnetic_x,
            "magnetic_y": magnetic_y,
            "magnetic_z": magnetic_z,
            "calibstat_sys": calibstat_sys,
            "calibstat_gyro": calibstat_gyro,
            "calibstat_accel": calibstat_accel,
            "calibstat_mag": calibstat_mag
        }

        return {column: data_dict[column] for column in self.COLUMNS if column in data_dict}


def format_sensor_data(data, labels):
    """
    Format sensor data into a string for display.

    This method takes a dictionary of sensor data and a list of labels, and formats
    the data into a string where each label is followed by its corresponding value.
    If a value is None, it is replaced with the string "None". Each label-value pair
    is separated by " / ".

    Args:
        data (dict): The sensor data to format.
        labels (list of str): The list of labels to include in the formatted string.

    Returns:
        str: A formatted string containing the sensor data.
    """
    formatted_str = ""
    if isinstance(data, dict):
        for label in labels:
            value = data.get(label, None)
            if value is None:
                value = "None"
            else:
                value = f"{value:.4f}"
            formatted_str += f"{label}: {value} / "
    return formatted_str.rstrip(" / ")

def test_main():
    """
    Main function for testing sensor data collection and display.

    This function initializes the BNO055 sensor, starts a loop to collect data,
    formats the data for display, and prints it in real-time. It also calculates
    and prints the sampling delay and reliability rate upon termination.

    The main loop runs until interrupted by the user.

    Raises:
        KeyboardInterrupt: If a keyboard interrupt occurs, the loop is terminated
                           and the final statistics are printed.
    """
    from utils.tools import wait_process
    from time import perf_counter
    import matplotlib.pyplot as plt

    print("Main start")

    config = load_config(config_path)
    meas_bno055 = BNO055(config.sensors['bno055'])

    start_time = perf_counter()
    sampling_counter = 0

    try:
        main_loop_start_time = perf_counter()
        while True:
            iteration_start_time = perf_counter()

            # Data acquisition process
            data = meas_bno055.get_data_from_sensor()
            current_time = perf_counter() - start_time
            sampling_counter += 1

            if meas_bno055.Is_show_real_time_data:
                formatted_data = format_sensor_data(data, meas_bno055.COLUMNS)
                # current time    
                print("--------------------------------------------------------------------")
                print("Current Time is: {:.3f}".format(current_time))
                print(formatted_data)

            # Wait to meet the sampling frequency based on the sampling interval and execution time
            elapsed_time = perf_counter() - iteration_start_time
            sleep_time = meas_bno055.SAMPLING_TIME - elapsed_time
            if sleep_time > 0:
                wait_process(sleep_time)

    except KeyboardInterrupt:
        print("Interrupted by user")

    finally:
        # Calculate the sampling delay from the number of samples and the current time
        main_loop_end_time = perf_counter() - main_loop_start_time
        print("Program terminated")
        print("main loop is ended. current time is: {:.3f}".format(current_time))
        print("main loop is ended. end time is: {:.3f}".format(main_loop_end_time))
        print("sampling num is: {}".format(sampling_counter)) # Since it is 0-based, the number of samples is current_time + 1

        # Calculate the ideal sampling time
        ideal_time = ((sampling_counter - 1) / meas_bno055.SAMPLING_FREQUENCY_HZ)
        # Calculate the delay
        delay_time = current_time - ideal_time
        # The reliability rate is the delay divided by the sampling time
        sampling_reliability_rate = (delay_time / (sampling_counter / meas_bno055.SAMPLING_FREQUENCY_HZ)) * 100

        print("sampling delay is: {:.3f} s".format(delay_time))
        print("sampling delay rate is: {:.3f} %".format(sampling_reliability_rate))




if __name__ == '__main__':
    test_main()

This script interfaces with the BNO055 sensor to collect and process data. It retrieves sensor readings, such as Euler angles and gyroscope data, formats them for display, and prints real-time updates if configured. The script includes a calibration method and calculates Euler angles from quaternion data. It manages data acquisition with a defined sampling frequency and calculates sampling delay and reliability. The main loop continues until interrupted, reporting performance metrics upon termination.

# elm327_measurement.py

import obd
import os
import time
from collections import deque
import numpy as np
import pandas as pd
import datetime
import asyncio
import scipy
from scipy import signal
import matplotlib as plt
import sys
from collections import defaultdict
import random

parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', '..'))
sys.path.append(parent_dir)

from config.config_manager import load_config
config_path = os.path.join(parent_dir, 'config', 'measurement_system_config.yaml')

class ELM327:
    def __init__(self, config):
        """
        Initialize the ELM327 class with configuration parameters.

        Args:
            config (dict): Configuration parameters for the ELM327.
        """
        self.COLUMNS = config.data_columns    
        self.SAMPLING_FREQUENCY_HZ = config.sampling_frequency_hz
        self.SAMPLING_TIME = 1 / self.SAMPLING_FREQUENCY_HZ
        self.SAVE_DATA_DIR = config.save_data_dir
        self.SEQUENCE_LENGTH = config.sequence_length
        self.FPASS = config.filter_params.fpass
        self.FSTOP = config.filter_params.fstop
        self.GPASS = config.filter_params.gpass
        self.GSTOP = config.filter_params.gstop
        self.Isfilter = config.filter_params.is_filter
        self.res = self.connect_to_elm327()

        self.is_offline = config.is_offline
        self.IsStart = False
        self.IsStop = True
        self.Is_show_real_time_data = config.is_show_real_time_data

    def initialize_BLE(self):
        """
        Initialize Bluetooth Low Energy (BLE) for ELM327 connection.
        """
        os.system('sudo hcitool scan')
        os.system('sudo hciconfig hci0 up')
        os.system('sudo rfcomm bind 0 8A:2A:D4:FF:38:F3')
        os.system('sudo rfcomm listen 0 1 &')

    def connect_to_elm327(self):
        """
        Establish a connection to the ELM327 device.

        Returns:
            res (obd.OBDStatus): The connection status of the ELM327 device.
        """
        res = None
        try:
            self.initialize_BLE()
            self.connection = obd.OBD()
            print(self.connection.status())
            res = self.connection.status()
            if res == obd.OBDStatus.CAR_CONNECTED:
                print("----------Connection establishment is successful!----------")
                return res
            else:
                print("----------Connection establishment failed!----------")
                print("End program. Please check settings of the computer and ELM327")
        except Exception as e:
            print("----------Exception!----------")
            print(e)
        finally:
            return res

    def get_data_from_sensor(self):
        """
        Retrieve data from the sensor.

        Returns:
            dict: A dictionary containing sensor data.
        """
        if self.is_offline:
            data = self.get_data_from_sensor_stub()
        else:
            # Retrieve data and save it in dictionary format
            data = {column: self.get_obd2_value(column) for column in self.COLUMNS}
        return data

    def get_obd2_value_debug(self, column):
        """
        Retrieve OBD-II value for a specific column with debug information.

        Args:
            column (str): The OBD-II command column.

        Returns:
            float or None: The value of the OBD-II command, or None if not available.
        """
        command = getattr(obd.commands, column, None)
        if command:
            response = self.connection.query(command)
            if response:
                print(f"Response for command '{command}': {response}")
                if response.value is not None:  # Check for None
                    print(f"Response value for command '{command}': {response.value}")
                    return response.value.magnitude
                else:
                    print(f"No value in response for command '{command}'")
            else:
                print(f"No response for command '{command}'")
        else:
            print(f"No command found for column '{column}'")
        return None

    def get_obd2_value(self, column):
        """
        Retrieve OBD-II value for a specific column.

        Args:
            column (str): The OBD-II command column.

        Returns:
            float or None: The value of the OBD-II command, or None if not available.
        """
        command = getattr(obd.commands, column, None)
        if command:
            response = self.connection.query(command)
            if response.value is not None:  # Check for None
                return response.value.magnitude
        return None

    def get_data_from_sensor_stub(self):
        """
        Generate stub data for the sensor.

        Returns:
            dict: A dictionary containing stub sensor data.
        """
        data_stub = {column: np.abs(np.random.randn()).astype(np.float32).item() for column in self.COLUMNS}        
        # Randomly insert None or 0.0
        if random.choice([True, False]):
            random_column = random.choice(self.COLUMNS)
            if random.choice([True, False]):
                data_stub[random_column] = None
            else:
                data_stub[random_column] = 0.0

        return data_stub

def format_data_for_display(data, labels):
    """
    Format sensor data for display.

    Args:
        data (dict): The sensor data to format.
        labels (list of str): The list of labels to include in the formatted string.

    Returns:
        str: A formatted string containing the sensor data.
    """
    formatted_str = ""
    for label, value in zip(labels, data.values()):
        if value is None:
            value = "None"
        else:
            value = f"{value:.4f}"
        formatted_str += f"{label}: {value} / "
    return formatted_str.rstrip(" / ")

def format_sensor_data(data, labels):
    """
    Format sensor data for display.

    Args:
        data (dict or list): The sensor data to format.
        labels (list of str): The list of labels to include in the formatted string.

    Returns:
        str: A formatted string containing the sensor data.
    """
    formatted_str = ""
    if isinstance(data, dict):
        for label in labels:
            value = data.get(label, None)
            if value is None:
                value = "None"
            else:
                value = f"{value:.4f}"
            formatted_str += f"{label}: {value} / "
    else:
        for label, value in zip(labels, data):
            if value is None:
                value = "None"
            else:
                value = f"{value:.4f}"
            formatted_str += f"{label}: {value} / "
    return formatted_str.rstrip(" / ")

def test_main():
    """
    Main function for testing sensor data collection and display.

    This function initializes the ELM327 sensor, starts a loop to collect data,
    formats the data for display, and prints it in real-time. It also calculates
    and prints the sampling delay and reliability rate upon termination.

    The main loop runs until interrupted by the user.

    Raises:
        KeyboardInterrupt: If a keyboard interrupt occurs, the loop is terminated
                           and the final statistics are printed.
    """
    from utils.tools import wait_process
    from time import perf_counter
    import matplotlib.pyplot as plt

    print("Main start")
    config = load_config(config_path)
    meas_elm327 = ELM327(config.sensors['elm327'])
    # res = meas_elm327.connect_to_elm327()

    start_time = perf_counter()
    sampling_counter = 0
    try:
        main_loop_start_time = perf_counter()
        while True:
            iteration_start_time = perf_counter()

            # Data acquisition process
            data = meas_elm327.get_data_from_sensor()

            current_time = perf_counter() - start_time
            sampling_counter += 1

            if meas_elm327.Is_show_real_time_data:
                formatted_data = format_sensor_data(data, meas_elm327.COLUMNS)
                print("--------------------------------------------------------------------")
                print("Current Time is: {:.3f}".format(current_time))
                print(formatted_data)

            # Wait to meet the sampling frequency based on the sampling interval and execution time
            elapsed_time = perf_counter() - iteration_start_time
            sleep_time = meas_elm327.SAMPLING_TIME - elapsed_time
            if sleep_time > 0:
                wait_process(sleep_time)

    except KeyboardInterrupt:
        print("Interrupted by user")

    finally:
        main_loop_end_time = perf_counter() - main_loop_start_time
        print("Program terminated")
        print("main loop is ended. current time is: {:.3f}".format(current_time))
        print("main loop is ended. end time is: {:.3f}".format(main_loop_end_time))
        print("sampling num is: {}".format(sampling_counter))

        # Calculate the ideal sampling time
        ideal_time = ((sampling_counter - 1) / meas_elm327.SAMPLING_FREQUENCY_HZ)
        # Calculate the delay
        delay_time = current_time - ideal_time
        # The reliability rate is the delay divided by the sampling time
        sampling_reliability_rate = (delay_time / (sampling_counter / meas_elm327.SAMPLING_FREQUENCY_HZ)) * 100

        print("sampling delay is: {:.3f} s".format(delay_time))
        print("sampling delay rate is: {:.3f} %".format(sampling_reliability_rate))

if __name__ == '__main__':
    test_main()

This script defines an ELM327 class for interfacing with an ELM327 device to collect sensor data via OBD-II. It initializes the connection, retrieves data, and formats it for display. The test_main function sets up the ELM327 sensor, collects data in a loop, and prints it in real-time. It also calculates and displays sampling delay and reliability rates upon interruption. The script handles both real-time data acquisition and simulated offline data for testing purposes.

7. Conclusion

In this post, I described the project focused on developing a measurement system to capture vehicle behavior. While the project is still ongoing, I have recognized the potential for capturing vehicle data with a focus on the vehicle dynamics domain. However, when it comes to measuring signals from vehicle ECUs, some limitations need to be considered, as the ELM327 may not reliably achieve sampling rates beyond 5 Hz with good accuracy. This issue could likely be resolved by using a more advanced OBD scanner, such as the OBDLink MX+.

8. Reference

[1] Adafruit. "Adafruit BNO055 Absolute Orientation Sensor."
https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/overview. Accessed September 3, 2024.

[2] Adafruit. "Adafruit_CircuitPython_BNO055." https://github.com/adafruit/Adafruit_CircuitPython_BNO055. Accessed September 3, 2024.

[3] Adafruit. "Adafruit_BNO055." https://github.com/adafruit/Adafruit_BNO055. Accessed September 3, 2024.

[4] Adafruit. "Adafruit BNO055 Demo." https://cdn-shop.adafruit.com/product-videos/1024x768/2472-04.mp4 Accessed September 3, 2024.

[5] Amazon. "Elm327 Launchh OBD2 Professional Bluetooth Scan Tool and Code Reader for Android and PC, Interface OBDII OBD2 Car Auto Diagnostic Scanner, Not Support J1850 VPW & J1850 PWM". https://a.co/d/5BFn4GN. Accessed September 3, 2024.

[6] MathWorks. "Coordinate Systems in Automated Driving Toolbox." https://www.mathworks.com/help/driving/ug/coordinate-systems.html. Accessed September 3, 2024.

[7] Apple. "Getting raw gyroscope events." https://developer.apple.com/documentation/coremotion/getting_raw_gyroscope_events. Accessed September 3, 2024.

[8] phyphox. "phyphox top page."https://phyphox.org/. Accessed September 3, 2024.

[9] Andrea Patrucco, Vehicle dynamics engineer presso Applus+ IDIADA. "Challenging your car's trip computer fuel efficiency figures." https://www.linkedin.com/pulse/challenging-your-cars-trip-computer-fuel-efficiency-figures-patrucco/. Accessed September 3, 2024.

[10] "Choosing OBDII adapter". https://www.carscanner.info/choosing-obdii-adapter/. Accessed September 3, 2024.

[11] "VDDM". https://github.com/Qooniee/VDDM/tree/master/. Accessed September 3, 2024.

Ce qui précède est le contenu détaillé de. pour plus d'informations, suivez d'autres articles connexes sur le site Web de PHP en chinois!