WIP IMU

chef/Makefile

@@ -11,7 +11,7 @@ CFLAGS_CUSTOM += -g
 ## Générateurs de drapeaux pour les bibliothèques
 PKG_CONFIG=pkg-config
 ## Nom des objets communs
-OBJS=buttons CF debug diagnostics i2c ihm lcd movement parcours points position
+OBJS=buttons CF debug diagnostics i2c imu ihm lcd movement parcours points position
 OBJS_O=$(addprefix obj/,$(addsuffix .o,$(OBJS)))
 
 # VARIABLES AUTOMATIQUES

chef/src/CF.c

@@ -27,7 +27,6 @@ void configureFpga()
     printf("Connexion à %s... ", FPGA_PORTNAME);
     fflush(stdout);
     fpga = open(FPGA_PORTNAME, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
-    fpga = serialOpen(FPGA_PORTNAME, 9600);
     if (fpga < 0) {
         printf("Échec !\n");
         exit(1);

chef/src/debug.c

@@ -51,9 +51,6 @@ void* TaskDebug(void* pdata)
 {
     (void)pdata;
 
-    if (DEBUG_INTERVAL <= 0) {
-        return NULL;
-    }
     clock_gettime(CLOCK_REALTIME, &debugStart);
 
     fprintf(debugFd, "\n");

chef/src/diagnostics.c

@@ -1,4 +1,5 @@
 #include <unistd.h>
+#include <stdio.h>
 
 #include "diagnostics.h"
 #include "buttons.h"
@@ -6,6 +7,7 @@
 
 #include "CF.h"
 #include "movement.h"
+#include "imu.h"
 
 bool recu;
 
@@ -71,6 +73,12 @@ bool diagCodeuse(void* arg)
     }
 }
 
+bool diagIMU(void* arg)
+{
+    (void) arg;
+    return connectedIMU();
+}
+
 void execDiagnostic(char *name, bool (*diagnostic)(void* arg), void* arg)
 {
     clearLCD();
@@ -90,6 +98,7 @@ void runDiagnostics()
 {
     execDiagnostic("Lien FPGA", diagFPGA, NULL);
     /* execDiagnostic("Lien Arduino", diagArduino); */
+    execDiagnostic("Lien IMU", diagIMU, NULL);
     int i;
     i = 0; execDiagnostic("Mot+Cod L AV", diagCodeuse, &i);
     i = 1; execDiagnostic("Mot+Cod L AR", diagCodeuse, &i);

chef/src/imu.c

@@ -0,0 +1,34 @@
+#include "imu.h"
+#include "i2c.h"
+
+int gyroFd;
+int accelFd;
+
+void configureIMU()
+{
+    gyroFd = openI2C(GYRO_ADDR);
+    accelFd = openI2C(ACCEL_ADDR);
+
+    writeI2C(gyroFd, GYRO_LOW_ODR, 0x00); // Low_ODR = 0 (low speed ODR disabled)
+    writeI2C(gyroFd, GYRO_CTRL_REG4, 0x00); // FS = 00 (+/- 250 dps full scale)
+    writeI2C(gyroFd, GYRO_CTRL_REG1, 0x6F); // DR = 01 (200 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
+}
+
+bool connectedIMU()
+{
+    return (uint8_t) readI2C(gyroFd, GYRO_WHO_AM_I) == GYRO_IDENTITY;
+}
+
+struct gyroRaw readGyroRaw()
+{
+    struct gyroRaw res;
+    res.x = (readI2C(gyroFd, GYRO_OUT_X_H) << 8 | readI2C(gyroFd, GYRO_OUT_X_L));
+    res.y = (readI2C(gyroFd, GYRO_OUT_Y_H) << 8 | readI2C(gyroFd, GYRO_OUT_Y_L));
+    res.z = (readI2C(gyroFd, GYRO_OUT_Z_H) << 8 | readI2C(gyroFd, GYRO_OUT_Z_L));
+    return res;
+}
+
+void deconfigureIMU()
+{
+
+}

chef/src/imu.h

@@ -0,0 +1,76 @@
+#ifndef __IMU_H_
+#define __IMU_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define GYRO_ADDR 0x6b
+#define ACCEL_ADDR 0x1d
+
+// Gyro
+
+#define GYRO_WHO_AM_I 0x0F
+#define GYRO_IDENTITY 0xD7
+
+#define GYRO_CTRL1 0x20 // D20H
+#define GYRO_CTRL_REG1 0x20 // D20, 4200D
+#define GYRO_CTRL2 0x21 // D20H
+#define GYRO_CTRL_REG2 0x21 // D20, 4200D
+#define GYRO_CTRL3 0x22 // D20H
+#define GYRO_CTRL_REG3 0x22 // D20, 4200D
+#define GYRO_CTRL4 0x23 // D20H
+#define GYRO_CTRL_REG4 0x23 // D20, 4200D
+#define GYRO_CTRL5 0x24 // D20H
+#define GYRO_CTRL_REG5 0x24 // D20, 4200D
+#define GYRO_REFERENCE 0x25
+#define GYRO_OUT_TEMP 0x26
+#define GYRO_STATUS 0x27 // D20H
+#define GYRO_STATUS_REG 0x27 // D20, 4200D
+
+#define GYRO_OUT_X_L 0x28
+#define GYRO_OUT_X_H 0x29
+#define GYRO_OUT_Y_L 0x2A
+#define GYRO_OUT_Y_H 0x2B
+#define GYRO_OUT_Z_L 0x2C
+#define GYRO_OUT_Z_H 0x2D
+
+#define GYRO_FIFO_CTRL 0x2E // D20H
+#define GYRO_FIFO_CTRL_REG 0x2E // D20, 4200D
+#define GYRO_FIFO_SRC 0x2F // D20H
+#define GYRO_FIFO_SRC_REG 0x2F // D20, 4200D
+
+#define GYRO_IG_CFG 0x30 // D20H
+#define GYRO_INT1_CFG 0x30 // D20, 4200D
+#define GYRO_IG_SRC 0x31 // D20H
+#define GYRO_INT1_SRC 0x31 // D20, 4200D
+#define GYRO_IG_THS_XH 0x32 // D20H
+#define GYRO_INT1_THS_XH 0x32 // D20, 4200D
+#define GYRO_IG_THS_XL 0x33 // D20H
+#define GYRO_INT1_THS_XL 0x33 // D20, 4200D
+#define GYRO_IG_THS_YH 0x34 // D20H
+#define GYRO_INT1_THS_YH 0x34 // D20, 4200D
+#define GYRO_IG_THS_YL 0x35 // D20H
+#define GYRO_INT1_THS_YL 0x35 // D20, 4200D
+#define GYRO_IG_THS_ZH 0x36 // D20H
+#define GYRO_INT1_THS_ZH 0x36 // D20, 4200D
+#define GYRO_IG_THS_ZL 0x37 // D20H
+#define GYRO_INT1_THS_ZL 0x37 // D20, 4200D
+#define GYRO_IG_DURATION 0x38 // D20H
+#define GYRO_INT1_DURATION 0x38 // D20, 4200D
+
+#define GYRO_LOW_ODR 0x39 // D20H
+
+// Structures
+
+struct gyroRaw {
+    int16_t x;
+    int16_t y;
+    int16_t z;
+};
+
+void configureIMU();
+bool connectedIMU();
+void deconfigureIMU();
+struct gyroRaw readGyroRaw();
+
+#endif

chef/src/premier.c

@@ -10,6 +10,7 @@
 #include "debug.h"
 #include "i2c.h"
 #include "ihm.h"
+#include "imu.h"
 #include "movement.h"
 #include "position.h"
 
@@ -18,7 +19,6 @@ pthread_mutex_t sRunning;
 void endRunning(int signal)
 {
     (void)signal;
-    printf("21\n");
     pthread_mutex_unlock(&sRunning);
 }
 
@@ -32,9 +32,10 @@ int main()
     initI2C();
     srand(time(NULL));
 
-    configureIHM();
     configureDebug();
+    configureIHM();
     configureCF();
+    configureIMU();
     configureMovement();
     configurePosition();
     startDebug();
@@ -48,11 +49,11 @@ int main()
     pthread_mutex_lock(&sRunning);
     pthread_mutex_lock(&sRunning);
 
-    deconfigureMovement();
-    deconfigureMovement();
     deconfigurePosition();
+    deconfigureMovement();
+    deconfigureIMU();
     deconfigureCF();
-    deconfigureDebug();
     deconfigureIHM();
+    deconfigureDebug();
     return EXIT_SUCCESS;
 }

chef/src/testIMU.c

@@ -0,0 +1,37 @@
+/* Teste l'IMU */
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <wiringPiI2C.h>
+
+#include "i2c.h"
+#include "imu.h"
+
+int main(int argc, char* argv[])
+{
+
+    (void)argc;
+    (void)argv;
+
+    initI2C();
+    configureIMU();
+    if (!connectedIMU()) {
+        printf("IMU not connected\n");
+        exit(1);
+    }
+
+    struct gyroRaw a;
+    struct gyroRaw t;
+    for (;;) {
+        a = readGyroRaw();
+        t.x += a.x;
+        t.y += a.y;
+        t.z += a.z;
+        usleep(100*1000);
+        printf("X:%5d Y:%5d Z:%5d\n", t.x, t.y, t.z);
+    }
+
+    exit(0);
+}

chef/src/testpin.c

@@ -1,32 +0,0 @@
-/* Teste si une broche est connecté à une autre */
-
-#include <stdlib.h>
-#include <stdio.h>
-#include <wiringPi.h>
-
-char testPin(char input, char output) {
-    pinMode(output, OUTPUT);
-    pinMode(input, INPUT);
-    digitalWrite(output, 1);
-    delay(0);
-    return digitalRead(input);
-}
-
-int main(int argc, char *argv[]) {
-
-    if (argc != 3) {
-        printf("Usage: %s INPUT_PIN OUTPUT_PIN\n", argv[0]);
-        return 2;
-    }
-
-    if (wiringPiSetup() == -1) {
-        return 3;
-    }
-
-    char input = atoi(argv[1]);
-    char output = atoi(argv[2]);
-
-    char rep = testPin(input, output);
-
-    return rep;
-}

raspberrypi/Makefile

@@ -112,7 +112,6 @@ chef:
 	make -C buildroot chef-rebuild
 
 upgrade-chef: chef
-	make -C buildroot chef-reinstall
 	ssh -F sshconf principal true
 	rsync --rsh 'ssh -F sshconf' --archive --chown root:root buildroot/output/target/opt/chef principal:/opt/
 

raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/etc/init.d/S30hardware

@@ -6,17 +6,16 @@
 start() {
 	printf "Starting hardware handling: "
 	modprobe pl2303 # USB↔Serial cable
-	modprobe i2c-bcm2708 # I2C
+	# I2C
-	modprobe i2c-dev # I2C
+	modprobe i2c-bcm2708 # RPi2
+	modprobe i2c-bcm2835 # RPi3
+	modprobe i2c-dev # Both
 	/opt/chef/lcdOff.sh
 	echo "OK"
 }
 
 stop() {
 	printf "Stopping hardware handling: "
-	rmmod i2c-dev # I2C
-	rmmod i2c-bcm2708 # I2C
-	rmmod pl2303 # USB↔Serial cable
 	echo "OK"
 }
 

raspberrypi/configs/cdfprincipal_defconfig

@@ -70,7 +70,6 @@ BR2_PACKAGE_WPA_SUPPLICANT=y
 # Pour upgrader à chaud
 BR2_PACKAGE_DROPBEAR=y
 BR2_PACKAGE_RSYNC=y
-BR2_PACKAGE_PPPD=y
 
 # Pour faire plaisir à Geoffrey
 BR2_PACKAGE_HTOP=y