From b51f54314725b2ed2cdac6da0d30017ae8b4e6c8 Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Thu, 28 Apr 2011 16:23:33 +0200 Subject: Control pitch/roll/yaw with ppm signal. --- main.cpp | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/main.cpp b/main.cpp index 9eb4594..4ce3f66 100644 --- a/main.cpp +++ b/main.cpp @@ -147,7 +147,7 @@ int ppm_n = 0; static void icuwidthcb(ICUDriver *icup) { last_width = icuGetWidthI(icup); - ppm_input[ppm_n] = last_width - 1000; + ppm_input[ppm_n] = last_width - 1050; } static void icuperiodcb(ICUDriver *icup) { @@ -214,16 +214,28 @@ void foo(PWMDriver*) { thrust = 1000; } - pitch = roll = yaw = 0; + pitch = ((ppm_input[1] - 500) * thrust) >> 10; + roll = ((ppm_input[0] - 500) * thrust) >> 10; + yaw = ((ppm_input[3] - 500) * thrust) >> 10; //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); + int16_t motor_1 = thrust + pitch + roll - yaw; + int16_t motor_2 = thrust + pitch - roll + yaw; + int16_t motor_3 = thrust - pitch + roll + yaw; + int16_t motor_4 = thrust - pitch - roll - yaw; + + sensordata[20] = motor_1; + sensordata[21] = motor_2; + sensordata[22] = motor_3; + sensordata[23] = motor_4; + + pwmEnableChannel(&PWMD2, 0, 1000 + motor_1); + pwmEnableChannel(&PWMD2, 1, 1000 + motor_2); + pwmEnableChannel(&PWMD2, 2, 1000 + motor_3); + pwmEnableChannel(&PWMD2, 3, 1000 + motor_4); } static PWMConfig pwmcfg = { -- cgit v1.2.3