From 8b3ff9feba2360e5443b7796bc3af54208da1709 Mon Sep 17 00:00:00 2001 From: Gavan Fantom Date: Sun, 23 Nov 2014 18:54:11 +0000 Subject: [PATCH] Add preliminary support for HMC5883L magnetometer and MPL3115A2 altimeter. Logging only, at this stage. --- Makefile | 3 +- event.h | 8 ++-- hmc5883l.c | 129 ++++++++++++++++++++++++++++++++++++++++++++++++++ hmc5883l.h | 10 ++++ log.c | 4 +- matrix.c | 4 ++ mpl3115a2.c | 132 ++++++++++++++++++++++++++++++++++++++++++++++++++++ mpl3115a2.h | 10 ++++ mpu6050.c | 12 ++++- sensors.c | 20 ++++++-- stick.c | 4 ++ 11 files changed, 327 insertions(+), 9 deletions(-) create mode 100644 hmc5883l.c create mode 100644 hmc5883l.h create mode 100644 mpl3115a2.c create mode 100644 mpl3115a2.h diff --git a/Makefile b/Makefile index 2441d81..3c4bb9c 100644 --- a/Makefile +++ b/Makefile @@ -5,10 +5,11 @@ NAME=quad SSRCS=crt0.s CSRCS=main.c i2c.c mpu6050.c timer.c interrupt.c uart.c event.c matrix.c dcm.c CSRCS+=fisqrt.c stick.c trig.c motor.c led.c watchdog.c panic.c status.c -CSRCS+=thrust.c sensors.c spi.c sdcard.c log.c +CSRCS+=thrust.c sensors.c spi.c sdcard.c log.c hmc5883l.c mpl3115a2.c #PROJOPTS=-DUSE_UART -DSEND_DCM -DSTICK_DEBUG_CALIBRATE PROJOPTS=-DTIMER_CPPM -DI2C_FAST +#PROJOPTS=-DTIMER_CPPM -DI2C_FAST -DUSE_UART #PROJOPTS=-DTIMER_CPPM -DUSE_UART -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DSEND_DCM #PROJOPTS=-DTIMER_CPPM -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DSEND_DCM #PROJOPTS=-DTIMER_CPPM -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DUSE_UART -DSEND_DCM diff --git a/event.h b/event.h index f18cc24..dba73ce 100644 --- a/event.h +++ b/event.h @@ -5,10 +5,12 @@ #define EVENT_TIMER 0 #define EVENT_MPU6050_I2C_COMPLETE 1 -#define EVENT_UART_INPUT 2 -#define EVENT_SDCARD 3 +#define EVENT_HMC5883L_I2C_COMPLETE 2 +#define EVENT_MPL3115A2_I2C_COMPLETE 3 +#define EVENT_UART_INPUT 4 +#define EVENT_SDCARD 5 -#define EVENT_MAX 3 +#define EVENT_MAX 5 typedef void event_handler(void); diff --git a/hmc5883l.c b/hmc5883l.c new file mode 100644 index 0000000..1a2130d --- /dev/null +++ b/hmc5883l.c @@ -0,0 +1,129 @@ +/* hmc5883l.c */ + +#include "sensors.h" +#include "hmc5883l.h" +#include "i2c.h" +#include "event.h" +#include "log.h" +#include "uart.h" + +bool hmc5883l_state; + +i2c_result hmc5883l_result; + +unsigned char hmc5883l_sample_data[6]; + +#define LOG_SIGNATURE_MAGNETOMETER 0xDA7AAA75 + +unsigned char hmc5883l_init_command[] = {0x00, 0x70}; +unsigned char hmc5883l_init_command2[] = {0x01, 0xA0}; + +struct i2c_transaction hmc5883l_init_transaction2; + +struct i2c_transaction hmc5883l_init_transaction = { + (0x1E << 1) + 0, /* write */ + 2, + hmc5883l_init_command, + &hmc5883l_result, + EVENT_HMC5883L_I2C_COMPLETE, + &hmc5883l_init_transaction2 +}; + +struct i2c_transaction hmc5883l_init_transaction2 = { + (0x1E << 1) + 0, /* write */ + 2, + hmc5883l_init_command2, + &hmc5883l_result, + EVENT_HMC5883L_I2C_COMPLETE, + NULL +}; + +unsigned char hmc5883l_start_command[] = {0x02, 0x01}; + +struct i2c_transaction hmc5883l_start_transaction = { + (0x1E << 1) + 0, /* write */ + 2, + hmc5883l_start_command, + &hmc5883l_result, + EVENT_HMC5883L_I2C_COMPLETE, + NULL +}; + +struct i2c_transaction hmc5883l_sample_transaction = { + (0x1E << 1) + 1, /* read */ + 6, + hmc5883l_sample_data, + &hmc5883l_result, + EVENT_HMC5883L_I2C_COMPLETE, + NULL +}; + +void hmc5883l_event_handler(void); + +bool hmc5883l_init(void) +{ + event_register(EVENT_HMC5883L_I2C_COMPLETE, hmc5883l_event_handler); + + if (!i2c_start_transaction(&hmc5883l_init_transaction)) + return FALSE; + while (i2c_busy()) ; + + hmc5883l_state = 0; + + return (hmc5883l_result == I2C_SUCCESS); +} + +bool hmc5883l_start_sample(void) +{ + hmc5883l_state = !hmc5883l_state; + + if (hmc5883l_state) + return i2c_start_transaction(&hmc5883l_start_transaction); + else + return i2c_start_transaction(&hmc5883l_sample_transaction); +} + +void hmc5883l_event_handler(void) +{ +#if 0 + signed short int xi, yi, zi; + signed short int tempi; + signed short int rolli, pitchi, yawi; + + float x, y, z; + float temp; + float roll, pitch, yaw; +#endif + + if (hmc5883l_result != I2C_SUCCESS) + return; + + hmc5883l_result = I2C_IN_PROGRESS; + + sensors_sample_done(); + + /* If we just started a conversion, our job is done for now */ + if (hmc5883l_state) + return; + +#if 0 + xi = (hmc5883l_sample_data[0] << 8) + hmc5883l_sample_data[1]; + yi = (hmc5883l_sample_data[2] << 8) + hmc5883l_sample_data[3]; + zi = (hmc5883l_sample_data[4] << 8) + hmc5883l_sample_data[5]; + + tempi = (hmc5883l_sample_data[6] << 8) + hmc5883l_sample_data[7]; + + rolli = (hmc5883l_sample_data[ 8] << 8)+hmc5883l_sample_data[ 9]; + pitchi = (hmc5883l_sample_data[10] << 8)+hmc5883l_sample_data[11]; + yawi = (hmc5883l_sample_data[12] << 8)+hmc5883l_sample_data[13]; + + sensors_write_gyro_data(roll, pitch, yaw); + sensors_write_accel_data(x, y, z); + sensors_write_temp_data(temp); + +#endif + + log_put_uint(LOG_SIGNATURE_MAGNETOMETER); + log_put_array((char *)hmc5883l_sample_data, 6); +} + diff --git a/hmc5883l.h b/hmc5883l.h new file mode 100644 index 0000000..888e894 --- /dev/null +++ b/hmc5883l.h @@ -0,0 +1,10 @@ +#ifndef __HMC5883L_H +#define __HMC5883L_H + +#include "types.h" + +bool hmc5883l_init(void); +bool hmc5883l_start_sample(void); +void hmc5883l_write_log(void); + +#endif /* __HMC5883L_H */ diff --git a/log.c b/log.c index 3a33fb6..edbbc48 100644 --- a/log.c +++ b/log.c @@ -36,8 +36,10 @@ void log_put_byte(char c) /* If the buffer is full, we just discard data. * Better than overrunning. */ - if (((log_bufferend + 1) % LOG_BUFFERSIZE) == log_bufferstart) + if (((log_bufferend + 1) % LOG_BUFFERSIZE) == log_bufferstart) { + putstr("^"); return; + } log_buffer[log_bufferend++] = c; log_bufferend = log_bufferend % LOG_BUFFERSIZE; #if 0 diff --git a/matrix.c b/matrix.c index 29a4614..7352b04 100644 --- a/matrix.c +++ b/matrix.c @@ -19,6 +19,7 @@ void matrix_multiply(float *dest, float *m1, float *m2, int r, int c, int n) } } +#if 0 /* dest[r][c] = m1[r][n] * m2[c][n] */ void matrix_multiply_t(float *dest, float *m1, float *m2, int r, int c, int n) { @@ -33,6 +34,7 @@ void matrix_multiply_t(float *dest, float *m1, float *m2, int r, int c, int n) } } } +#endif /* dest[r][c] = m1[r][c] + m2[r][c] */ void matrix_add(float *dest, float *m1, float *m2, int r, int c) @@ -64,6 +66,7 @@ void dump_matrix(float *m, int r, int c) } #endif +#if 0 /* Invert src into dst. * NOTE: destroys the src matrix */ @@ -145,3 +148,4 @@ void matrix_invert(float *dst, float *src, int size) /* src should now be the identiy matrix while dst holds the answer */ } +#endif diff --git a/mpl3115a2.c b/mpl3115a2.c new file mode 100644 index 0000000..d9e968f --- /dev/null +++ b/mpl3115a2.c @@ -0,0 +1,132 @@ +/* mpl3115a2.c */ + +#include "sensors.h" +#include "mpl3115a2.h" +#include "i2c.h" +#include "event.h" +#include "log.h" +#include "uart.h" + +bool mpl3115a2_state; + +i2c_result mpl3115a2_result; + +unsigned char mpl3115a2_sample_data[5]; + +#define LOG_SIGNATURE_ALTIMETER 0xDA7AAA70 + +unsigned char mpl3115a2_start_command[] = {0x26, 0x82}; +unsigned char mpl3115a2_read_address[] = {0x01}; + +struct i2c_transaction mpl3115a2_start_transaction = { + (0x60 << 1) + 0, /* write */ + sizeof(mpl3115a2_start_command), + mpl3115a2_start_command, + &mpl3115a2_result, + EVENT_MPL3115A2_I2C_COMPLETE, + NULL +}; + +struct i2c_transaction mpl3115a2_sample_transaction2; + +struct i2c_transaction mpl3115a2_sample_transaction = { + (0x60 << 1) + 0, /* write */ + sizeof(mpl3115a2_read_address), + mpl3115a2_read_address, + &mpl3115a2_result, + EVENT_MPL3115A2_I2C_COMPLETE, + &mpl3115a2_sample_transaction2 +}; +struct i2c_transaction mpl3115a2_sample_transaction2 = { + (0x60 << 1) + 1, /* read */ + 5, + mpl3115a2_sample_data, + &mpl3115a2_result, + EVENT_MPL3115A2_I2C_COMPLETE, + NULL +}; + +void mpl3115a2_event_handler(void); + +bool mpl3115a2_init(void) +{ + event_register(EVENT_MPL3115A2_I2C_COMPLETE, mpl3115a2_event_handler); + + mpl3115a2_state = 0; + + return TRUE; + +#if 0 + if (!i2c_start_transaction(&mpl3115a2_init_transaction)) + return FALSE; + while (i2c_busy()) ; + + + return (mpl3115a2_result == I2C_SUCCESS); +#endif +} + +bool mpl3115a2_start_sample(void) +{ + mpl3115a2_state = !mpl3115a2_state; + + putstr("X"); + + if (mpl3115a2_state) + return i2c_start_transaction(&mpl3115a2_start_transaction); + else + return i2c_start_transaction(&mpl3115a2_sample_transaction); +} + +void mpl3115a2_event_handler(void) +{ +#if 0 + signed short int xi, yi, zi; + signed short int tempi; + signed short int rolli, pitchi, yawi; + + float x, y, z; + float temp; + float roll, pitch, yaw; +#endif + + if (mpl3115a2_result != I2C_SUCCESS) + return; + + mpl3115a2_result = I2C_IN_PROGRESS; + + sensors_sample_done(); + + /* If we just started a conversion, our job is done for now */ + if (mpl3115a2_state) + return; + +#if 0 + xi = (mpl3115a2_sample_data[0] << 8) + mpl3115a2_sample_data[1]; + yi = (mpl3115a2_sample_data[2] << 8) + mpl3115a2_sample_data[3]; + zi = (mpl3115a2_sample_data[4] << 8) + mpl3115a2_sample_data[5]; + + tempi = (mpl3115a2_sample_data[6] << 8) + mpl3115a2_sample_data[7]; + + rolli = (mpl3115a2_sample_data[ 8] << 8)+mpl3115a2_sample_data[ 9]; + pitchi = (mpl3115a2_sample_data[10] << 8)+mpl3115a2_sample_data[11]; + yawi = (mpl3115a2_sample_data[12] << 8)+mpl3115a2_sample_data[13]; + + sensors_write_gyro_data(roll, pitch, yaw); + sensors_write_accel_data(x, y, z); + sensors_write_temp_data(temp); + +#endif + + log_put_uint(LOG_SIGNATURE_ALTIMETER); + log_put_array((char *)mpl3115a2_sample_data, 5); + putstr("Y"); + +#if 0 + puthex(mpl3115a2_sample_data[0]); + puthex(mpl3115a2_sample_data[1]); + puthex(mpl3115a2_sample_data[2]); + putstr("\n"); +#endif +} + diff --git a/mpl3115a2.h b/mpl3115a2.h new file mode 100644 index 0000000..73ee0ad --- /dev/null +++ b/mpl3115a2.h @@ -0,0 +1,10 @@ +#ifndef __MPL3115A2_H +#define __MPL3115A2_H + +#include "types.h" + +bool mpl3115a2_init(void); +bool mpl3115a2_start_sample(void); +void mpl3115a2_write_log(void); + +#endif /* __MPL3115A2_H */ diff --git a/mpu6050.c b/mpu6050.c index c9c3b3e..cf16c53 100644 --- a/mpu6050.c +++ b/mpu6050.c @@ -57,6 +57,16 @@ struct i2c_transaction mpu6050_whoami_transaction2 = { unsigned char mpu6050_init_command[] = {0x6B, 0x01}; unsigned char mpu6050_accel_init_command[] = {0x1c, (AFS_SEL<<3)}; +unsigned char mpu6050_bypass_init_command[] = {0x37, 0x02}; + +struct i2c_transaction mpu6050_bypass_init_transaction = { + (0x68 << 1) + 0, /* write */ + 2, + mpu6050_bypass_init_command, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + NULL +}; struct i2c_transaction mpu6050_accel_init_transaction = { (0x68 << 1) + 0, /* write */ @@ -64,7 +74,7 @@ struct i2c_transaction mpu6050_accel_init_transaction = { mpu6050_accel_init_command, &mpu6050_result, EVENT_MPU6050_I2C_COMPLETE, - NULL + &mpu6050_bypass_init_transaction }; struct i2c_transaction mpu6050_init_transaction = { diff --git a/sensors.c b/sensors.c index d516d6b..ffcf229 100644 --- a/sensors.c +++ b/sensors.c @@ -1,6 +1,8 @@ /* sensors.c */ #include "mpu6050.h" +#include "hmc5883l.h" +#include "mpl3115a2.h" #include "dcm.h" #include "fisqrt.h" #include "watchdog.h" @@ -11,10 +13,19 @@ #include "log.h" #include "stick.h" +bool (*sensor_init_fns[])(void) = { + mpu6050_init, + hmc5883l_init, + mpl3115a2_init, +}; + bool (*sensor_start_fns[])(void) = { mpu6050_start_sample, + hmc5883l_start_sample, + mpl3115a2_start_sample, }; +#define SENSOR_INIT_FNS (sizeof(sensor_init_fns)/sizeof(sensor_init_fns[0])) #define SENSOR_START_FNS (sizeof(sensor_start_fns)/sizeof(sensor_start_fns[0])) static unsigned int next_sensor; @@ -53,10 +64,13 @@ void sensors_dump(void); bool sensors_init(void) { + unsigned int i; + next_sensor = 0; - if (!mpu6050_init()) - return FALSE; + for (i = 0; i < SENSOR_INIT_FNS; i++) + if (!(sensor_init_fns[i])()) + return FALSE; return TRUE; } @@ -65,7 +79,7 @@ bool sensors_next_sample(void) { bool result; - result = (sensor_start_fns[next_sensor])(); + result = (sensor_start_fns[next_sensor++])(); if (next_sensor >= SENSOR_START_FNS) next_sensor = 0; diff --git a/stick.c b/stick.c index 60e53e2..26a6349 100644 --- a/stick.c +++ b/stick.c @@ -152,6 +152,10 @@ void stick_input(void) { if (throttle < 0.0) throttle = 0.0; } else { + log_put_uint16(0); + log_put_uint16(0); + log_put_uint16(0); + log_put_uint16(0); x = 0.0f; y = 0.0f; z = 0.0f;