coolfactor
/
quadrotor
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Add missing file and tidy up somewhat.

master
Gavan Fantom 10 years ago
parent
8b3ff9feba
commit
4f171f3023
12 changed files with 136 additions and 186 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 43
      dcm.c
  2. 8
      dcm.h
  3. 27
      hmc5883l.c
  4. 30
      motor.c
  5. 9
      motor.h
  6. 46
      mpl3115a2.c
  7. 19
      mpu6050.c
  8. 91
      sensors.c
  9. 21
      sensors.h
  10. 22
      stick.c
  11. 2
      stick.h
  12. 4
      types.h

43
dcm.c
Unescape View File

@ -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)

8
dcm.h
Unescape View File

@ -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);

27
hmc5883l.c
Unescape View File

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

30
motor.c
Unescape View File

@ -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;

9
motor.h
Unescape View File

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

46
mpl3115a2.c
Unescape View File

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

19
mpu6050.c
Unescape View File

@ -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);

91
sensors.c
Unescape View File

@ -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("(");

21
sensors.h
Unescape View File

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

22
stick.c
Unescape View File

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

2
stick.h
Unescape View File

@ -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);

4
types.h
Unescape View File

@ -9,4 +9,8 @@ typedef int bool;
typedef unsigned int size_t;
typedef struct {
float x, y, z;
} vec3f;
#endif /* __TYPES_H */

Write Preview
Loading…
Cancel
Save