Lightweight FlyBed1, structure based on KK-Board Firmware First this one should work, then I can get more complex FlyBed1 working :)
Diff: main.cpp
- Revision:
- 2:c2ebab7c189f
- Parent:
- 1:b4f1227a0ff6
- Child:
- 3:9cde4e9740fc
--- a/main.cpp Sat Jun 15 14:29:26 2013 +0000 +++ b/main.cpp Mon Jun 24 14:19:15 2013 +0000 @@ -6,7 +6,8 @@ // Defines #define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz) -#define RC_SENSITIVITY 0.2 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz) +#define RC_SENSITIVITY 0.3 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz) +#define YAW_SENSITIVITY 0.4 // RC #define AILERON 0 #define ELEVATOR 1 @@ -47,7 +48,7 @@ bool armed = false; float ESC_value[4] = {0,0,0,0}; float Error[3] = {0,0,0}; -float P[3] = {1.2,1.2,1.2}; +float P[3] = {1.45,1.45,1.45}; // Timing float dt = 0; float time_for_dt = 0; @@ -138,6 +139,7 @@ Gyro[i] *= 0.07; // make result degree per second // fill ringbuffer + /* Gyro_history[i][Gyro_history_index] = Gyro[i]; // save newest value to ringbuffer if (Gyro_history_index < 5-1) Gyro_history_index++; @@ -150,16 +152,19 @@ sum += Gyro_history[i][j]; } Gyro[i] = sum/5; - + */ Gyro_sum[i] += Gyro[i]*dt; // integrate speed to get angle } // calculate ESC ------------------------------------------------------------------------------------------------------------- - for (int i = 0; i < 3; i++) - if (RC[i].read() != -100) // only count RC when it's available - Error[i] = ((RC[i].read() - 500)*RC_SENSITIVITY - Gyro[i]) * P[i]; - else - Error[i] = (- Gyro[i]) * P[i]; + + if (RC[ROLL].read() != -100) { // only count RC when it's available + Error[ROLL] = ((RC[ROLL].read() - 500)*RC_SENSITIVITY - Gyro[ROLL]) * P[ROLL]; + Error[PITCH] = ((RC[PITCH].read() - 500)*RC_SENSITIVITY - Gyro[PITCH]) * P[PITCH]; + Error[YAW] = ((RC[YAW].read() - 500)*YAW_SENSITIVITY - Gyro[YAW]) * P[YAW]; + } else + for (int i = 0; i < 3; i++) + Error[i] = (- Gyro[i]) * P[i]; for (int i = 0; i < 4; i++) ESC_value[i] = RC[THROTTLE].read(); @@ -242,4 +247,4 @@ while(1) { loop(); } -} +} \ No newline at end of file