Added code for gyro, still some huge bugs

main controller code/.vscode/settings.json

@@ -1,8 +1,10 @@
 {
     "files.associations": {
+        "*.md": "markdown",
         "binary_info.h": "c",
         "i2c.h": "c",
         "stdlib.h": "c",
-        "stdint.h": "c"
+        "stdint.h": "c",
+        "gyro.h": "c"
     }
 }

main controller code/src/gyro.c

@@ -1 +1,104 @@
-#include "include/gyro.h"
+#include "include/gyro.h"
+
+#include <stdint.h>
+#include "include/i2c_master.h"
+#include "include/robot.h"
+
+/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
+* Thank you Keuronde !                                                                              *
+*   https://git.poivron-robotique.fr/Keuronde/Holonome_2024/src/branch/Demo_2025_03/gyro_L3GD20H.c  *
+\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
+#define DPS_PER_DIGIT 0.00875f
+
+#define SAMPLE_MIN_ELAPSED_TIME 10 // ms
+
+int init_gyro(void)
+{
+    // Verify gyro initialisation
+    uint8_t data;
+    i2c_master_read_reg(I2C_GYRO_ADDRESS, 0x0F, &data, 1);
+
+    if(data != 0xd7) return -1;
+
+    // Configure gyro
+    const uint8_t CTRL1_REG = 0x20;
+    const uint8_t CTRL1_CONFIG = 0b11101111; // DR : 11 // BW : 10 // PD : 1 // Zen : 1 // Xen : 1 // Yen : 1 //
+
+    uint8_t config[] = {CTRL1_REG, CTRL1_CONFIG};
+    uint8_t config_verification;
+
+    i2c_master_write(I2C_GYRO_ADDRESS, config, 2);
+    i2c_master_read_reg(I2C_GYRO_ADDRESS, CTRL1_REG, &config_verification, 1);
+
+    if(config_verification != config[1]) return -1;
+
+    return 0;
+}
+
+static inline void __attribute__((always_inline)) gyro_read(int16_t *x, int16_t *y, int16_t *z)
+{
+    const uint8_t X_OUT_L_REG = 0x28;
+
+    uint8_t data[6];
+    i2c_master_read_reg(I2C_GYRO_ADDRESS, X_OUT_L_REG | 0x80, data, 6); // 0x80 for auto incrementing
+
+    *x = (int16_t)((data[1] << 8) | data[0]);
+    *y = (int16_t)((data[3] << 8) | data[2]);
+    *z = (int16_t)((data[5] << 8) | data[4]);
+}
+
+void gyro_calibrate(void)
+{
+    const uint nb_samples = 100;
+
+    int16_t x, y, z;
+    int32_t x_sum = 0, y_sum = 0, z_sum = 0;
+
+    for(uint i = 0; i < nb_samples; i++)
+    {
+        gyro_read(&x, &y, &z);
+        x_sum += x;
+        sleep_ms(SAMPLE_MIN_ELAPSED_TIME);
+    }
+
+    robot.gyro_data.x_offset = (float)x_sum / (float)nb_samples * DPS_PER_DIGIT;
+    robot.gyro_data.y_offset = (float)y_sum / (float)nb_samples * DPS_PER_DIGIT;
+    robot.gyro_data.z_offset = (float)z_sum / (float)nb_samples * DPS_PER_DIGIT;
+    //robot.gyro_data.x_offset = 0.0;
+    //robot.gyro_data.y_offset = 0.0;
+    //robot.gyro_data.z_offset = 0.0;
+}
+
+static inline void __attribute__((always_inline)) gyro_get_dps(double* x_dps, double* y_dps, double* z_dps)
+{
+    int16_t x, y, z;
+    gyro_read(&x, &y, &z);
+
+    *x_dps = x * DPS_PER_DIGIT;
+    *y_dps = y * DPS_PER_DIGIT;
+    *z_dps = z * DPS_PER_DIGIT;
+}
+
+void gyro_update(void)
+{
+    static double elapsed_since_sample_ms = 10.0;
+
+    elapsed_since_sample_ms += robot.delta_time_ms;
+
+    if(elapsed_since_sample_ms >= SAMPLE_MIN_ELAPSED_TIME)
+    {
+        double x_dps, y_dps, z_dps;
+        gyro_get_dps(&x_dps, &y_dps, &z_dps);
+
+        x_dps -= robot.gyro_data.x_offset;
+        y_dps -= robot.gyro_data.y_offset;
+        z_dps -= robot.gyro_data.z_offset;
+
+        robot.gyro_data.x_angle += x_dps * elapsed_since_sample_ms * 1000.0f;
+        robot.gyro_data.y_angle += y_dps * elapsed_since_sample_ms * 1000.0f;
+        robot.gyro_data.z_angle += z_dps * elapsed_since_sample_ms * 1000.0f;
+
+        elapsed_since_sample_ms = 0.0;
+    }
+}

main controller code/src/include/gyro.h

@@ -3,4 +3,16 @@
 
 #define I2C_GYRO_ADDRESS 0x6b
 
+typedef struct gyro_data_t {
+    float x_offset, y_offset, z_offset;
+    float x_angle, y_angle, z_angle;
+} gyro_data_t;
+
+// Check if gyro has correctly initialised and configure it for simple use
+int init_gyro(void);
+// Calibrate gyro
+void gyro_calibrate(void);
+// Update gyro data
+void gyro_update(void);
+
 #endif // GYRO_H

main controller code/src/include/robot.h

@@ -2,8 +2,11 @@
 #define ROBOT_H
 
 #include <stdbool.h>
+#include "gyro.h"
 
 typedef struct robot_t {
+    gyro_data_t gyro_data;
+
     bool is_running;
     double delta_time_ms;
 } robot_t;
@@ -11,7 +14,7 @@ typedef struct robot_t {
 extern robot_t robot;
 
 // Init all robot's components
-void robot_init(void);
+int robot_init(void);
 // Handle inputs and outputs
 void robot_handle_inputs_outputs(void);
 // Deinit all robot's components

main controller code/src/main.c

@@ -15,7 +15,7 @@ robot_t robot;
 
 int main(void)
 {
-    robot_init();
+    if(robot_init()) return -1;
 
     while(robot.is_running)
     {

main controller code/src/robot.c

@@ -1,16 +1,45 @@
 #include "include/robot.h"
 
 #include <pico/stdlib.h>
+#include <time.h>
+#include "include/i2c_master.h"
+#include "include/gyro.h"
 
-void robot_init(void)
+#include <stdio.h>
+
+int robot_init(void)
 {
     stdio_init_all();
 
+    sleep_ms(5000);
+
+    i2c_master_init();
+
+    if(init_gyro()) return -1;
+    gyro_calibrate();
+
     robot.is_running = true;
+
+    return 0;
+}
+
+static inline void update_time(void)
+{
+    static clock_t last_clock_state = 0;
+    clock_t start_clock = clock();
+    robot.delta_time_ms = (double)(start_clock - last_clock_state) * 1000.0 / CLOCKS_PER_SEC;
+    last_clock_state = start_clock;
 }
 
 void robot_handle_inputs_outputs(void)
 {
+    update_time();
+
+    gyro_update();
+
+    printf(">gyro_x:%f\n", robot.gyro_data.x_angle);
+    printf(">gyro_y:%f\n", robot.gyro_data.y_angle);
+    printf(">gyro_z:%f\n", robot.gyro_data.z_angle);
 }
 
 void robot_deinit(void)