diff options
| -rw-r--r-- | main.cpp | 24 |
1 files changed, 18 insertions, 6 deletions
@@ -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 = {
|