diff --git a/dcm.c b/dcm.c index 32f0974..96bd5cf 100644 --- a/dcm.c +++ b/dcm.c @@ -42,7 +42,7 @@ float dcm[3*3] = {1, 0, 0, float omega_p[3] = {0.0, 0.0, 0.0}; float omega_i[3] = {0.0, 0.0, 0.0}; -float omega_x, omega_y, omega_z; +vec3f omega; float delta_t = 0.005; @@ -54,15 +54,15 @@ void dcm_log(unsigned int magic) log_put_float(dcm[i]); } -void dcm_update(float x, float y, float z) +void dcm_update(vec3f gyro) { - omega_x = x + omega_i[0] + omega_p[0]; - omega_y = y + omega_i[1] + omega_p[1]; - omega_z = z + omega_i[2] + omega_p[2]; + omega.x = gyro.x + omega_i[0] + omega_p[0]; + omega.y = gyro.y + omega_i[1] + omega_p[1]; + omega.z = gyro.z + omega_i[2] + omega_p[2]; - float tx = delta_t * omega_x; - float ty = delta_t * omega_y; - float tz = delta_t * omega_z; + float tx = delta_t * omega.x; + float ty = delta_t * omega.y; + float tz = delta_t * omega.z; float update_matrix[3*3] = { 0, -tz, ty, tz, 0, -tx, @@ -77,12 +77,12 @@ void dcm_update(float x, float y, float z) /* dcm_log(LOG_MAGIC_DCM_UPDATE); */ } -void dcm_setvector(float x, float y, float z) +void dcm_setvector(vec3f zvec) { /* We're given the Z axis */ - dcm[6] = x; - dcm[7] = y; - dcm[8] = z; + dcm[6] = zvec.x; + dcm[7] = zvec.y; + dcm[8] = zvec.z; /* Second row = cross product of unit X and third rows */ dcm[3] = 0.0; @@ -171,7 +171,7 @@ bool dcm_renormalise(float *v) return TRUE; } -void dcm_drift_correction(float x, float y, float z) +void dcm_drift_correction(vec3f accel) { float weight; float error[3]; @@ -180,7 +180,10 @@ void dcm_drift_correction(float x, float y, float z) #if DCM_WEIGHT float mag; - mag = (1.0/fisqrt(x*x+y*y+z*z)) / GRAVITY; + mag = (1.0/fisqrt(accel.x*accel.x+ + accel.y*accel.y+ + accel.z*accel.z)) + / GRAVITY; mag = 1-mag; if (mag < 0.0) @@ -198,9 +201,9 @@ void dcm_drift_correction(float x, float y, float z) /* error = cross product of dcm last row and acceleration vector */ /* third row = cross product of first two rows */ - error[0] = dcm[7]*z - dcm[8]*y; - error[1] = dcm[8]*x - dcm[6]*z; - error[2] = dcm[6]*y - dcm[7]*x; + error[0] = dcm[7]*accel.z - dcm[8]*accel.y; + error[1] = dcm[8]*accel.x - dcm[6]*accel.z; + error[2] = dcm[6]*accel.y - dcm[7]*accel.x; if (!status_armed()) { if ((abs(error[0]) < ERROR_THRESHOLD) && @@ -251,7 +254,8 @@ void dcm_drift_correction(float x, float y, float z) /* Maximum angle to the horizontal for arming: 30 degrees */ #define ATTITUDE_THRESHOLD (0.5) -void dcm_attitude_error(float roll, float pitch, float yaw) +/* x = roll, y = pitch, z = yaw */ +void dcm_attitude_error(vec3f target) { /* dcm[6] = sine of pitch */ /* dcm[7] = sine of roll */ @@ -272,7 +276,8 @@ void dcm_attitude_error(float roll, float pitch, float yaw) status_set_ready(STATUS_MODULE_ATTITUDE, FALSE); } - motor_pid_update(roll, dcm[6], pitch, -dcm[7], yaw, -omega_z); + vec3f measured = {dcm[6], -dcm[7], -omega.z}; + motor_pid_update(target, measured); } void dcm_dump(void) diff --git a/dcm.h b/dcm.h index c2e068e..72cc67e 100644 --- a/dcm.h +++ b/dcm.h @@ -4,11 +4,11 @@ extern float delta_t; -void dcm_update(float omega_x, float omega_y, float omega_z); +void dcm_update(vec3f gyro); void dcm_normalise(void); bool dcm_renormalise(float *v); void dcm_dump(void); void dcm_send_packet(void); -void dcm_setvector(float x, float y, float z); -void dcm_drift_correction(float x, float y, float z); -void dcm_attitude_error(float x, float y, float z); +void dcm_setvector(vec3f zvec); +void dcm_drift_correction(vec3f accel); +void dcm_attitude_error(vec3f target); diff --git a/hmc5883l.c b/hmc5883l.c index 1a2130d..1b1a90d 100644 --- a/hmc5883l.c +++ b/hmc5883l.c @@ -85,16 +85,6 @@ bool hmc5883l_start_sample(void) 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; @@ -106,23 +96,6 @@ void hmc5883l_event_handler(void) 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/motor.c b/motor.c index 0f9c714..d4a5ad1 100644 --- a/motor.c +++ b/motor.c @@ -27,10 +27,12 @@ float throttle = 0.0f; * Perform a PID loop iteration. * roll and pitch are absolute values * yaw is, currently, a rate. + * For this function only, coordinate convention is: + * x = roll + * y = pitch + * z = yaw */ -void motor_pid_update(float troll, float mroll, - float tpitch, float mpitch, - float tyaw, float myaw) +void motor_pid_update(vec3f target, vec3f measured) { float derivative[3]; float out[3]; @@ -40,16 +42,16 @@ void motor_pid_update(float troll, float mroll, float min_motor; int i; - roll = troll - mroll; - pitch = tpitch - mpitch; - yaw = tyaw - myaw; + roll = target.x - measured.x; + pitch = target.y - measured.y; + yaw = target.z - measured.z; #if 0 if ((stick_counter % 100) == 0) { putstr("{"); - putint_s((int)(tyaw * 10000)); + putint_s((int)(target.z * 10000)); putstr(", "); - putint_s((int)(myaw * 10000)); + putint_s((int)(measured.z * 10000)); putstr("}\r\n"); } #endif @@ -67,13 +69,13 @@ void motor_pid_update(float troll, float mroll, } /* The measurements are the opposite sign to the error */ - derivative[0] = (-mroll - last[0]) / delta_t; - derivative[1] = (-mpitch - last[1]) / delta_t; - derivative[2] = (-myaw - last[2]) / delta_t; + derivative[0] = (-measured.x - last[0]) / delta_t; + derivative[1] = (-measured.y - last[1]) / delta_t; + derivative[2] = (-measured.z - last[2]) / delta_t; - last[0] = -mroll; - last[1] = -mpitch; - last[2] = -myaw; + last[0] = -measured.x; + last[1] = -measured.y; + last[2] = -measured.z; out[0] = roll * Kp + integral[0] * Ki + derivative[0] * Kd; out[1] = pitch * Kp + integral[1] * Ki + derivative[1] * Kd; diff --git a/motor.h b/motor.h index c0bdb90..9fcbeb3 100644 --- a/motor.h +++ b/motor.h @@ -1,9 +1,12 @@ /* motor.h */ +#ifndef __MOTOR_H +#define __MOTOR_H +#include "types.h" + extern float temp_kp; -void motor_pid_update(float troll, float mroll, - float tpitch, float mpitch, - float tyaw, float myaw); +void motor_pid_update(vec3f target, vec3f measured); void motor_kill(void); void motor_set_throttle(float t); +#endif /* __MOTOR_H */ diff --git a/mpl3115a2.c b/mpl3115a2.c index d9e968f..61cdb99 100644 --- a/mpl3115a2.c +++ b/mpl3115a2.c @@ -55,23 +55,12 @@ bool mpl3115a2_init(void) 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 @@ -80,16 +69,6 @@ bool mpl3115a2_start_sample(void) 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; @@ -101,32 +80,7 @@ void mpl3115a2_event_handler(void) 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/mpu6050.c b/mpu6050.c index cf16c53..a9b002c 100644 --- a/mpu6050.c +++ b/mpu6050.c @@ -214,9 +214,8 @@ void mpu6050_event_handler(void) signed short int tempi; signed short int rolli, pitchi, yawi; - float x, y, z; + vec3f accel, gyro; float temp; - float roll, pitch, yaw; CHECKPOINT(9); @@ -252,18 +251,18 @@ void mpu6050_event_handler(void) yawi -= gyro_zero_yaw; } - x = ((float)xi) * ACCEL_SCALE; - y = ((float)yi) * ACCEL_SCALE; - z = ((float)zi) * ACCEL_SCALE; + accel.x = ((float)xi) * ACCEL_SCALE; + accel.y = ((float)yi) * ACCEL_SCALE; + accel.z = ((float)zi) * ACCEL_SCALE; temp = ((float)tempi) * TEMP_SCALE + TEMP_OFFSET; - roll = ((float)rolli) * GYRO_SCALE; - pitch = ((float)pitchi) * GYRO_SCALE; - yaw = ((float)yawi) * GYRO_SCALE; + gyro.x = ((float)rolli) * GYRO_SCALE; + gyro.y = ((float)pitchi) * GYRO_SCALE; + gyro.z = ((float)yawi) * GYRO_SCALE; - sensors_write_gyro_data(roll, pitch, yaw); - sensors_write_accel_data(x, y, z); + sensors_write_gyro_data(gyro); + sensors_write_accel_data(accel); sensors_write_temp_data(temp); log_put_array((char *)mpu6050_sample_data, 14); diff --git a/sensors.c b/sensors.c index ffcf229..8ccfe7f 100644 --- a/sensors.c +++ b/sensors.c @@ -35,19 +35,16 @@ static bool sensors_update; static unsigned int sensors_discard; static unsigned int sensors_generation; -float sensors_gyro_roll; -float sensors_gyro_pitch; -float sensors_gyro_yaw; +/* x = roll + y = pitch + z = yaw + */ +vec3f sensors_gyro; +vec3f gyro_zero; float sensors_temp; -float sensors_accel_x; -float sensors_accel_y; -float sensors_accel_z; - -float gyro_yaw_zero; -float gyro_pitch_zero; -float gyro_roll_zero; +vec3f sensors_accel; void sensors_write_log(void); void sensors_process(void); @@ -103,24 +100,20 @@ bool sensors_start_sample(void) return sensors_next_sample(); } -void sensors_write_gyro_data(float roll, float pitch, float yaw) +void sensors_write_gyro_data(vec3f gyro) { #if 0 - sensors_gyro_roll = roll - gyro_roll_zero; - sensors_gyro_pitch = pitch - gyro_pitch_zero; - sensors_gyro_yaw = yaw - gyro_yaw_zero; + sensors_gyro.x = gyro.x - gyro_zero.x; + sensors_gyro.y = gyro.y - gyro_zero.y; + sensors_gyro.z = gyro.z - gyro_zero.z; #else - sensors_gyro_roll = roll; - sensors_gyro_pitch = pitch; - sensors_gyro_yaw = yaw; + sensors_gyro = gyro; #endif } -void sensors_write_accel_data(float x, float y, float z) +void sensors_write_accel_data(vec3f accel) { - sensors_accel_x = x; - sensors_accel_y = y; - sensors_accel_z = z; + sensors_accel = accel; } void sensors_write_temp_data(float temp) @@ -136,12 +129,12 @@ void sensors_write_log(void) { #if 0 log_put_uint(LOG_SIGNATURE_SENSORS); - log_put_float(sensors_accel_x); - log_put_float(sensors_accel_y); - log_put_float(sensors_accel_z); - log_put_float(sensors_gyro_roll); - log_put_float(sensors_gyro_pitch); - log_put_float(sensors_gyro_yaw); + log_put_float(sensors_accel.x); + log_put_float(sensors_accel.y); + log_put_float(sensors_accel.z); + log_put_float(sensors_gyro.x); + log_put_float(sensors_gyro.y); + log_put_float(sensors_gyro.z); log_put_float(sensors_temp); #else /* XXX this just about comes out in the right place, but by luck */ @@ -163,20 +156,19 @@ void sensors_process(void) { if (sensors_update) { #if 1 - dcm_update(-sensors_gyro_pitch, -sensors_gyro_roll, - -sensors_gyro_yaw); + vec3f dcm_gyro = {-sensors_gyro.y, -sensors_gyro.x, -sensors_gyro.z}; #else - dcm_update(0.0, 0.0, 0.0); + vec3f dcm_gyro = {0.0, 0.0, 0.0}; #endif + dcm_update(dcm_gyro); if (!status_armed()) { - if ( (abs(sensors_gyro_roll) < GYRO_RATE_THRESHOLD) && - (abs(sensors_gyro_pitch) < GYRO_RATE_THRESHOLD) && - (abs(sensors_gyro_yaw) < GYRO_RATE_THRESHOLD)) { + if ( (abs(sensors_gyro.x) < GYRO_RATE_THRESHOLD) && + (abs(sensors_gyro.y) < GYRO_RATE_THRESHOLD) && + (abs(sensors_gyro.z) < GYRO_RATE_THRESHOLD)) { status_set_ready(STATUS_MODULE_GYRO_RATE, TRUE); } else { status_set_ready(STATUS_MODULE_GYRO_RATE, FALSE); } - } sensors_generation++; @@ -192,17 +184,17 @@ void sensors_process(void) if (sensors_discard) { sensors_discard--; } else { - gyro_yaw_zero += sensors_gyro_yaw; - gyro_pitch_zero += sensors_gyro_pitch; - gyro_roll_zero += sensors_gyro_roll; + gyro_zero.z += sensors_gyro.z; + gyro_zero.y += sensors_gyro.y; + gyro_zero.x += sensors_gyro.x; sensors_generation++; if (sensors_generation >= GYRO_ZERO_COUNT) { sensors_zero = FALSE; sensors_update = TRUE; sensors_generation = 0; - gyro_yaw_zero /= GYRO_ZERO_COUNT; - gyro_pitch_zero /= GYRO_ZERO_COUNT; - gyro_roll_zero /= GYRO_ZERO_COUNT; + gyro_zero.z /= GYRO_ZERO_COUNT; + gyro_zero.y /= GYRO_ZERO_COUNT; + gyro_zero.x /= GYRO_ZERO_COUNT; putstr("Zero finished\r\n"); status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE); } @@ -211,13 +203,12 @@ void sensors_process(void) watchdog_kick(WATCHDOG_GYRO); #if 1 - dcm_drift_correction(-sensors_accel_y, -sensors_accel_x, - -sensors_accel_z); + vec3f dcm_accel = {-sensors_accel.y, -sensors_accel.x, -sensors_accel.z}; #endif #if 0 - dcm_drift_correction(sensors_accel_x, sensors_accel_y, - sensors_accel_z); + vec3f dcm_accel = {sensors_accel.x, sensors_accel.y, sensors_accel.z}; #endif + dcm_drift_correction(dcm_accel); watchdog_kick(WATCHDOG_ACCEL); stick_input(); @@ -226,19 +217,19 @@ void sensors_process(void) void sensors_dump(void) { putstr("("); - putint_s((int)(sensors_accel_x * 1000.0)); + putint_s((int)(sensors_accel.x * 1000.0)); putstr(","); - putint_s((int)(sensors_accel_y * 1000.0)); + putint_s((int)(sensors_accel.y * 1000.0)); putstr(","); - putint_s((int)(sensors_accel_z * 1000.0)); + putint_s((int)(sensors_accel.z * 1000.0)); putstr(")"); putstr("("); - putint_s((int)(sensors_gyro_roll * 1000.0)); + putint_s((int)(sensors_gyro.x * 1000.0)); putstr(","); - putint_s((int)(sensors_gyro_pitch * 1000.0)); + putint_s((int)(sensors_gyro.y * 1000.0)); putstr(","); - putint_s((int)(sensors_gyro_yaw * 1000.0)); + putint_s((int)(sensors_gyro.z * 1000.0)); putstr(")"); putstr("("); diff --git a/sensors.h b/sensors.h new file mode 100644 index 0000000..07dbe27 --- /dev/null +++ b/sensors.h @@ -0,0 +1,21 @@ +/* sensors.h */ + +#ifndef __SENSORS_H +#define __SENSORS_H + +#include "types.h" + +/* functions to be called from above */ +bool sensors_init(void); +bool sensors_start_sample(void); +void sensors_dump(void); + +/* functions to be called from below */ +void sensors_sample_done(void); +/* x = roll, y = pitch, z = yaw */ +void sensors_write_gyro_data(vec3f gyro_data); +void sensors_write_accel_data(vec3f accel_data); +void sensors_write_temp_data(float temp); +void sensors_start_zero(void); + +#endif /* __SENSORS_H */ diff --git a/stick.c b/stick.c index 26a6349..49d371f 100644 --- a/stick.c +++ b/stick.c @@ -59,9 +59,9 @@ float stick_yaw = 0.0f; unsigned int stick_counter; -void stick_update(float x, float y, float z) +void stick_update(vec3f stick) { - float tz = delta_t * z; + float tz = delta_t * stick.z; stick_yaw += tz; @@ -72,16 +72,16 @@ void stick_update(float x, float y, float z) stick_yaw -= TWO_PI; #if 0 - z = stick_yaw; + stick.z = stick_yaw; #endif - x = sine(x); - y = sine(y); + stick.x = sine(stick.x); + stick.y = sine(stick.y); #if 0 - z = 1.0/fisqrt(1-x*x-y*y); + stick.z = 1.0/fisqrt(1-stick.x*stick.x-stick.y*stick.y); #endif - dcm_attitude_error(x, y, z); + dcm_attitude_error(stick); } #ifdef STICK_DEBUG_CALIBRATE @@ -168,9 +168,7 @@ void stick_input(void) { watchdog_kick(WATCHDOG_STICK); /* So the controls are back to front. Let's fix that. */ - x = -x; - y = -y; - z = -z; + vec3f stick = {-x, -y, -z}; #if 0 if ((stick_counter % 100) == 0) { @@ -181,10 +179,10 @@ void stick_input(void) { #endif #if 1 - stick_update(x, y, z); + stick_update(stick); #else if ((stick_counter % 100) == 0) - stick_update(x, y, z); + stick_update(stick); #endif /* diff --git a/stick.h b/stick.h index 10c9051..74bb01a 100644 --- a/stick.h +++ b/stick.h @@ -7,4 +7,4 @@ extern bool armed; extern unsigned int stick_counter; void stick_input(void); -void stick_update(float x, float y, float omega_z); +void stick_update(vec3f stick); diff --git a/types.h b/types.h index 5e38bdd..65f22e4 100644 --- a/types.h +++ b/types.h @@ -9,4 +9,8 @@ typedef int bool; typedef unsigned int size_t; +typedef struct { + float x, y, z; +} vec3f; + #endif /* __TYPES_H */