summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--main.cpp71
1 files 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, 256> {
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, 128> {
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();