Subversion Repositories FlightCtrl

Rev

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

Rev 2131 Rev 2132
Line 133... Line 133...
133 
        error[axis] = attitude[axis] - target[axis];
 133 
        error[axis] = attitude[axis] - target[axis];
Line 134... Line 134...
134 
 
 134 
 
135 
#define ROTATETARGET 1
 135 
#define ROTATETARGET 1
Line 136... Line 136...
136 
// #define TRUNCATEERROR 1
 136 
// #define TRUNCATEERROR 1
137 
 
 137 
 
138 
#ifdef(ROTATETARGET)
 138 
#ifdef ROTATETARGET
139 
                if(abs(error[axis]) > OVER180) {
 139 
                if(abs(error[axis]) > OVER180) {
140 
                  // The shortest way from attitude to target crosses -180.
 140 
                  // The shortest way from attitude to target crosses -180.
141 
                  // Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative.
 141 
                  // Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative.
142 
                  // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive.
 142 
                  // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive.
143 
                  if (error[axis] > 0) {
 143 
                  if (error[axis] > 0) {
144 
                    if (error[axis] < OVER360 - maxError[axis]) {
 144 
                    if (error[axis] < OVER360 - maxError[axis]) {
145 
                      // too much err.
 145 
                      // too much err.
146 
                      error[axis] = -maxError[axis];
 146 
                      error[axis] = -maxError[axis];
147 
                      target[axis] = attitude[axis] + maxError[axis];
 147 
                      target[axis] = attitude[axis] + maxError[axis];
148 
                      if (target[axis]) > OVER180) target[axis] -= OVER360;
 148 
                      if (target[axis] > OVER180) target[axis] -= OVER360;
149 
                    } else {
 149 
                    } else {
150 
                      // Normal case, we just need to correct for the wrap. Error will be negative.
 150 
                      // Normal case, we just need to correct for the wrap. Error will be negative.
151 
                      error[axis] -= OVER360;
 151 
                      error[axis] -= OVER360;
152 
                    }
 152 
                    }
153 
                  } else {
 153 
                  } else {
154 
            if (error[axis] > maxError[axis] - OVER360) {
 154 
            if (error[axis] > maxError[axis] - OVER360) {
155 
              // too much err.
 155 
              // too much err.
156 
              error[axis] = maxError[axis];
 156 
              error[axis] = maxError[axis];
157 
              target[axis] = attitude[axis] - maxError[axis];
 157 
              target[axis] = attitude[axis] - maxError[axis];
158 
              if (target[axis]) < -OVER180) target[axis] += OVER360;
 158 
              if (target[axis] < -OVER180) target[axis] += OVER360;
159 
            } else {
 159 
            } else {
160 
              // Normal case, we just need to correct for the wrap. Error will be negative.
 160 
              // Normal case, we just need to correct for the wrap. Error will be negative.
161 
              error[axis] += OVER360;
 161 
              error[axis] += OVER360;
Line 170... Line 170...
170 
              error[axis] = -maxError[axis];
 170 
              error[axis] = -maxError[axis];
171 
              target[axis] = attitude[axis] + maxError[axis];
 171 
              target[axis] = attitude[axis] + maxError[axis];
172 
            }
 172 
            }
173 
                }
 173 
                }
174 
#endif
 174 
#endif
175 
#ifdef(TUNCATEERROR)
 175 
#ifdef TUNCATEERROR
176 
                if (error[axis] > maxError[axis]) {
 176 
                if (error[axis] > maxError[axis]) {
177 
                  error[axis] = maxError[axis];
 177 
                  error[axis] = maxError[axis];
178 
                } else if (error[axis] < -maxError[axis]) {
 178 
                } else if (error[axis] < -maxError[axis]) {
179 
                  error[axis] = -maxError[axis];
 179 
                  error[axis] = -maxError[axis];
180 
                } else {
 180 
                } else {
181 
                        // update I parts here for angles mode. I parts in rate mode is something different.
 181 
                        // update I parts here for angles mode. I parts in rate mode is something different.
182 
                }
 182 
                }
183 
#endif
 183 
#endif
Line 184... Line -...
184 
 
 - 
 
185 
                /*
 - 
 
186 
                 * This is the beginning of a version that adjusts the target to differ from the attitude
 - 
 
187 
                 * by a limited amount. This will eliminate memory over large errors but also knock target angles.
 - 
 
188 
                 * Idea: The limit could be calculated from the max. servo deflection divided by I factor...
 - 
 
189 
                 *
 - 
 
190 
                 */
 - 
 
191 
                /*
 - 
 
192 
                if(abs(attitude[axis]-target[axis]) > OVER180) {
 - 
 
193 
                        if(target[axis] > attitude[axis]) {
 - 
 
194 
 
 - 
 
195 
                        }
 - 
 
196 
                }
 - 
 
197 
                */
 - 
 
198 
 
 184 
 
199 
                /************************************************************************/
 185 
                /************************************************************************/
200 
                /* Calculate control feedback from angle (gyro integral)                */
 186 
                /* Calculate control feedback from angle (gyro integral)                */
201 
                /* and angular velocity (gyro signal)                                   */
 187 
                /* and angular velocity (gyro signal)                                   */
202 
                /************************************************************************/
 188 
                /************************************************************************/
Line 209... Line 195...
209 
                        PDPart[axis] = 0;
 195 
                        PDPart[axis] = 0;
210 
                }
 196 
                }
Line 211... Line 197...
211 
 
 197 
 
212 
                if (currentFlightMode == FLIGHT_MODE_ANGLES) {
 198 
                if (currentFlightMode == FLIGHT_MODE_ANGLES) {
213 
                        int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;
 - 
 
214 
                //      if (reverse[axis])
 199 
                        int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;
215 
                        PDPart[axis] += anglePart;
 - 
 
216 
                //      else
 - 
 
217 
                //              PDPart[axis] -= anglePart;
 200 
                        PDPart[axis] += anglePart;
Line 218... Line 201...
218 
                }
 201 
                }
219 
 
 202 
 
220 
                // Add I parts here... these are integrated errors.
 203 
                // Add I parts here... these are integrated errors.