diff --git a/Makefile b/Makefile index 3c4bb9c..ef50de3 100644 --- a/Makefile +++ b/Makefile @@ -5,7 +5,8 @@ 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 hmc5883l.c mpl3115a2.c +CSRCS+=thrust.c sensors.c spi.c sdcard.c log.c hmc5883l.c mpl3115a2.c config.c +CSRCS+=crc.c memcpy.c #PROJOPTS=-DUSE_UART -DSEND_DCM -DSTICK_DEBUG_CALIBRATE PROJOPTS=-DTIMER_CPPM -DI2C_FAST @@ -14,6 +15,9 @@ PROJOPTS=-DTIMER_CPPM -DI2C_FAST #PROJOPTS=-DTIMER_CPPM -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DSEND_DCM #PROJOPTS=-DTIMER_CPPM -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DUSE_UART -DSEND_DCM +# Enable debug +#PROJOPTS+=-DUSE_UART + COPTIM?=-Os CFLAGS=-march=armv4t -msoft-float $(COPTIM) -Wall -Werror -Wextra $(PROJOPTS) diff --git a/config.c b/config.c new file mode 100644 index 0000000..7e2d5fe --- /dev/null +++ b/config.c @@ -0,0 +1,35 @@ +/* config.c */ + +#include "types.h" +#include "config.h" +#include "crc.h" +#include "log.h" +#include "status.h" +#include "panic.h" + +struct config config; + +bool config_valid = FALSE; + +#define READ_UINT(b, i) ((b)[(i)] + ((b)[(i)+1] << 8) + \ + ((b)[(i)+2] << 16) + ((b)[(i)+3] << 24)) + +unsigned int foobar = sizeof(struct config); + +void config_init(char *buffer) +{ + config_valid = FALSE; + if (READ_UINT(buffer, 0) == CONFIG_MAGIC) { + if (READ_UINT(buffer, 4) == CONFIG_VERSION) { + if (check_crc(buffer, sizeof(struct config)+12)) { + config = *(struct config *)(buffer+8); + config_valid = TRUE; + } + } + } + if (config_valid) { + log_put_config(); + status_set_ready(STATUS_MODULE_CONFIG, TRUE); + } +} + diff --git a/config.h b/config.h new file mode 100644 index 0000000..02563c2 --- /dev/null +++ b/config.h @@ -0,0 +1,36 @@ +/* config.h */ + +#ifndef __CONFIG_H +#define __CONFIG_H + +#include "types.h" + +#define CONFIG_VERSION 0 +#define CONFIG_MAGIC 0xc07f18 + +struct config { + struct { + pid_params rollpitch; + pid_params yaw; + } pid; + struct { + struct { + stick_timing x, y, z, throttle; + } timing; + struct { + stick_sensitivity x, y, z; + } sensitivity; + } stick; + struct { + unsigned int gyro_sensitivity; + unsigned int accel_sensitivity; + } mpu6050; + /* Magnetometer calibration */ +}; + +extern struct config config; +extern bool config_valid; + +void config_init(char *buffer); + +#endif /* __CONFIG_H */ diff --git a/crc.c b/crc.c new file mode 100644 index 0000000..99241e0 --- /dev/null +++ b/crc.c @@ -0,0 +1,70 @@ +/* crc.c */ + +#include "crc.h" +#include "uart.h" + +static unsigned int crc32_table[] = { + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, + 0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, + 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2, + 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, + 0xfa0f3d63, 0x8d080df5, 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, + 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 0x35b5a8fa, 0x42b2986c, + 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, + 0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, + 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, 0x76dc4190, 0x01db7106, + 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, + 0x91646c97, 0xe6635c01, 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, + 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950, + 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, + 0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, + 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa, + 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, + 0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, + 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 0xe3630b12, 0x94643b84, + 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, + 0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, + 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 0xd6d6a3e8, 0xa1d1937e, + 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, + 0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, + 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28, + 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, + 0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, + 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242, + 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, + 0x616bffd3, 0x166ccf45, 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, + 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc, + 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, + 0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, + 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d +}; + +bool check_crc(char *buffer, int len) +{ + unsigned int crc = 0xffffffff; + + putstr("Calculating CRC for "); + putint(len); + putstr(" bytes: "); + + while (len--) { + int index = (crc ^ *buffer++) & 0xff; + unsigned int tval = crc32_table[index]; + crc = tval ^ (crc >> 8); + } + + puthex(crc); + putstr("\n"); + + return (crc == 0); +} diff --git a/crc.h b/crc.h new file mode 100644 index 0000000..f874051 --- /dev/null +++ b/crc.h @@ -0,0 +1,11 @@ +/* crc.h */ + +#ifndef __CRC_H +#define __CRC_H + +#include "types.h" + +extern bool check_crc(char *buffer, int len); + +#endif /* __CRC_H */ + diff --git a/event.h b/event.h index dba73ce..7b84964 100644 --- a/event.h +++ b/event.h @@ -12,6 +12,8 @@ #define EVENT_MAX 5 +#define EVENT_NONE 0xffff + typedef void event_handler(void); void event_set(unsigned int event); diff --git a/log.c b/log.c index edbbc48..6e072c8 100644 --- a/log.c +++ b/log.c @@ -5,6 +5,7 @@ #include "uart.h" #include "timer.h" #include "log.h" +#include "config.h" /* This is shared with sdcard.c */ bool log_enabled; @@ -37,7 +38,9 @@ void log_put_byte(char c) * Better than overrunning. */ if (((log_bufferend + 1) % LOG_BUFFERSIZE) == log_bufferstart) { +#if 0 putstr("^"); +#endif return; } log_buffer[log_bufferend++] = c; @@ -70,6 +73,14 @@ void log_put_header(unsigned int timestamp) log_put_uint(log_read_busytime()); } +void log_put_config(void) +{ + log_put_uint(LOG_MAGIC_CONFIG); + log_put_uint(CONFIG_VERSION); + log_put_uint(sizeof(struct config)); + log_put_array((char *)&config, sizeof(struct config)); +} + void log_put_array(char *data, int length) { int i; diff --git a/log.h b/log.h index 061df1c..ec6566c 100644 --- a/log.h +++ b/log.h @@ -12,6 +12,7 @@ void log_put_uint(unsigned int i); void log_put_float(float f); void log_put_header(unsigned int timestamp); void log_put_array(char *data, int length); +void log_put_config(void); void log_mark_busy(void); void log_mark_idle(void); @@ -19,6 +20,7 @@ unsigned int log_read_busytime(void); /* Needed by log.c and sdcard.c only */ #define LOG_MAGIC 0x00000CFC +#define LOG_MAGIC_CONFIG 0x00C07F18 #define LOG_BUFFERSIZE 4096 diff --git a/memcpy.c b/memcpy.c new file mode 100644 index 0000000..35a7554 --- /dev/null +++ b/memcpy.c @@ -0,0 +1,14 @@ +/* memcpy.c */ + +#include "types.h" + +void *memcpy(void *str1, const void *str2, size_t n) +{ + char *dest = str1; + const char *src = str2; + + while (n--) + *dest++ = *src++; + + return str1; +} diff --git a/motor.c b/motor.c index d4a5ad1..f0732de 100644 --- a/motor.c +++ b/motor.c @@ -6,12 +6,14 @@ #include "uart.h" #include "status.h" #include "log.h" +#include "config.h" float integral[3] = {0.0f, 0.0f, 0.0f}; float last[3]; float throttle = 0.0f; +#if 0 #define Kp 0.2 #define Ki 0.04 #define Kd 0.08 @@ -22,6 +24,19 @@ float throttle = 0.0f; #define Kd_y 0.00 #define Ka_y 0.0 +#else + +#define Kp config.pid.rollpitch.p +#define Ki config.pid.rollpitch.i +#define Kd config.pid.rollpitch.d +#define Ka config.pid.rollpitch.a + +#define Kp_y config.pid.yaw.p +#define Ki_y config.pid.yaw.i +#define Kd_y config.pid.yaw.d +#define Ka_y config.pid.yaw.a +#endif + /* * Perform a PID loop iteration. diff --git a/mpu6050.c b/mpu6050.c index a9b002c..8c402f6 100644 --- a/mpu6050.c +++ b/mpu6050.c @@ -14,6 +14,7 @@ #include "timer.h" #include "panic.h" #include "log.h" +#include "config.h" i2c_result mpu6050_result; unsigned int mpu6050_generation; @@ -51,12 +52,13 @@ struct i2c_transaction mpu6050_whoami_transaction2 = { NULL }; -/* Accelerometer scaling */ -#define AFS_SEL 2 +#define FS_SEL config.mpu6050.gyro_sensitivity +#define AFS_SEL config.mpu6050.accel_sensitivity unsigned char mpu6050_init_command[] = {0x6B, 0x01}; -unsigned char mpu6050_accel_init_command[] = {0x1c, (AFS_SEL<<3)}; +unsigned char mpu6050_gyro_init_command[] = {0x1b, 0 /* (FS_SEL<<3) to be filled in at runtime */}; +unsigned char mpu6050_accel_init_command[] = {0x1c, 0 /* (AFS_SEL<<3) to be filled in at runtime */}; unsigned char mpu6050_bypass_init_command[] = {0x37, 0x02}; struct i2c_transaction mpu6050_bypass_init_transaction = { @@ -77,13 +79,22 @@ struct i2c_transaction mpu6050_accel_init_transaction = { &mpu6050_bypass_init_transaction }; +struct i2c_transaction mpu6050_gyro_init_transaction = { + (0x68 << 1) + 0, /* write */ + 2, + mpu6050_gyro_init_command, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + &mpu6050_accel_init_transaction +}; + struct i2c_transaction mpu6050_init_transaction = { (0x68 << 1) + 0, /* write */ 2, mpu6050_init_command, &mpu6050_result, EVENT_MPU6050_I2C_COMPLETE, - &mpu6050_accel_init_transaction + &mpu6050_gyro_init_transaction }; unsigned char mpu6050_sample_command[] = {0x3B}; @@ -114,6 +125,10 @@ bool mpu6050_init(void) { event_register(EVENT_MPU6050_I2C_COMPLETE, mpu6050_event_handler); + /* Fill in sensitivity accordingly, as it's not known at compile time */ + mpu6050_gyro_init_command[1] = FS_SEL << 3; + mpu6050_accel_init_command[1] = AFS_SEL << 3; + if (!i2c_start_transaction(&mpu6050_init_transaction)) return FALSE; while (i2c_busy()) ; @@ -133,7 +148,7 @@ bool mpu6050_init(void) /* A step of 131 = 1 degree. */ /* Overall, this is LSB / rad/s */ -#define GYRO_STEP (131.0 / DEG_TO_RAD) +#define GYRO_STEP (131.0 / DEG_TO_RAD / (float)(1<