From 0dc7f360a4c495a04553df27e6799428798b090f Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Sat, 21 May 2011 11:28:11 +0200 Subject: dos2unix --- main.cpp | 506 +++++++++++++++++++++++++++++++-------------------------------- 1 file changed, 253 insertions(+), 253 deletions(-) (limited to 'main.cpp') diff --git a/main.cpp b/main.cpp index 3bebf74..4d3240c 100644 --- a/main.cpp +++ b/main.cpp @@ -1,253 +1,253 @@ -#include "thread.h" -#include "usbserial.h" -#include "itg3200.h" -#include "bma150.h" -#include "ak8975.h" -#include "ppmsum.h" -#include "motormixer.h" - -#include -#include - -#include "IMU.h" - -class LEDThread : public BaseThread { - public: - noreturn_t thread_main() { - systime_t time = chTimeNow(); // T0 - while (TRUE) { - time += MS2ST(1000); // Next deadline - palClearPad(GPIOA, 5); - chThdSleepUntil(time); - time += MS2ST(1000); // Next deadline - palSetPad(GPIOA, 5); - chThdSleepUntil(time); - } - } -}; - -LEDThread led_thread; - -PPMSum ppmsum; -MotorMixer motors; - -class USBThread : public BaseThread { - private: - typedef enum {W_S, W_N, W_V} w_s_t; - - public: - USBSerial* usbs; - - uint8_t data[9]; - - noreturn_t thread_main() { - for(int i = 0; i < 9; i++) { - data[i] = 0; - } - - w_s_t w_s = W_S; - uint8_t w_n = 0; - - while(1) { - size_t buffer = usbs->getc(); - if(buffer >= 0 && buffer < 256) { - if(w_s == W_S && buffer == 'S') { - w_s = W_N; - } else if(w_s == W_N && buffer >= '1' && buffer <= '9') { - w_s = W_V; - w_n = buffer - '1'; - } else if(w_s == W_V) { - w_s = W_S; - data[w_n] = buffer; - } else { - w_s = W_S; - } - } - } - } -}; - -USBThread usb_thread; -USBSerial usbs; - -#include "foo.h" -#include - -uint8_t syncword[] = {0xff, 0x00, 0xaa, 0x55}; -uint8_t buf[64]; -int16_t* sensordata = (int16_t*)buf; - -template -inline void saturate(T& var, int absmax) { - if(var > absmax) { - var = absmax; - } else if(var < -absmax) { - var = -absmax; - } -} - -class I2CThread : public BaseThread { - public: - ITG3200 gyro; - BMA150 acc; - AK8975 magn; - - int16_t x, y, z; - - noreturn_t thread_main() { - I2CSensor::enable_bus(); - - gyro.init(); - acc.init(); - magn.init(); - - systime_t time = chTimeNow(); - - int32_t pitch_angle_accum = 0; - int32_t roll_angle_accum = 0; - - while (1) { - gyro.update(); - acc.update(); - magn.update(); - x = gyro.x; - y = gyro.y; - z = gyro.z; - - IMUupdate(gyro.x * 0.0012141420883438813, gyro.y * 0.0012141420883438813, gyro.z * 0.0012141420883438813, acc.x, acc.y, acc.z); - - //float pitch = asinf(2*(q0*q2 - q3*q1)); - //int16_t pitch = atan2f(2*(q2*q3 + q0*q1), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767; - //int16_t roll = atan2f(2*(-q1*q3 + q0*q2), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767; - //int16_t yaw = atan2f(2*(q2*q1 + q0*q3), 1 - 2 * (q3*q3 + q2*q2)) / M_PI * 32767; - - float norm_x = 2*(q0*q2 - q1*q3); - float norm_y = 2*(q0*q1 + q2*q3); - float norm_z = (1 - 2*(q1*q1 + q2*q2)); - - float elev = acosf(norm_z); - float azim = atan2f(norm_y, norm_x); - - int16_t pitch = elev * sinf(azim) / M_PI * 32767; - int16_t roll = elev * cosf(azim) / M_PI * 32767; - int16_t yaw = 0; - - sensordata[0] = gyro.x; - sensordata[1] = gyro.y; - sensordata[2] = gyro.z; - sensordata[3] = acc.x; - sensordata[4] = acc.y; - sensordata[5] = acc.z; - sensordata[6] = magn.x; - sensordata[7] = magn.y; - sensordata[8] = magn.z; - sensordata[9] = pitch; - sensordata[10] = roll; - sensordata[11] = yaw; - - usbs.write(syncword, sizeof(syncword)); - usbs.write(buf, sizeof(buf)); - - /*usbprintf(usbs, "%6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d, %6d | %6d, %6d, %6d\r\n", - gyro.x, gyro.y, gyro.z, - acc.x, acc.y, acc.z, - magn.x, magn.y, magn.z, - int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000), - int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*/ - - int16_t pitch_angle_target = (ppmsum.data[1] - 500) * 8; - int16_t roll_angle_target = (ppmsum.data[0] - 500) * 8; - - int16_t pitch_angle_error = pitch_angle_target - pitch; - int16_t roll_angle_error = roll_angle_target - roll; - - // 25 deg max error. - saturate(pitch_angle_error, 4551); - saturate(roll_angle_error, 4551); - - pitch_angle_accum += pitch_angle_error; - roll_angle_accum += roll_angle_error; - - // 20 deg s max error. - saturate(pitch_angle_error, 364088); - saturate(roll_angle_error, 364088); - - int32_t pitch_rate_target = (pitch_angle_error * 4 * 65536 + pitch_angle_accum * 98) >> 16; - int32_t roll_rate_target = (roll_angle_error * 4 * 65536 + roll_angle_accum * 98) >> 16; - - int16_t pitch_rate_comp = ((pitch_rate_target - (gyro.x * 4000 / 360)) * 6 * 36) >> 16; - int16_t roll_rate_comp = ((roll_rate_target - (gyro.y * 4000 / 360)) * 6 * 36) >> 16; - - saturate(pitch_rate_comp, 250); - saturate(roll_rate_comp, 250); - - motors.update(ppmsum.data[2], pitch_rate_comp, roll_rate_comp, 0); - - time += MS2ST(10); - if(time > chTimeNow()) { - chThdSleepUntil(time); - } - } - } -}; - -I2CThread i2c_thread; - -static const ADCConversionGroup adcgrpcfg = { - FALSE, - 2, - 0, - 0, - 0, - 0, - 0, - ADC_SQR1_NUM_CH(2), - 0, - ADC_SQR3_SQ2_N(ADC_CHANNEL_IN14) | ADC_SQR3_SQ1_N(ADC_CHANNEL_IN15) -}; - -class ADCThread : public BaseThread { - private: - adcsample_t adc_samples[2]; - - public: - noreturn_t thread_main() { - adcStart(&ADCD1, NULL); - - systime_t time = chTimeNow(); - while (TRUE) { - adcStartConversion(&ADCD1, &adcgrpcfg, adc_samples, 1); - sensordata[12] = adc_samples[0] * 1265 / 1000; - sensordata[13] = adc_samples[1] * 2201 / 1000; - - time += MS2ST(1000); - chThdSleepUntil(time); - } - } -}; - -ADCThread adc_thread; - -int main(void) { - halInit(); - chSysInit(); - - led_thread.start(); - - ppmsum.start(); - - usbs.init(); - - i2c_thread.start(); - - adc_thread.start(); - - motors.start(); - - usb_thread.usbs = &usbs; - usb_thread.start(); - - while (1) { - chThdSleepMilliseconds(1000); - } -} +#include "thread.h" +#include "usbserial.h" +#include "itg3200.h" +#include "bma150.h" +#include "ak8975.h" +#include "ppmsum.h" +#include "motormixer.h" + +#include +#include + +#include "IMU.h" + +class LEDThread : public BaseThread { + public: + noreturn_t thread_main() { + systime_t time = chTimeNow(); // T0 + while (TRUE) { + time += MS2ST(1000); // Next deadline + palClearPad(GPIOA, 5); + chThdSleepUntil(time); + time += MS2ST(1000); // Next deadline + palSetPad(GPIOA, 5); + chThdSleepUntil(time); + } + } +}; + +LEDThread led_thread; + +PPMSum ppmsum; +MotorMixer motors; + +class USBThread : public BaseThread { + private: + typedef enum {W_S, W_N, W_V} w_s_t; + + public: + USBSerial* usbs; + + uint8_t data[9]; + + noreturn_t thread_main() { + for(int i = 0; i < 9; i++) { + data[i] = 0; + } + + w_s_t w_s = W_S; + uint8_t w_n = 0; + + while(1) { + size_t buffer = usbs->getc(); + if(buffer >= 0 && buffer < 256) { + if(w_s == W_S && buffer == 'S') { + w_s = W_N; + } else if(w_s == W_N && buffer >= '1' && buffer <= '9') { + w_s = W_V; + w_n = buffer - '1'; + } else if(w_s == W_V) { + w_s = W_S; + data[w_n] = buffer; + } else { + w_s = W_S; + } + } + } + } +}; + +USBThread usb_thread; +USBSerial usbs; + +#include "foo.h" +#include + +uint8_t syncword[] = {0xff, 0x00, 0xaa, 0x55}; +uint8_t buf[64]; +int16_t* sensordata = (int16_t*)buf; + +template +inline void saturate(T& var, int absmax) { + if(var > absmax) { + var = absmax; + } else if(var < -absmax) { + var = -absmax; + } +} + +class I2CThread : public BaseThread { + public: + ITG3200 gyro; + BMA150 acc; + AK8975 magn; + + int16_t x, y, z; + + noreturn_t thread_main() { + I2CSensor::enable_bus(); + + gyro.init(); + acc.init(); + magn.init(); + + systime_t time = chTimeNow(); + + int32_t pitch_angle_accum = 0; + int32_t roll_angle_accum = 0; + + while (1) { + gyro.update(); + acc.update(); + magn.update(); + x = gyro.x; + y = gyro.y; + z = gyro.z; + + IMUupdate(gyro.x * 0.0012141420883438813, gyro.y * 0.0012141420883438813, gyro.z * 0.0012141420883438813, acc.x, acc.y, acc.z); + + //float pitch = asinf(2*(q0*q2 - q3*q1)); + //int16_t pitch = atan2f(2*(q2*q3 + q0*q1), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767; + //int16_t roll = atan2f(2*(-q1*q3 + q0*q2), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767; + //int16_t yaw = atan2f(2*(q2*q1 + q0*q3), 1 - 2 * (q3*q3 + q2*q2)) / M_PI * 32767; + + float norm_x = 2*(q0*q2 - q1*q3); + float norm_y = 2*(q0*q1 + q2*q3); + float norm_z = (1 - 2*(q1*q1 + q2*q2)); + + float elev = acosf(norm_z); + float azim = atan2f(norm_y, norm_x); + + int16_t pitch = elev * sinf(azim) / M_PI * 32767; + int16_t roll = elev * cosf(azim) / M_PI * 32767; + int16_t yaw = 0; + + sensordata[0] = gyro.x; + sensordata[1] = gyro.y; + sensordata[2] = gyro.z; + sensordata[3] = acc.x; + sensordata[4] = acc.y; + sensordata[5] = acc.z; + sensordata[6] = magn.x; + sensordata[7] = magn.y; + sensordata[8] = magn.z; + sensordata[9] = pitch; + sensordata[10] = roll; + sensordata[11] = yaw; + + usbs.write(syncword, sizeof(syncword)); + usbs.write(buf, sizeof(buf)); + + /*usbprintf(usbs, "%6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d, %6d | %6d, %6d, %6d\r\n", + gyro.x, gyro.y, gyro.z, + acc.x, acc.y, acc.z, + magn.x, magn.y, magn.z, + int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000), + int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*/ + + int16_t pitch_angle_target = (ppmsum.data[1] - 500) * 8; + int16_t roll_angle_target = (ppmsum.data[0] - 500) * 8; + + int16_t pitch_angle_error = pitch_angle_target - pitch; + int16_t roll_angle_error = roll_angle_target - roll; + + // 25 deg max error. + saturate(pitch_angle_error, 4551); + saturate(roll_angle_error, 4551); + + pitch_angle_accum += pitch_angle_error; + roll_angle_accum += roll_angle_error; + + // 20 deg s max error. + saturate(pitch_angle_error, 364088); + saturate(roll_angle_error, 364088); + + int32_t pitch_rate_target = (pitch_angle_error * 4 * 65536 + pitch_angle_accum * 98) >> 16; + int32_t roll_rate_target = (roll_angle_error * 4 * 65536 + roll_angle_accum * 98) >> 16; + + int16_t pitch_rate_comp = ((pitch_rate_target - (gyro.x * 4000 / 360)) * 6 * 36) >> 16; + int16_t roll_rate_comp = ((roll_rate_target - (gyro.y * 4000 / 360)) * 6 * 36) >> 16; + + saturate(pitch_rate_comp, 250); + saturate(roll_rate_comp, 250); + + motors.update(ppmsum.data[2], pitch_rate_comp, roll_rate_comp, 0); + + time += MS2ST(10); + if(time > chTimeNow()) { + chThdSleepUntil(time); + } + } + } +}; + +I2CThread i2c_thread; + +static const ADCConversionGroup adcgrpcfg = { + FALSE, + 2, + 0, + 0, + 0, + 0, + 0, + ADC_SQR1_NUM_CH(2), + 0, + ADC_SQR3_SQ2_N(ADC_CHANNEL_IN14) | ADC_SQR3_SQ1_N(ADC_CHANNEL_IN15) +}; + +class ADCThread : public BaseThread { + private: + adcsample_t adc_samples[2]; + + public: + noreturn_t thread_main() { + adcStart(&ADCD1, NULL); + + systime_t time = chTimeNow(); + while (TRUE) { + adcStartConversion(&ADCD1, &adcgrpcfg, adc_samples, 1); + sensordata[12] = adc_samples[0] * 1265 / 1000; + sensordata[13] = adc_samples[1] * 2201 / 1000; + + time += MS2ST(1000); + chThdSleepUntil(time); + } + } +}; + +ADCThread adc_thread; + +int main(void) { + halInit(); + chSysInit(); + + led_thread.start(); + + ppmsum.start(); + + usbs.init(); + + i2c_thread.start(); + + adc_thread.start(); + + motors.start(); + + usb_thread.usbs = &usbs; + usb_thread.start(); + + while (1) { + chThdSleepMilliseconds(1000); + } +} -- cgit v1.2.3