| 0,0 → 1,169 |
|---|
| #include <avr/io.h> |
| #include <math.h> |
| #include <stdlib.h> |
| #include "vector.h" |
| #include <util/delay.h> |
| #include <inttypes.h> |
| #include <avr/interrupt.h> |
| #include "headtracker.h" |
| |
| #if(DEBUG) |
| #include "UART.h" |
| #include <string.h> |
| #endif |
| |
| |
| volatile uint16_t ppm_ch[PPM_CHANNELS]; |
| |
| enum calibration_mode {CAL_NONE, CAL_X, CAL_Y, CAL_Z}; |
| |
| void ppm_init(void) |
| { |
| |
| for(int i = 0; i<PPM_CHANNELS; i++) ppm_ch[i] = DEFAULT_US; |
| |
| //TCNT 1 - PPM out - |
| DDRD |= PPM_PIN; |
| TIMSK |= (1<<OCIE1B); |
| |
| TCNT1 = 0; |
| OCR1A = ppm_ch[0]; |
| OCR1B = PPM_SYNC_PW; |
| TCCR1A |= (1<<COM1B1) | (PPM_INV<<COM1B0) | (1<<WGM11) | (1<<WGM10); |
| TCCR1B |= (1<<WGM13) | (1<<WGM12); |
| TCCR1B |= (0<<CS12) | (1<<CS11) | (0<<CS10 ); |
| |
| sei(); |
| |
| } |
| |
| |
| ISR(TIMER1_COMPB_vect) |
| { |
| static uint8_t i = 0; |
| static uint16_t frame_pw = PPM_FRAME; |
| |
| //output PPM channels |
| ++i; |
| |
| if(i >= PPM_CHANNELS+1) |
| { |
| i = 0; |
| frame_pw = PPM_FRAME; |
| } |
| |
| if(i < PPM_CHANNELS) |
| { |
| |
| OCR1A = ppm_ch[i]; |
| frame_pw -= OCR1A; |
| } |
| else |
| { |
| OCR1A = frame_pw; |
| } |
| |
| } |
| |
| |
| |
| int main() |
| { |
| _delay_ms(500); //delay for VCC stabilization |
| |
| ppm_init(); |
| |
| #if(DEBUG) |
| uart_init(); |
| #endif |
| |
| //************************************************************************ |
| //** init LSM303DLH ** |
| //************************************************************************ |
| DDRC = 0; // all inputs |
| PORTC = (1 << PORTC4) | (1 << PORTC5); // enable pull-ups on SDA and SCL |
| |
| |
| TWSR = 0; // clear bit-rate prescale bits |
| TWBR = 17; |
| |
| //enable accelerometer |
| i2c_start(); |
| i2c_write_byte(0x30); // write acc |
| i2c_write_byte(0x20); // CTRL_REG1_A |
| i2c_write_byte(0x27); // normal power mode, 50 Hz data rate, all axes enabled |
| i2c_stop(); |
| |
| //enable magnetometer |
| i2c_start(); |
| i2c_write_byte(0x3C); // write mag |
| i2c_write_byte(0x02); // MR_REG_M |
| i2c_write_byte(0x00); // continuous conversion mode |
| i2c_stop(); |
| |
| //raw acc and mag |
| vector a, m; |
| //filtered acc and mag |
| vector a_avg, m_avg; |
| |
| //IIR filter will overshoot at startup, read in a couple of values to get it stable |
| for(int i = 0; i<20; i++) read_data(&a_avg, &m_avg); |
| |
| //get zero reference |
| float heading_start, pitch_start, roll_start; |
| heading_start = get_heading(&a_avg, &m_avg, &p); |
| pitch_start = get_perpendicular(&a_avg, &down, &p); |
| roll_start = get_perpendicular(&a_avg, &down, &right); |
| |
| enum calibration_mode mode = CAL_NONE; |
| vector cal_m_max = {0, 0, 0}; |
| vector cal_m_min = {0, 0, 0}; |
| |
| while(1) |
| { |
| |
| if (mode == CAL_NONE) // run mode |
| { |
| |
| int thr_in, thr_out; |
| |
| read_data(&a_avg, &m_avg); |
| |
| //get angle - startoffset |
| int heading_us = thr_filter(get_us(get_heading(&a_avg, &m_avg, &p) - heading_start, HDG_MIN, HDG_MAX, HDG_US_MIN, HDG_US_MAX),&thr_in, &thr_out); |
| int pitch_us = get_us(get_perpendicular(&a_avg, &down, &p) - pitch_start, PIT_MIN, PIT_MAX, PIT_US_MIN, PIT_US_MAX); |
| int roll_us = get_us(get_perpendicular(&a_avg, &down, &right) - roll_start, ROL_MIN, ROL_MAX, ROL_US_MIN, ROL_US_MAX);; |
| |
| #if(DEBUG) |
| printf("H %d P %d R %d \n", heading_us, pitch_us, roll_us); |
| #endif |
| |
| |
| ppm_ch[HDG_CH-1] = heading_us; //subtract 1, CHs from 1 - N, array bounds from 0 - N-1 |
| ppm_ch[PIT_CH-1] = pitch_us; |
| ppm_ch[ROL_CH-1] = roll_us; |
| } |
| |
| |
| else // calibration mode - record max/min measurements |
| { |
| //TODO |
| //implement calib_test.c |
| |
| |
| read_data_raw(&a, &m); |
| if (m.x < cal_m_min.x) cal_m_min.x = m.x; |
| if (m.x > cal_m_max.x) cal_m_max.x = m.x; |
| if (m.y < cal_m_min.y) cal_m_min.y = m.y; |
| if (m.y > cal_m_max.y) cal_m_max.y = m.y; |
| if (m.z < cal_m_min.z) cal_m_min.z = m.z; |
| if (m.z > cal_m_max.z) cal_m_max.z = m.z; |
| |
| |
| |
| } |
| |
| _delay_ms(40); |
| } |
| } |
| |