| 44,7 → 44,7 |
|---|
| * horizontal plane, yaw relative to yaw at start. Not really used for anything else |
| * than diagnostics. |
| */ |
| int32_t angle[2]; |
| int32_t angle[3]; |
| |
| /* |
| * Error integrals. Stick is always positive. Gyro is configurable positive or negative. |
| 52,21 → 52,6 |
|---|
| */ |
| int32_t error[3]; |
| |
| // Yaw angle and compass stuff. |
| // This is updated/written from MM3. Negative angle indicates invalid data. |
| int16_t compassHeading = -1; |
| |
| // This is NOT updated from MM3. Negative angle indicates invalid data. |
| int16_t compassCourse = -1; |
| |
| // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
| // Not necessary. Never read anywhere. |
| // int16_t compassOffCourse = 0; |
| |
| uint8_t updateCompassCourse = 0; |
| uint8_t compassCalState = 0; |
| uint16_t ignoreCompassTimer = 500; |
| |
| int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
| |
| int16_t correctionSum[2] = { 0, 0 }; |
| 119,11 → 104,8 |
|---|
| // reset gyro integrals to acc guessing |
| setStaticAttitudeAngles(); |
| |
| // update compass course to current heading |
| compassCourse = compassHeading; |
| |
| // Inititialize YawGyroIntegral value with current compass heading |
| yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW; |
| angle[YAW] = 0; |
| |
| // Servo_On(); //enable servo output |
| } |
| 159,43 → 141,35 |
|---|
| // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
| uint8_t axis; |
| |
| // TODO: Multiply on a factor. Wont work without... |
| error[PITCH] += control[CONTROL_ELEVATOR]; |
| error[ROLL] += control[CONTROL_AILERONS]; |
| error[YAW] += control[CONTROL_RUDDER]; |
| |
| // TODO: Multiply on a factor on control. Wont work without... |
| if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) { |
| error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
| error[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
| error[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
| } else { |
| error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
| error[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
| error[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
| error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
| error[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
| error[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
| |
| angle[PITCH] += rate_ATT[PITCH]; |
| angle[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
| angle[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
| } else { |
| error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
| error[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
| error[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
| angle[PITCH] += rate_ATT[PITCH]; |
| angle[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
| angle[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
| } |
| |
| for (axis=PITCH; axis<=YAW; axis++) { |
| // TODO: Configurable. |
| #define ERRORLIMIT 1000 |
| for (axis=PITCH; axis<=YAW; axis++) { |
| if (error[axis] > ERRORLIMIT) { |
| error[axis] = ERRORLIMIT; |
| } else if (angle[axis] <= -ERRORLIMIT) { |
| angle[axis] = -ERRORLIMIT; |
| } |
| } |
| } |
| |
| /* |
| * Yaw |
| * Calculate yaw gyro integral (~ to rotation angle) |
| * Limit yawGyroHeading proportional to 0 deg to 360 deg |
| */ |
| yawGyroHeading += ACYawRate; |
| |
| if (yawGyroHeading >= YAWOVER360) { |
| yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
| } else if (yawGyroHeading < 0) { |
| yawGyroHeading += YAWOVER360; |
| } |
| |
| /* |
| * Pitch axis integration and range boundary wrap. |
| */ |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| 206,6 → 180,18 |
|---|
| angle[axis] += PITCHROLLOVER360; |
| } |
| } |
| |
| /* |
| * Yaw |
| * Calculate yaw gyro integral (~ to rotation angle) |
| * Limit yawGyroHeading proportional to 0 deg to 360 deg |
| */ |
| if (angle[YAW] >= YAWOVER360) { |
| angle[YAW] -= YAWOVER360; // 360 deg. wrap |
| } else if (angle[YAW] < 0) { |
| angle[YAW] += YAWOVER360; |
| } |
| |
| } |
| |
| /************************************************************************ |
| 221,7 → 207,7 |
|---|
| // Well actually the Z axis acc. check is not so silly. |
| uint8_t axis; |
| int32_t temp; |
| if (acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
| if (acc[Z] >= -staticParams.accCorrectionZAccLimit && acc[Z] |
| <= dynamicParams.UserParams[7]) { |
| DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
| |