| 0,0 → 1,222 |
|---|
| #include "vector.h" |
| #include <math.h> |
| #include <inttypes.h> |
| #include <avr/io.h> |
| #include <stdlib.h> |
| |
| extern vector m_max; |
| extern vector m_min; |
| |
| |
| void i2c_start() { |
| TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN); // send start condition |
| while (!(TWCR & (1 << TWINT))); |
| } |
| |
| void i2c_write_byte(char byte) { |
| TWDR = byte; |
| TWCR = (1 << TWINT) | (1 << TWEN); // start address transmission |
| while (!(TWCR & (1 << TWINT))); |
| } |
| |
| char i2c_read_byte() { |
| TWCR = (1 << TWINT) | (1 << TWEA) | (1 << TWEN); // start data reception, transmit ACK |
| while (!(TWCR & (1 << TWINT))); |
| return TWDR; |
| } |
| |
| char i2c_read_last_byte() { |
| TWCR = (1 << TWINT) | (1 << TWEN); // start data reception |
| while (!(TWCR & (1 << TWINT))); |
| return TWDR; |
| } |
| |
| void i2c_stop() { |
| TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); // send stop condition |
| } |
| |
| |
| // Returns a set of acceleration and raw magnetic readings from the cmp01a. |
| void read_data_raw(vector *a, vector *m) |
| { |
| // read accelerometer values |
| i2c_start(); |
| i2c_write_byte(0x30); // write acc |
| i2c_write_byte(0xa8); // OUT_X_L_A, MSB set to enable auto-increment |
| i2c_start(); // repeated start |
| i2c_write_byte(0x31); // read acc |
| unsigned char axl = i2c_read_byte(); |
| unsigned char axh = i2c_read_byte(); |
| unsigned char ayl = i2c_read_byte(); |
| unsigned char ayh = i2c_read_byte(); |
| unsigned char azl = i2c_read_byte(); |
| unsigned char azh = i2c_read_last_byte(); |
| i2c_stop(); |
| |
| // read magnetometer values |
| i2c_start(); |
| i2c_write_byte(0x3C); // write mag |
| i2c_write_byte(0x03); // OUTXH_M |
| i2c_start(); // repeated start |
| i2c_write_byte(0x3D); // read mag |
| unsigned char mxh = i2c_read_byte(); |
| unsigned char mxl = i2c_read_byte(); |
| unsigned char myh = i2c_read_byte(); |
| unsigned char myl = i2c_read_byte(); |
| unsigned char mzh = i2c_read_byte(); |
| unsigned char mzl = i2c_read_last_byte(); |
| i2c_stop(); |
| |
| a->x = axh << 8 | axl; |
| a->y = ayh << 8 | ayl; |
| a->z = azh << 8 | azl; |
| m->x = mxh << 8 | mxl; |
| m->y = myh << 8 | myl; |
| m->z = mzh << 8 | mzl; |
| } |
| |
| float IIR2(float x, float* z) |
| { |
| |
| //const for butterworth lowpass fc 0.5Hz |
| // const float a[3] = {1.0000, -1.8521, 0.8623}; |
| // const float b[3] = {0.0026, 0.0051, 0.0026}; |
| |
| //const for butterworth lowpass fc 2Hz |
| const float a[3] = {1.0000, -1.4190, 0.5533}; |
| const float b[3] = {0.0336, 0.0671, 0.0336}; |
| |
| |
| float y,r; |
| |
| r = a[1]*z[0]+a[2]*z[1]; |
| y = b[0]*(x-r)+b[1]*z[0]+b[2]*z[1]; |
| z[1]= z[0]; |
| z[0]= x-r; |
| |
| return y; |
| |
| } |
| |
| |
| //cancels out movemt below threshold while using step sum to |
| int thr_filter(int x, int * x_reg, int * y_reg) |
| { |
| int y; |
| int diff; |
| int sum = 0; |
| |
| const int thr = 4; |
| const int lmt = 5; |
| |
| diff = x - *x_reg; |
| |
| if(abs(diff) <= thr) |
| { |
| sum += diff; |
| if(abs(sum) >= lmt) |
| { |
| sum = 0; |
| y = x; |
| } |
| else y = *y_reg; |
| } |
| else |
| { |
| y = x; |
| sum = 0; |
| } |
| |
| |
| *x_reg = x; |
| *y_reg = y; |
| |
| return y; |
| } |
| |
| |
| // Returns corrected and low-pass filtered magnetometer and accelerometer values |
| void read_data(vector *a, vector *m) |
| { |
| //interal state buffers for IIR axis filtering |
| static float zm_x[2] = {0.0, 0.0}; |
| static float zm_y[2] = {0.0, 0.0}; |
| static float zm_z[2] = {0.0, 0.0}; |
| static float za_x[2] = {0.0, 0.0}; |
| static float za_y[2] = {0.0, 0.0}; |
| static float za_z[2] = {0.0, 0.0}; |
| |
| |
| read_data_raw(a, m); |
| |
| //low pass filter acc |
| a->x = IIR2(a->x, za_x); |
| a->y = IIR2(a->y, za_y); |
| a->z = IIR2(a->z, za_z); |
| |
| //compensate scale and offset, low pass filter mag |
| m->x = IIR2(((m->x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0), zm_x); |
| m->y = IIR2(((m->y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0), zm_y); |
| m->z = IIR2(((m->z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0), zm_z); |
| } |
| |
| |
| |
| float get_heading(const vector *a, const vector *m, const vector *p) |
| { |
| vector E; |
| vector N; |
| |
| // cross magnetic vector (magnetic north + inclination) with "down" (acceleration vector) to produce "west" |
| // -- right hand rule says |
| |
| vector_cross(m, a, &E); |
| vector_normalize(&E); |
| |
| // cross "down" with "east" to produce "north" (parallel to the ground) |
| vector_cross(a, &E, &N); |
| vector_normalize(&N); |
| |
| // compute heading |
| |
| float heading = atan2(vector_dot(&E, p), vector_dot(&N, p)) * 180.0 / M_PI; |
| return heading; |
| |
| } |
| |
| float get_perpendicular(const vector *a, const vector *d, const vector *q) |
| { |
| |
| |
| float sign = 0.0; |
| vector norma = *a; |
| |
| if (q->x == 0.0) {norma.x = 0.0; sign = norma.y;}// cancel out movement on undesired axis |
| else if (q->y == 0.0) {norma.y = 0.0; sign = norma.x;} |
| vector_normalize(&norma); |
| |
| |
| // compute angle |
| float angle = acos(vector_dot(&norma,d)) * 180.0/M_PI; |
| if(sign >= 0.0) angle *= -1; |
| |
| return angle; |
| |
| } |
| |
| int get_us(float angle, float deg_min, float deg_max, int pwm_min,int pwm_max) |
| { |
| //adjust sign change of angular function to new zero offset |
| if(angle < -180.0) angle += 360.0; |
| if(angle >= 180.0) angle -= 360.0; |
| |
| //crop |
| if(angle < deg_min) angle = deg_min; |
| else if (angle > deg_max) angle = deg_max; |
| |
| //scale to pwm |
| float ratio = ((float)(pwm_max - pwm_min)) / (deg_max - deg_min); |
| int diff = ((int)((angle-deg_min) * ratio)); |
| |
| return pwm_min + diff; |
| } |