From cad960d05606320a8c9634e87ae3975f627aa345 Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Sat, 16 Apr 2011 17:02:30 +0200 Subject: Control thrust with ppm signal. --- main.cpp | 71 ++++++++++++++++++++++++++++++++++------------------------------ 1 file changed, 38 insertions(+), 33 deletions(-) diff --git a/main.cpp b/main.cpp index f8c203f..9eb4594 100644 --- a/main.cpp +++ b/main.cpp @@ -138,38 +138,6 @@ class I2CThread : public BaseThread { I2CThread i2c_thread; -void foo(PWMDriver*) { - - int16_t thrust, pitch, roll, yaw; - - pitch = roll = 0; - - uint8_t shift = 8 - (usb_thread.data[8] >> 3); - - thrust = usb_thread.data[0] * 10; - - yaw = i2c_thread.z >> shift; - - - pwmEnableChannel(&PWMD2, 0, 1000 + thrust + pitch + roll - yaw); - pwmEnableChannel(&PWMD2, 1, 1000 + thrust + pitch - roll + yaw); - pwmEnableChannel(&PWMD2, 2, 1000 + thrust - pitch + roll + yaw); - pwmEnableChannel(&PWMD2, 3, 1000 + thrust - pitch - roll - yaw); -} - -static PWMConfig pwmcfg = { - 1000000, - 1000000 / 50, - foo, - { - {PWM_OUTPUT_ACTIVE_HIGH, NULL}, - {PWM_OUTPUT_ACTIVE_HIGH, NULL}, - {PWM_OUTPUT_ACTIVE_HIGH, NULL}, - {PWM_OUTPUT_ACTIVE_HIGH, NULL} - }, - 0 -}; - icucnt_t last_width, last_period; int16_t ppm_input[4]; @@ -179,7 +147,7 @@ int ppm_n = 0; static void icuwidthcb(ICUDriver *icup) { last_width = icuGetWidthI(icup); - ppm_input[ppm_n] = last_width; + ppm_input[ppm_n] = last_width - 1000; } static void icuperiodcb(ICUDriver *icup) { @@ -234,6 +202,43 @@ class ADCThread : public BaseThread { ADCThread adc_thread; +void foo(PWMDriver*) { + + int16_t thrust, pitch, roll, yaw; + + thrust = ppm_input[2]; + if(thrust < 0) { + thrust = 0; + } + if(thrust > 1000) { + thrust = 1000; + } + + pitch = roll = yaw = 0; + + //pitch = (-i2c_thread.x) >> 5; + //roll = i2c_thread.y >> 5; + //yaw = i2c_thread.z >> 5; + + pwmEnableChannel(&PWMD2, 0, 1000 + thrust + pitch + roll - yaw); + pwmEnableChannel(&PWMD2, 1, 1000 + thrust + pitch - roll + yaw); + pwmEnableChannel(&PWMD2, 2, 1000 + thrust - pitch + roll + yaw); + pwmEnableChannel(&PWMD2, 3, 1000 + thrust - pitch - roll - yaw); +} + +static PWMConfig pwmcfg = { + 1000000, + 1000000 / 50, + foo, + { + {PWM_OUTPUT_ACTIVE_HIGH, NULL}, + {PWM_OUTPUT_ACTIVE_HIGH, NULL}, + {PWM_OUTPUT_ACTIVE_HIGH, NULL}, + {PWM_OUTPUT_ACTIVE_HIGH, NULL} + }, + 0 +}; + int main(void) { halInit(); chSysInit(); -- cgit v1.2.3