Subversion Repositories FlightCtrl

Rev

Rev 2138 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2138 Rev 2142
Line 21... Line 21...
21 
 
 21 
 
22 
/*
 22 
/*
23 
 * Error integrals.
 23 
 * Error integrals.
Line 24... Line -...
24 
 */
 - 
 
25 
 
 24 
 */
26 
uint8_t reverse[3];
 25 
 
27 
int32_t maxError[3];
 26 
int32_t maxError[3];
Line 28... Line 27...
28 
int32_t IPart[3] = { 0, 0, 0 };
 27 
int32_t IPart[3] = { 0, 0, 0 };
Line 42... Line 41...
42 
        target[YAW] = attitude[YAW];
 41 
        target[YAW] = attitude[YAW];
43 
}
 42 
}
Line 44... Line 43...
44 
 
 43 
 
45 
void flight_updateFlightParametersToFlightMode(void) {
 44 
void flight_updateFlightParametersToFlightMode(void) {
46 
        debugOut.analog[16] = currentFlightMode;
 - 
 
47 
       
 - 
 
48 
        reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;
 - 
 
49 
        reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS;
 - 
 
Line 50... Line 45...
50 
        reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER;
 45 
        debugOut.analog[16] = currentFlightMode;
51 
 
 46 
 
52 
        // At a switch to angles, we want to kill errors first.
 47 
        // At a switch to angles, we want to kill errors first.
53 
        // This should be triggered only once per mode change!
 48 
        // This should be triggered only once per mode change!
Line 83... Line 78...
83 
                airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be???
 78 
                airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be???
84 
                airspeedPID[axis].D = dynamicParams.gyroPID[axis].D;
 79 
                airspeedPID[axis].D = dynamicParams.gyroPID[axis].D;
85 
        }
 80 
        }
86 
}
 81 
}
Line 87... Line -...
87 
 
 - 
 
88 
#define LOG_STICK_SCALE 8
 - 
 
89 
#define LOG_P_SCALE 6
 - 
 
90 
#define LOG_I_SCALE 10
 - 
 
91 
#define LOG_D_SCALE 6
 - 
 
92 
 
 82 
 
93 
/************************************************************************/
 83 
/************************************************************************/
94 
/*  Main Flight Control                                                 */
 84 
/*  Main Flight Control                                                 */
95 
/************************************************************************/
 85 
/************************************************************************/
96 
void flight_control(void) {
 86 
void flight_control(void) {
Line 113... Line 103...
113 
 
 103 
 
114 
        // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
 104 
        // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
Line 115... Line 105...
115 
        int32_t tmp;
 105 
        int32_t tmp;
116 
 
 106 
 
Line 117... Line 107...
117 
        tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE;
 107 
        tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE;
118 
        if (reverse[PITCH]) target[PITCH] += tmp; else target[PITCH] -= tmp;
 108 
        if (staticParams.servos[PITCH].reverse) target[PITCH] += tmp; else target[PITCH] -= tmp;
Line 119... Line 109...
119 
 
 109 
 
120 
        tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE;
 110 
        tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE;
Line 121... Line 111...
121 
        if (reverse[ROLL]) target[ROLL] += tmp; else target[ROLL] -= tmp;
 111 
        if (staticParams.servos[ROLL].reverse) target[ROLL] += tmp; else target[ROLL] -= tmp;
122 
 
 112 
 
123 
        tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE;
 113 
        tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE;
124 
        if (reverse[YAW]) target[YAW] += tmp; else target[YAW] -= tmp;
 114 
        if (staticParams.servos[YAW].reverse) target[YAW] += tmp; else target[YAW] -= tmp;
Line 190... Line 180...
190 
                /* and angular velocity (gyro signal)                                   */
 180 
                /* and angular velocity (gyro signal)                                   */
191 
                /************************************************************************/
 181 
                /************************************************************************/
192 
                if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode == FLIGHT_MODE_RATE) {
 182 
                if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode == FLIGHT_MODE_RATE) {
193 
                        PDPart[axis] = +(((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE)
 183 
                        PDPart[axis] = +(((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE)
194 
                                + (((int16_t)gyroD[axis] * (int16_t) airspeedPID[axis].D) >> LOG_D_SCALE);
 184 
                                + (((int16_t)gyroD[axis] * (int16_t) airspeedPID[axis].D) >> LOG_D_SCALE);
195 
                        //if (reverse[axis])
 - 
 
196 
                        //      PDPart[axis] = -PDPart[axis];
 - 
 
197 
                } else {
 185 
                } else {
198 
                        PDPart[axis] = 0;
 186 
                        PDPart[axis] = 0;
199 
                }
 187 
                }
Line 200... Line 188...
200 
 
 188 
 
201 
                if (currentFlightMode == FLIGHT_MODE_ANGLES) {
 189 
                if (currentFlightMode == FLIGHT_MODE_ANGLES) {
202 
                        int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;
 190 
                        int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;
203 
                        PDPart[axis] += anglePart;
 191 
                        PDPart[axis] += anglePart;
Line 204... Line 192...
204 
                }
 192 
                }
205 
 
 193 
 
206 
                // Add I parts here... these are integrated errors.
 194 
                // Add I parts here... these are integrated errors.
207 
                if (reverse[axis])
 195 
                if (staticParams.servos[axis].reverse)
208 
                  term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis];
 196 
                  term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis];
209 
                else
 197 
                else
Line 210... Line -...
210 
                  term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis];
 - 
 
211 
        }
 - 
 
212 
 
 - 
 
213 
        debugOut.analog[12] = term[PITCH];
 - 
 
214 
        debugOut.analog[13] = term[ROLL];
 - 
 
215 
        debugOut.analog[14] = term[YAW];
 198 
                  term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis];
216 
        debugOut.analog[15] = term[THROTTLE];
 199 
        }
217 
 
 200 
 
218 
        for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {
 201 
        for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {
219 
                int16_t tmp;
 202 
                int16_t tmp;
Line 262... Line 245...
262 
                debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10);
 245 
                debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10);
Line 263... Line 246...
263 
 
 246 
 
264 
                debugOut.analog[12] = term[PITCH];
 247 
                debugOut.analog[12] = term[PITCH];
265 
                debugOut.analog[13] = term[ROLL];
 248 
                debugOut.analog[13] = term[ROLL];
- 
 
 249 
                debugOut.analog[14] = term[YAW];
266 
                debugOut.analog[14] = term[YAW];
 250 
            debugOut.analog[15] = term[THROTTLE];
267 
        }
 251 
        }