Subversion Repositories FlightCtrl

Rev

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

Rev 1924 Rev 1926
Line 42... Line 42...
42 
/*
 42 
/*
43 
 * Gyro integrals. These are the rotation angles of the airframe compared to the
 43 
 * Gyro integrals. These are the rotation angles of the airframe compared to the
44 
 * horizontal plane, yaw relative to yaw at start. Not really used for anything else
 44 
 * horizontal plane, yaw relative to yaw at start. Not really used for anything else
45 
 * than diagnostics.
 45 
 * than diagnostics.
46 
 */
 46 
 */
47 
int32_t angle[2], yawAngleDiff;
 47 
int32_t angle[2];
Line 48... Line 48...
48 
 
 48 
 
49 
/*
 49 
/*
50 
 * Error integrals. Stick is always positive. Gyro is configurable positive or negative.
 50 
 * Error integrals. Stick is always positive. Gyro is configurable positive or negative.
51 
 * These represent the deviation of the attitude angle from the desired on each axis.
 51 
 * These represent the deviation of the attitude angle from the desired on each axis.
Line 66... Line 66...
66 
uint8_t updateCompassCourse = 0;
 66 
uint8_t updateCompassCourse = 0;
67 
uint8_t compassCalState = 0;
 67 
uint8_t compassCalState = 0;
68 
uint16_t ignoreCompassTimer = 500;
 68 
uint16_t ignoreCompassTimer = 500;
Line 69... Line 69...
69 
 
 69 
 
70 
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass
 - 
 
Line 71... Line 70...
71 
int16_t yawGyroDrift;
 70 
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass
Line 72... Line 71...
72 
 
 71 
 
73 
int16_t correctionSum[2] = { 0, 0 };
 72 
int16_t correctionSum[2] = { 0, 0 };
Line 109... Line 108...
109 
/************************************************************************
 108 
/************************************************************************
110 
 * Neutral Readings                                                    
 109 
 * Neutral Readings                                                    
111 
 ************************************************************************/
 110 
 ************************************************************************/
112 
void attitude_setNeutral(void) {
 111 
void attitude_setNeutral(void) {
113 
  // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
 112 
  // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
114 
  driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0;
 113 
  driftComp[PITCH] = driftComp[ROLL];
115 
  correctionSum[PITCH] = correctionSum[ROLL] = 0;
 114 
  correctionSum[PITCH] = correctionSum[ROLL] = 0;
Line 116... Line 115...
116 
 
 115 
 
117 
  // Calibrate hardware.
 116 
  // Calibrate hardware.
Line 118... Line 117...
118 
  analog_calibrate();
 117 
  analog_calibrate();
119 
 
 118 
 
120 
  // reset gyro integrals to acc guessing
 - 
 
Line 121... Line 119...
121 
  setStaticAttitudeAngles();
 119 
  // reset gyro integrals to acc guessing
122 
  yawAngleDiff = 0;
 120 
  setStaticAttitudeAngles();
Line 123... Line 121...
123 
 
 121 
 
Line 159... Line 157...
159 
 
 157 
 
160 
void integrate(void) {
 158 
void integrate(void) {
161 
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
 159 
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
Line -... Line 160...
- 
 
 160 
  uint8_t axis;
162 
  uint8_t axis;
 161 
 
163 
 
 162 
    // TODO: Multiply on a factor. Wont work without...
164 
  error[PITCH] += control[CONTROL_ELEVATOR];
 163 
  error[PITCH] += control[CONTROL_ELEVATOR];
Line 165... Line 164...
165 
  error[ROLL] += control[CONTROL_AILERONS];
 164 
  error[ROLL] += control[CONTROL_AILERONS];
166 
  error[YAW] += control[CONTROL_RUDDER];
 165 
  error[YAW] += control[CONTROL_RUDDER];
167 
   
 166 
   
168 
  if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) {
 167 
  if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) {
169 
      error[PITCH] += (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
 168 
      error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
170 
      error[ROLL]  += (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
 169 
      error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
171 
      error[YAW]   += (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
 170 
      error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
172 
  } else {
 171 
  } else {
173 
      error[PITCH] += (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
 172 
      error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
Line 174... Line 173...
174 
      error[ROLL]  += (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
 173 
      error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
175 
      error[YAW]   += (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
 174 
      error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
176 
  }
 175 
  }
Line 187... Line 186...
187 
   * Yaw
 186 
   * Yaw
188 
   * Calculate yaw gyro integral (~ to rotation angle)
 187 
   * Calculate yaw gyro integral (~ to rotation angle)
189 
   * Limit yawGyroHeading proportional to 0 deg to 360 deg
 188 
   * Limit yawGyroHeading proportional to 0 deg to 360 deg
190 
   */
 189 
   */
191 
  yawGyroHeading += ACYawRate;
 190 
  yawGyroHeading += ACYawRate;
192 
  yawAngleDiff += yawRate;
 - 
 
Line 193... Line 191...
193 
 
 191 
 
194 
  if (yawGyroHeading >= YAWOVER360) {
 192 
  if (yawGyroHeading >= YAWOVER360) {
195 
    yawGyroHeading -= YAWOVER360; // 360 deg. wrap
 193 
    yawGyroHeading -= YAWOVER360; // 360 deg. wrap
196 
  } else if (yawGyroHeading < 0) {
 194 
  } else if (yawGyroHeading < 0) {