Browse Source

Add preliminary support for HMC5883L magnetometer and MPL3115A2 altimeter.

Logging only, at this stage.
master
Gavan Fantom 10 years ago
parent
commit
8b3ff9feba
  1. 3
      Makefile
  2. 8
      event.h
  3. 129
      hmc5883l.c
  4. 10
      hmc5883l.h
  5. 4
      log.c
  6. 4
      matrix.c
  7. 132
      mpl3115a2.c
  8. 10
      mpl3115a2.h
  9. 12
      mpu6050.c
  10. 20
      sensors.c
  11. 4
      stick.c

3
Makefile

@ -5,10 +5,11 @@ NAME=quad
SSRCS=crt0.s SSRCS=crt0.s
CSRCS=main.c i2c.c mpu6050.c timer.c interrupt.c uart.c event.c matrix.c dcm.c 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+=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=-DUSE_UART -DSEND_DCM -DSTICK_DEBUG_CALIBRATE
PROJOPTS=-DTIMER_CPPM -DI2C_FAST 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 -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 -DSEND_DCM
#PROJOPTS=-DTIMER_CPPM -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DUSE_UART -DSEND_DCM #PROJOPTS=-DTIMER_CPPM -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DUSE_UART -DSEND_DCM

8
event.h

@ -5,10 +5,12 @@
#define EVENT_TIMER 0 #define EVENT_TIMER 0
#define EVENT_MPU6050_I2C_COMPLETE 1 #define EVENT_MPU6050_I2C_COMPLETE 1
#define EVENT_UART_INPUT 2 #define EVENT_HMC5883L_I2C_COMPLETE 2
#define EVENT_SDCARD 3 #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); typedef void event_handler(void);

129
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);
}

10
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 */

4
log.c

@ -36,8 +36,10 @@ void log_put_byte(char c)
/* If the buffer is full, we just discard data. /* If the buffer is full, we just discard data.
* Better than overrunning. * Better than overrunning.
*/ */
if (((log_bufferend + 1) % LOG_BUFFERSIZE) == log_bufferstart) if (((log_bufferend + 1) % LOG_BUFFERSIZE) == log_bufferstart) {
putstr("^");
return; return;
}
log_buffer[log_bufferend++] = c; log_buffer[log_bufferend++] = c;
log_bufferend = log_bufferend % LOG_BUFFERSIZE; log_bufferend = log_bufferend % LOG_BUFFERSIZE;
#if 0 #if 0

4
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] */ /* 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) 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] */ /* dest[r][c] = m1[r][c] + m2[r][c] */
void matrix_add(float *dest, float *m1, float *m2, int r, int 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 #endif
#if 0
/* Invert src into dst. /* Invert src into dst.
* NOTE: destroys the src matrix * 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 */ /* src should now be the identiy matrix while dst holds the answer */
} }
#endif

132
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
}

10
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 */

12
mpu6050.c

@ -57,6 +57,16 @@ struct i2c_transaction mpu6050_whoami_transaction2 = {
unsigned char mpu6050_init_command[] = {0x6B, 0x01}; unsigned char mpu6050_init_command[] = {0x6B, 0x01};
unsigned char mpu6050_accel_init_command[] = {0x1c, (AFS_SEL<<3)}; 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 = { struct i2c_transaction mpu6050_accel_init_transaction = {
(0x68 << 1) + 0, /* write */ (0x68 << 1) + 0, /* write */
@ -64,7 +74,7 @@ struct i2c_transaction mpu6050_accel_init_transaction = {
mpu6050_accel_init_command, mpu6050_accel_init_command,
&mpu6050_result, &mpu6050_result,
EVENT_MPU6050_I2C_COMPLETE, EVENT_MPU6050_I2C_COMPLETE,
NULL &mpu6050_bypass_init_transaction
}; };
struct i2c_transaction mpu6050_init_transaction = { struct i2c_transaction mpu6050_init_transaction = {

20
sensors.c

@ -1,6 +1,8 @@
/* sensors.c */ /* sensors.c */
#include "mpu6050.h" #include "mpu6050.h"
#include "hmc5883l.h"
#include "mpl3115a2.h"
#include "dcm.h" #include "dcm.h"
#include "fisqrt.h" #include "fisqrt.h"
#include "watchdog.h" #include "watchdog.h"
@ -11,10 +13,19 @@
#include "log.h" #include "log.h"
#include "stick.h" #include "stick.h"
bool (*sensor_init_fns[])(void) = {
mpu6050_init,
hmc5883l_init,
mpl3115a2_init,
};
bool (*sensor_start_fns[])(void) = { bool (*sensor_start_fns[])(void) = {
mpu6050_start_sample, 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])) #define SENSOR_START_FNS (sizeof(sensor_start_fns)/sizeof(sensor_start_fns[0]))
static unsigned int next_sensor; static unsigned int next_sensor;
@ -53,10 +64,13 @@ void sensors_dump(void);
bool sensors_init(void) bool sensors_init(void)
{ {
unsigned int i;
next_sensor = 0; next_sensor = 0;
if (!mpu6050_init()) for (i = 0; i < SENSOR_INIT_FNS; i++)
return FALSE; if (!(sensor_init_fns[i])())
return FALSE;
return TRUE; return TRUE;
} }
@ -65,7 +79,7 @@ bool sensors_next_sample(void)
{ {
bool result; bool result;
result = (sensor_start_fns[next_sensor])(); result = (sensor_start_fns[next_sensor++])();
if (next_sensor >= SENSOR_START_FNS) if (next_sensor >= SENSOR_START_FNS)
next_sensor = 0; next_sensor = 0;

4
stick.c

@ -152,6 +152,10 @@ void stick_input(void) {
if (throttle < 0.0) if (throttle < 0.0)
throttle = 0.0; throttle = 0.0;
} else { } else {
log_put_uint16(0);
log_put_uint16(0);
log_put_uint16(0);
log_put_uint16(0);
x = 0.0f; x = 0.0f;
y = 0.0f; y = 0.0f;
z = 0.0f; z = 0.0f;

Loading…
Cancel
Save