Consequences of changing i2c_master

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/i2c_master.c

@@ -1,17 +1,31 @@
 #include "include/i2c_master.h"
 
-inline void i2c_master_send(uint8_t address, const uint8_t *src, size_t len)
+#include <pico/stdlib.h>
+#include <hardware/i2c.h>
+
+void i2c_master_init(void)
 {
-    i2c_write_blocking_until(I2C_MASTER_INSTANCE, address, src, len, false, I2C_MASTER_MAX_TIMEOUT);
+    gpio_set_function(I2C_MASTER_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(I2C_MASTER_SCL_PIN, GPIO_FUNC_I2C);
+
+    gpio_pull_up(I2C_MASTER_SDA_PIN);
+    gpio_pull_up(I2C_MASTER_SCL_PIN);
+
+    i2c_init(I2C_MASTER_INSTANCE, I2C_MASTER_BAUD_RATE);
 }
 
-inline void i2c_master_receive(uint8_t address, uint8_t *dst, size_t len)
+inline void i2c_master_write(uint8_t address, const uint8_t *src, size_t len)
 {
-    i2c_read_blocking_until(I2C_MASTER_INSTANCE, address, dst, len, false, I2C_MASTER_MAX_TIMEOUT);
+    i2c_write_blocking(I2C_MASTER_INSTANCE, address, src, len, false);
 }
 
-inline void i2c_master_send_receive(uint8_t address, uint8_t *data, size_t len)
+inline void i2c_master_read(uint8_t address, uint8_t *dst, size_t len)
 {
-    i2c_write_blocking_until(I2C_MASTER_INSTANCE, address, data, len, true, I2C_MASTER_MAX_TIMEOUT);
+    i2c_read_blocking(I2C_MASTER_INSTANCE, address, dst, len, false);
-    i2c_master_receive(address, data, len);
+}
+
+inline void i2c_master_read_reg(uint8_t address, uint8_t reg, uint8_t *dst, size_t len)
+{
+    i2c_master_write(address, &reg, 1);
+    i2c_master_read(address, dst, len);
 }

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/i2c_master.h

@@ -6,13 +6,15 @@
 #define I2C_MASTER_SDA_PIN 4
 #define I2C_MASTER_SCL_PIN 5
 #define I2C_MASTER_INSTANCE i2c0
-#define I2C_MASTER_MAX_TIMEOUT 10000
+#define I2C_MASTER_BAUD_RATE 100 * 1000
 
+// Init master i2c
+void i2c_master_init(void);
 // Send [src] of [len] to [address] and close communication
-void i2c_master_send(uint8_t address, const uint8_t *src, size_t len);
+void i2c_master_write(uint8_t address, const uint8_t *src, size_t len);
 // Receive [dst] of [len] from [address] and close communication
-void i2c_master_receive(uint8_t address, uint8_t *dst, size_t len);
+void i2c_master_read(uint8_t address, uint8_t *dst, size_t len);
-// Send [data] of [len] to [address], receive [data] of [len] from [address] and close communication
+// Send [reg] and receive data in [dst] and close communition
-void i2c_master_send_receive(uint8_t address, uint8_t *data, size_t len);
+void i2c_master_read_reg(uint8_t address, uint8_t reg, uint8_t *dst, size_t len);
 
 #endif // I2C_MASTER_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/motors.c

@@ -22,19 +22,19 @@ void i2c_set_motor_speed(motors_enum_t motor, int8_t value)
     const motor_def_t *motor_def = &MOTORS_DEFS[motor];
 
     uint8_t x = *(uint8_t *)&value;
-    uint8_t data[2] = {motor_def->buffer_address, x};
+    uint8_t data[] = {motor_def->buffer_address, x};
 
-    i2c_master_send(I2C_MOTION_CONTROLLER_ADDRESS, data, 2);
+    i2c_master_write(I2C_MOTION_CONTROLLER_ADDRESS, data, 2);
 }
 
 int8_t i2c_get_motor_speed(motors_enum_t motor)
 {
     const motor_def_t *motor_def = &MOTORS_DEFS[motor];
 
-    uint8_t data[1] = {motor_def->buffer_address};
+    uint8_t reg = motor_def->buffer_address; // Register of the i2c_buffer to read
+    uint8_t data;
+    i2c_master_read_reg(I2C_MOTION_CONTROLLER_ADDRESS, reg, &data, 1);
 
-    i2c_master_send_receive(I2C_MOTION_CONTROLLER_ADDRESS, data, 1);
+    int8_t value = *(int8_t *)&data;
-
-    int8_t value = *(int8_t *)data;
     return value;
 }

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)

motion controller code/src/i2c_slave.c

@@ -70,9 +70,6 @@ void i2c_slave_init(void)
     gpio_set_function(I2C_SLAVE_SDA_PIN, GPIO_FUNC_I2C);
     gpio_set_function(I2C_SLAVE_SCL_PIN, GPIO_FUNC_I2C);
 
-    gpio_pull_up(I2C_SLAVE_SDA_PIN);
-    gpio_pull_up(I2C_SLAVE_SCL_PIN);
-
     // Note: The I2C slave does clock stretching implicitly after a RD_REQ, while the Tx FIFO is empty.
     // There is also an option to enable clock stretching while the Rx FIFO is full, but we leave it
     // disabled since the Rx FIFO should never fill up (unless i2c_slave.handler() is way too slow).