From 678b7e939b548d2620cbe7c3365c50be3b39e642 Mon Sep 17 00:00:00 2001 From: Geoffrey Frogeye Date: Sat, 5 May 2018 15:56:39 +0200 Subject: [PATCH] WIP IMU --- chef/Makefile | 2 +- chef/src/CF.c | 1 - chef/src/debug.c | 3 - chef/src/diagnostics.c | 9 +++ chef/src/imu.c | 34 +++++++++ chef/src/imu.h | 76 +++++++++++++++++++ chef/src/premier.c | 11 +-- chef/src/testIMU.c | 37 +++++++++ chef/src/testpin.c | 32 -------- chef/src/testpin.h | 0 raspberrypi/Makefile | 1 - .../rootfs_overlay/etc/init.d/S30hardware | 9 +-- raspberrypi/configs/cdfprincipal_defconfig | 1 - 13 files changed, 167 insertions(+), 49 deletions(-) create mode 100644 chef/src/imu.c create mode 100644 chef/src/imu.h create mode 100644 chef/src/testIMU.c delete mode 100644 chef/src/testpin.c delete mode 100644 chef/src/testpin.h diff --git a/chef/Makefile b/chef/Makefile index 71efb31..1747ef6 100644 --- a/chef/Makefile +++ b/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 diff --git a/chef/src/CF.c b/chef/src/CF.c index 3e3a551..f5f79e1 100644 --- a/chef/src/CF.c +++ b/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); diff --git a/chef/src/debug.c b/chef/src/debug.c index e2f55a2..d8b58a7 100644 --- a/chef/src/debug.c +++ b/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"); diff --git a/chef/src/diagnostics.c b/chef/src/diagnostics.c index ddf1f28..445f2ad 100644 --- a/chef/src/diagnostics.c +++ b/chef/src/diagnostics.c @@ -1,4 +1,5 @@ #include +#include #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); diff --git a/chef/src/imu.c b/chef/src/imu.c new file mode 100644 index 0000000..339a256 --- /dev/null +++ b/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() +{ + +} diff --git a/chef/src/imu.h b/chef/src/imu.h new file mode 100644 index 0000000..49c8149 --- /dev/null +++ b/chef/src/imu.h @@ -0,0 +1,76 @@ +#ifndef __IMU_H_ +#define __IMU_H_ + +#include +#include + +#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 diff --git a/chef/src/premier.c b/chef/src/premier.c index ac3503b..4d11a8c 100644 --- a/chef/src/premier.c +++ b/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; } diff --git a/chef/src/testIMU.c b/chef/src/testIMU.c new file mode 100644 index 0000000..8855c11 --- /dev/null +++ b/chef/src/testIMU.c @@ -0,0 +1,37 @@ +/* Teste l'IMU */ + +#include +#include +#include +#include +#include + +#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); +} diff --git a/chef/src/testpin.c b/chef/src/testpin.c deleted file mode 100644 index 1bc3fd2..0000000 --- a/chef/src/testpin.c +++ /dev/null @@ -1,32 +0,0 @@ -/* Teste si une broche est connecté à une autre */ - -#include -#include -#include - -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; -} diff --git a/chef/src/testpin.h b/chef/src/testpin.h deleted file mode 100644 index e69de29..0000000 diff --git a/raspberrypi/Makefile b/raspberrypi/Makefile index 015b9ac..4f9117e 100644 --- a/raspberrypi/Makefile +++ b/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/ diff --git a/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/etc/init.d/S30hardware b/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/etc/init.d/S30hardware index d463989..68d281b 100755 --- a/raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/etc/init.d/S30hardware +++ b/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 - modprobe i2c-dev # I2C + # 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" } diff --git a/raspberrypi/configs/cdfprincipal_defconfig b/raspberrypi/configs/cdfprincipal_defconfig index 39bfc4c..215c387 100644 --- a/raspberrypi/configs/cdfprincipal_defconfig +++ b/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