Subversion Repositories FlightCtrl

Rev

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

Rev 2088 Rev 2089
Line 11... Line 11...
11 
#include "attitude.h"
 11 
#include "attitude.h"
12 
#include "commands.h"
 12 
#include "commands.h"
13 
#include "output.h"
 13 
#include "output.h"
Line 14... Line 14...
14 
 
 14 
 
15 
// uint16_t maxControl[2] = { 0, 0 };
 15 
// uint16_t maxControl[2] = { 0, 0 };
16 
uint16_t controlActivity = 0;
 16 
// uint16_t controlActivity = 0;
Line 17... Line 17...
17 
int16_t controls[4] = { 0, 0, 0, 0 };
 17 
int16_t controls[4] = { 0, 0, 0, 0 };
18 
 
 18 
 
19 
// Internal variables for reading commands made with an R/C stick.
 19 
// Internal variables for reading commands made with an R/C stick.
Line 79... Line 79...
79 
 
 79 
 
80 
  // This needs not be the only correct solution...
 80 
  // This needs not be the only correct solution...
81 
  return rcQ > ecQ ? rcQ : ecQ;
 81 
  return rcQ > ecQ ? rcQ : ecQ;
Line -... Line 82...
- 
 
 82 
}
82 
}
 83 
 
83 
 
 84 
/*
84 
void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {
 85 
void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {
Line 85... Line 86...
85 
  int16_t tmp = controls[index];
 86 
  int16_t tmp = controls[index];
86 
  controls[index] = newValue;
 87 
  controls[index] = newValue;
87 
 
 88 
 
88 
  tmp -= newValue;
 89 
  tmp -= newValue;
- 
 
 90 
  tmp /= 2;
89 
  tmp /= 2;
 91 
  tmp = tmp * tmp;
90 
  tmp = tmp * tmp;
 92 
  // tmp += (newValue >= 0) ? newValue : -newValue;
91 
  // tmp += (newValue >= 0) ? newValue : -newValue;
 93 
 
92 
  /*
 94 
  / *
93 
  if (controlActivity + (uint16_t)tmp >= controlActivity)
 95 
  if (controlActivity + (uint16_t)tmp >= controlActivity)
94 
    controlActivity += tmp;
 96 
    controlActivity += tmp;
95 
  else controlActivity = 0xffff;
 97 
  else controlActivity = 0xffff;
96 
  */
 98 
  * /
Line 97... Line 99...
97 
  if (controlActivity + (uint16_t)tmp < 0x8000)
 99 
  if (controlActivity + (uint16_t)tmp < 0x8000)
Line 103... Line 105...
103 
  uint32_t tmp = controlActivity;
 105 
  uint32_t tmp = controlActivity;
104 
  tmp *= ((1<<CADAMPING)-1);
 106 
  tmp *= ((1<<CADAMPING)-1);
105 
  tmp >>= CADAMPING;
 107 
  tmp >>= CADAMPING;
106 
  controlActivity = tmp;
 108 
  controlActivity = tmp;
107 
}
 109 
}
- 
 
 110 
*/
Line 108... Line 111...
108 
 
 111 
 
109 
/*
 112 
/*
110 
 * Update the variables indicating stick position from the sum of R/C, GPS and external control
 113 
 * Update the variables indicating stick position from the sum of R/C, GPS and external control
111 
 * and whatever other controls we invented in the meantime...
 114 
 * and whatever other controls we invented in the meantime...
Line 172... Line 175...
172 
    AC_getPRTY(tempPRTY);
 175 
    AC_getPRTY(tempPRTY);
173 
  }
 176 
  }
Line 174... Line 177...
174 
 
 177 
 
175 
  // Commit results to global variable and also measure control activity.
 178 
  // Commit results to global variable and also measure control activity.
176 
  controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE];
 179 
  controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE];
177 
  updateControlAndMeasureControlActivity(CONTROL_PITCH, tempPRTY[CONTROL_PITCH]);
 180 
  controls[CONTROL_PITCH] = tempPRTY[CONTROL_PITCH];
178 
  updateControlAndMeasureControlActivity(CONTROL_ROLL, tempPRTY[CONTROL_ROLL]);
 181 
  controls[CONTROL_ROLL] = tempPRTY[CONTROL_ROLL];
179 
  updateControlAndMeasureControlActivity(CONTROL_YAW, tempPRTY[CONTROL_YAW]);
 182 
  controls[CONTROL_YAW] = tempPRTY[CONTROL_YAW];
Line 180... Line 183...
180 
  dampenControlActivity();
 183 
  // dampenControlActivity();
181 
 
 184 
 
182 
  // We can safely do this even with a bad signal - the variables will not have been updated then.
 - 
 
183 
  configuration_applyVariablesToParams();
 - 
 
184 
 
 - 
 
185 
  // part1a end.
 - 
 
186 
 
 - 
 
187 
  /* This is not really necessary with the dead-band feature on all sticks (see rc.c)
 - 
 
188 
     if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
 - 
 
189 
     if (controlYaw > 2) controlYaw-= 2;
 - 
 
190 
     else if (controlYaw< -2) controlYaw += 2;
 - 
 
191 
     else controlYaw = 0;
 - 
 
192 
     }
 - 
 
193 
  */
 - 
 
194 
 
 - 
 
195 
  /*
 - 
 
196 
   * Record maxima. Predecessor of the control activity stuff.
 - 
 
197 
   for (axis = PITCH; axis <= ROLL; axis++) {
 - 
 
198 
   if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {
 - 
 
199 
   maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
 - 
 
200 
   if (maxControl[axis] > 100)
 - 
 
201 
   maxControl[axis] = 100;
 - 
 
202 
   } else if (maxControl[axis])
 - 
 
203 
   maxControl[axis]--;
 - 
 
204 
   }
 185 
  // We can safely do this even with a bad signal - the variables will not have been updated then.