Subversion Repositories FlightCtrl

Rev

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

Rev 2102 Rev 2103
Line 6... Line 6...
6 
 
 6 
 
7 
// Necessary for external control and motor test
 7 
// Necessary for external control and motor test
Line 8... Line 8...
8 
#include "uart0.h"
 8 
#include "uart0.h"
- 
 
 9 
 
9 
 
 10 
#include "timer2.h"
10 
#include "timer2.h"
 11 
#include "analog.h"
Line 11... Line 12...
11 
#include "attitude.h"
 12 
#include "attitude.h"
Line 42... Line 43...
42 
        target[ROLL] = attitude[ROLL];
 43 
        target[ROLL] = attitude[ROLL];
43 
        target[YAW] = attitude[YAW];
 44 
        target[YAW] = attitude[YAW];
44 
}
 45 
}
Line 45... Line 46...
45 
 
 46 
 
46 
// this should be followed by a call to switchToFlightMode!!
 47 
// this should be followed by a call to switchToFlightMode!!
47 
void updateFlightParametersToFlightMode(uint8_t flightMode) {
 48 
void flight_updateFlightParametersToFlightMode(uint8_t flightMode) {
Line 48... Line 49...
48 
        debugOut.analog[15] = flightMode;
 49 
        debugOut.analog[16] = flightMode;
49 
 
 50 
 
50 
        reverse[CONTROL_ELEVATOR] = staticParams.controlServosReverse
 51 
        reverse[PITCH] = staticParams.controlServosReverse
51 
                        & CONTROL_SERVO_REVERSE_ELEVATOR;
 52 
                        & CONTROL_SERVO_REVERSE_ELEVATOR;
52 
        reverse[CONTROL_AILERONS] = staticParams.controlServosReverse
 53 
        reverse[ROLL] = staticParams.controlServosReverse
53 
                        & CONTROL_SERVO_REVERSE_AILERONS;
 54 
                        & CONTROL_SERVO_REVERSE_AILERONS;
Line 54... Line 55...
54 
        reverse[CONTROL_RUDDER] = staticParams.controlServosReverse
 55 
        reverse[YAW] = staticParams.controlServosReverse
55 
                        & CONTROL_SERVO_REVERSE_RUDDER;
 56 
                        & CONTROL_SERVO_REVERSE_RUDDER;
56 
 
 57 
 
Line 74... Line 75...
74 
/************************************************************************/
 75 
/************************************************************************/
75 
/*  Main Flight Control                                                 */
 76 
/*  Main Flight Control                                                 */
76 
/************************************************************************/
 77 
/************************************************************************/
77 
void flight_control(void) {
 78 
void flight_control(void) {
78 
        // Mixer Fractions that are combined for Motor Control
 79 
        // Mixer Fractions that are combined for Motor Control
79 
        int16_t throttleTerm, term[3];
 80 
        int16_t term[4];
Line 80... Line 81...
80 
 
 81 
 
81 
        // PID controller variables
 82 
        // PID controller variables
Line 82... Line 83...
82 
        int16_t PDPart[3];
 83 
        int16_t PDPart[3];
Line 90... Line 91...
90 
 
 91 
 
91 
        // TODO: Check modern version.
 92 
        // TODO: Check modern version.
92 
        // calculateFlightAttitude();
 93 
        // calculateFlightAttitude();
93 
        // TODO: Check modern version.
 94 
        // TODO: Check modern version.
94 
        // controlMixer_update();
 95 
        // controlMixer_update();
Line 95... Line 96...
95 
        throttleTerm = controls[CONTROL_THROTTLE];
 96 
        term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE];
96 
 
 97 
 
97 
        // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
 98 
        // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
98 
        target[PITCH] += controls[CONTROL_ELEVATOR] * staticParams.stickIElevator;
 99 
        target[PITCH] += (controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 6;
Line 99... Line 100...
99 
        target[ROLL] += controls[CONTROL_AILERONS] * staticParams.stickIAilerons;
 100 
        target[ROLL] += (controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 6;
100 
        target[YAW] += controls[CONTROL_RUDDER] * staticParams.stickIRudder;
 101 
        target[YAW] += (controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 6;
101 
 
 102 
 
102 
        for (axis = PITCH; axis <= YAW; axis++) {
 103 
        for (axis = PITCH; axis <= YAW; axis++) {
103 
                if (target[axis] > OVER180) {
 104 
                if (target[axis] > OVER180) {
104 
                        target[axis] -= OVER360;
 105 
                        target[axis] -= OVER360;
Line 105... Line 106...
105 
                } else if (attitude[axis] <= -OVER180) {
 106 
                } else if (target[axis] <= -OVER180) {
106 
                        attitude[axis] += OVER360;
 107 
                  target[axis] += OVER360;
107 
                }
 108 
                }
Line 118... Line 119...
118 
 
 119 
 
119 
                /************************************************************************/
 120 
                /************************************************************************/
120 
                /* Calculate control feedback from angle (gyro integral)                */
 121 
                /* Calculate control feedback from angle (gyro integral)                */
121 
                /* and angular velocity (gyro signal)                                   */
 122 
                /* and angular velocity (gyro signal)                                   */
122 
                /************************************************************************/
 123 
                /************************************************************************/
123 
                PDPart[axis] = (((int32_t) rate_PID[axis] * pFactor[axis]) >> 6)
 124 
                PDPart[axis] = (((int32_t) gyro_PID[axis] * pFactor[axis]) >> 6)
124 
                                + ((differential[axis] * (int16_t) dFactor[axis]) >> 4);
 125 
                                + ((gyroD[axis] * (int16_t) dFactor[axis]) >> 4);
125 
                if (reverse[axis])
 126 
                if (reverse[axis])
Line 126... Line 127...
126 
                        PDPart[axis] = -PDPart[axis];
 127 
                        PDPart[axis] = -PDPart[axis];
127 
 
 128 
 
Line 137... Line 138...
137 
                term[axis] = controls[axis] + PDPart[axis] + IPart[axis];
 138 
                term[axis] = controls[axis] + PDPart[axis] + IPart[axis];
138 
        }
 139 
        }
Line 139... Line 140...
139 
 
 140 
 
140 
        debugOut.analog[12] = term[PITCH];
 141 
        debugOut.analog[12] = term[PITCH];
141 
        debugOut.analog[13] = term[ROLL];
 142 
        debugOut.analog[13] = term[ROLL];
142 
        debugOut.analog[14] = throttleTerm;
 143 
        debugOut.analog[14] = term[YAW];
Line 143... Line 144...
143 
        debugOut.analog[15] = term[YAW];
 144 
        debugOut.analog[15] = term[THROTTLE];
144 
 
 145 
 
145 
        for (uint8_t i = 0; i < MAX_CONTROL_SERVOS; i++) {
 146 
        for (uint8_t i = 0; i < MAX_CONTROL_SERVOS; i++) {
146 
                int16_t tmp;
 147 
                int16_t tmp;
Line 154... Line 155...
154 
                                break;
 155 
                                break;
155 
                        case 1:
 156 
                        case 1:
156 
                                tmp = term[PITCH];
 157 
                                tmp = term[PITCH];
157 
                                break;
 158 
                                break;
158 
                        case 2:
 159 
                        case 2:
159 
                                tmp = throttleTerm;
 160 
                                tmp = term[THROTTLE];
160 
                                break;
 161 
                                break;
161 
                        case 3:
 162 
                        case 3:
162 
                                tmp = term[YAW];
 163 
                                tmp = term[YAW];
163 
                                break;
 164 
                                break;
164 
                        default:
 165 
                        default:
Line 167... Line 168...
167 
                        // These are all signed and in the same units as the RC stuff in rc.c.
 168 
                        // These are all signed and in the same units as the RC stuff in rc.c.
168 
                        controlServos[i] = tmp;
 169 
                        controlServos[i] = tmp;
169 
                }
 170 
                }
170 
        }
 171 
        }
Line 171... Line 172...
171 
 
 172 
 
Line 172... Line 173...
172 
        calculateControlServoValues;
 173 
        calculateControlServoValues();
173 
 
 174 
 
174 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 175 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
175 
        // Debugging
 176 
        // Debugging
176 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 177 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
177 
        if (!(--debugDataTimer)) {
 178 
        if (!(--debugDataTimer)) {
178 
                debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
 179 
                debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
179 
                debugOut.analog[0] = rate_PID[PITCH]; // in 0.1 deg
 180 
                debugOut.analog[0] = gyro_PID[PITCH]; // in 0.1 deg
Line 180... Line 181...
180 
                debugOut.analog[1] = rate_PID[ROLL]; // in 0.1 deg
 181 
                debugOut.analog[1] = gyro_PID[ROLL]; // in 0.1 deg
181 
                debugOut.analog[2] = rate_PID[YAW];
 182 
                debugOut.analog[2] = gyro_PID[YAW];
182 
 
 183