diff options
Diffstat (limited to 'main.cpp')
-rw-r--r-- | main.cpp | 256 |
1 files changed, 0 insertions, 256 deletions
@@ -230,259 +230,3 @@ int main() { } } } - -/* -#include "thread.h" -#include "usbserial.h" -#include "itg3200.h" -#include "bma150.h" -#include "ak8975.h" -#include "ppmsum.h" -#include "motormixer.h" - -#include <ch.h> -#include <hal.h> - -#include "IMU.h" - -class LEDThread : public BaseThread<LEDThread, 128> { - 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<USBThread, 256> { - 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 <cmath> - -uint8_t syncword[] = {0xff, 0x00, 0xaa, 0x55}; -uint8_t buf[64]; -int16_t* sensordata = (int16_t*)buf; - -template<class T> -inline void saturate(T& var, int absmax) { - if(var > absmax) { - var = absmax; - } else if(var < -absmax) { - var = -absmax; - } -} - -class I2CThread : public BaseThread<I2CThread, 256> { - 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_accum, 364088); - saturate(roll_angle_accum, 364088); - - int32_t pitch_rate_target = (pitch_angle_error * 2 * 65536 + pitch_angle_accum * 98) >> 16; - int32_t roll_rate_target = (roll_angle_error * 2 * 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<ADCThread, 128> { - 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); - } -} -*/
\ No newline at end of file |