Store the configuration on the SD card

9 years ago · d0f9f46f9c
16 changed files with 271 additions and 26 deletions
--- a/6
+++ b/6
@ -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)

--- a/config.c
+++ 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);
+    }
+}
+
--- a/config.h
+++ 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 */
--- a/crc.c
+++ 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);
+}
--- a/crc.h
+++ 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 */
+
--- 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);
--- 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;
--- 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

--- a/memcpy.c
+++ 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;
+}
--- 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.
--- 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<<FS_SEL))

 /* LSB / degree C */
 #define TEMP_STEP	340.0
--- a/sdcard.c
+++ b/sdcard.c
@ -6,6 +6,7 @@
 #include "timer.h"
 #include "event.h"
 #include "log.h"
+#include "config.h"

 #define spi_write_array(x) spi_write_bytes(x, sizeof(x)/sizeof(x[0]))

@ -492,6 +493,7 @@ void sdcard_prepare(void)
 {
 	unsigned int magic;
 	unsigned int start_sector;
+	unsigned int config_sector;
 	unsigned int count;

 	if (!sdcard_read(0, log_buffer, 512))
@ -506,10 +508,12 @@ void sdcard_prepare(void)
 		WRITE_UINT(log_buffer, 0, LOG_MAGIC);
 		start_sector = SDCARD_BOUNDARY_SIZE;
 		log_generation = 0;
+		config_sector = 1;
 		putstr("Did not find header. Formatting.\r\n");
 	} else {
 		start_sector = READ_UINT(log_buffer, 4);
 		log_generation = READ_UINT(log_buffer, 8);
+		config_sector = READ_UINT(log_buffer, 12);
 		count = 0;
 		putstr("Found header.\r\n");
 		putstr("Last started at sector ");
@ -539,6 +543,7 @@ void sdcard_prepare(void)

 	WRITE_UINT(log_buffer, 4, start_sector);
 	WRITE_UINT(log_buffer, 8, log_generation);
+	WRITE_UINT(log_buffer, 12, config_sector);

 	putstr("Starting at sector ");
 	putint(start_sector);
@ -551,7 +556,13 @@ void sdcard_prepare(void)

 	sdcard_sector = start_sector;
 	sdcard_offset = 0;
+
+	if (!sdcard_read(config_sector, log_buffer, 512))
+		return;
+
 	log_enabled = TRUE;
+
+	config_init(log_buffer);
 }


--- a/status.c
+++ b/status.c
@ -15,6 +15,8 @@ led_pattern led_pattern_attitude[] = {100, 200, 100, 200, 100, 200, 100,
 							1000, 0};
 led_pattern led_pattern_dcm_error[] = {100, 200, 100, 200, 100, 200,
 						100, 200, 100, 1000, 0};
+led_pattern led_pattern_config[] = {100, 200, 100, 200, 100, 200,
+					    1000, 1000, 0};

 bool status_armed(void)
 {
@ -74,6 +76,9 @@ void status_set_led_pattern(unsigned int module)
 	case STATUS_MODULE_DCM_ERROR:
 		led_set_pattern(led_pattern_dcm_error);
 		break;
+	case STATUS_MODULE_CONFIG:
+		led_set_pattern(led_pattern_config);
+		break;
 	default:
 		led_set_pattern(led_pattern_unknown);
 		break;
--- a/status.h
+++ b/status.h
@ -8,15 +8,17 @@ void status_set_led_pattern(unsigned int module);
 void init_status(void);


-#define STATUS_MODULE_GYRO_ZERO 0
-#define STATUS_MODULE_GYRO_RATE 1
-#define STATUS_MODULE_ATTITUDE  2
-#define STATUS_MODULE_DCM_ERROR 3
-#define STATUS_MODULE_STICK     4
+#define STATUS_MODULE_CONFIG    0
+#define STATUS_MODULE_GYRO_ZERO 1
+#define STATUS_MODULE_GYRO_RATE 2
+#define STATUS_MODULE_ATTITUDE  3
+#define STATUS_MODULE_DCM_ERROR 4
+#define STATUS_MODULE_STICK     5

-#define STATUS_MODULES		5
+#define STATUS_MODULES		6

 #define STATUS_COUNT {				\
+		STATUS_COUNT_CONFIG,		\
 		STATUS_COUNT_GYRO_ZERO,		\
 		STATUS_COUNT_GYRO_RATE,		\
 		STATUS_COUNT_ATTITUDE,		\
@ -28,6 +30,7 @@ void init_status(void);
 * Each condition must be valid for so many samples, typically once
 * per 100Hz loop
 */
+#define STATUS_COUNT_CONFIG    1
 #define STATUS_COUNT_STICK     100
 #define STATUS_COUNT_GYRO_ZERO 1
 #define STATUS_COUNT_GYRO_RATE 100
--- a/stick.c
+++ b/stick.c
@ -15,24 +15,25 @@
 #include "status.h"
 #include "watchdog.h"
 #include "log.h"
+#include "config.h"

 #define TWO_PI 6.28318531f
 #define PI 3.14159265f

-#define MIN_X 8720
-#define MAX_X 23800
-#define CENTRE_X 16260
+#define MIN_X config.stick.timing.x.min
+#define MAX_X config.stick.timing.x.max
+#define CENTRE_X config.stick.timing.x.centre

-#define MIN_Y 8720
-#define MAX_Y 23800
-#define CENTRE_Y 16260
+#define MIN_Y config.stick.timing.y.min
+#define MAX_Y config.stick.timing.y.max
+#define CENTRE_Y config.stick.timing.y.centre

-#define MIN_Z 8720
-#define MAX_Z 23800
-#define CENTRE_Z 16300
+#define MIN_Z config.stick.timing.z.min
+#define MAX_Z config.stick.timing.z.max
+#define CENTRE_Z config.stick.timing.z.centre

-#define MIN_THR 9720
-#define MAX_THR 23750
+#define MIN_THR config.stick.timing.throttle.min
+#define MAX_THR config.stick.timing.throttle.max

 #define MIN_REAL_THR 8720

@ -46,11 +47,11 @@
 */

 /* Full scale is a roll/pitch angle of 30 degrees from the vertical */
-#define SCALE_X (TWO_PI*30.0/360.0 / (MAX_X-CENTRE_X))
-#define SCALE_Y (TWO_PI*30.0/360.0 / (MAX_Y-CENTRE_Y))
+#define SCALE_X (TWO_PI*config.stick.sensitivity.x/360.0 / (MAX_X-CENTRE_X))
+#define SCALE_Y (TWO_PI*config.stick.sensitivity.y/360.0 / (MAX_Y-CENTRE_Y))

 /* Full scale is a complete rotation in one second */
-#define SCALE_Z (TWO_PI / (MAX_Z-CENTRE_Z))
+#define SCALE_Z (TWO_PI*config.stick.sensitivity.z/360.0 / (MAX_Z-CENTRE_Z))

 /* 0 is min throttle, 1 is max throttle */
 #define SCALE_THROTTLE (1.0f/(MAX_THR - MIN_THR))
--- a/types.h
+++ b/types.h
@ -13,4 +13,14 @@ typedef struct {
    float x, y, z;
 } vec3f;

+typedef struct {
+    float p, i, d, a;
+} pid_params;
+
+typedef struct {
+    int min, max, centre, deadband;
+} stick_timing;
+
+typedef float stick_sensitivity;
+
 #endif /* __TYPES_H */