| /branches/dongfang_FC_rewrite/attitude.c |
|---|
| 1,63 → 1,9 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + Nur f�r den privaten Gebrauch |
| // + www.MikroKopter.com |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation), |
| // + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zul�ssig ist. |
| // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
| // + bzgl. der Nutzungsbedingungen aufzunehmen. |
| // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen, |
| // + Verkauf von Luftbildaufnahmen, usw. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht, |
| // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
| // + auf anderen Webseiten oder Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt und genannt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zul�ssig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| /************************************************************************/ |
| /* Flight Attitude */ |
| /************************************************************************/ |
| #include <stdlib.h> |
| #include <avr/io.h> |
| #include "attitude.h" |
| #include "dongfangMath.h" |
| #include "commands.h" |
| // For scope debugging only! |
| #include "rc.h" |
| 102,9 → 48,9 |
| * Gyro integrals. These are the rotation angles of the airframe compared to the |
| * horizontal plane, yaw relative to yaw at start. |
| */ |
| int32_t angle[2], yawAngleDiff; |
| int32_t attitude[2]; |
| int readingHeight = 0; |
| //int readingHeight = 0; |
| // Yaw angle and compass stuff. |
| 112,23 → 58,19 |
| int16_t magneticHeading = -1; |
| // This is NOT updated from MM3. Negative angle indicates invalid data. |
| int16_t compassCourse = -1; |
| // int16_t headingInDegrees = -1; |
| int32_t targetHeading; |
| // 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 |
| int32_t heading; // Yaw Gyro Integral supported by compass |
| int16_t yawGyroDrift; |
| #define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L) |
| #define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
| #define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L) |
| int16_t correctionSum[2] = { 0, 0 }; |
| // For NaviCTRL use. |
| 156,16 → 98,16 |
| int32_t getAngleEstimateFromAcc(uint8_t axis) { |
| //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
| return (int32_t)GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis];// + correctionTerm; |
| return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; // + correctionTerm; |
| // return 342L * filteredAcc[axis]; |
| } |
| void setStaticAttitudeAngles(void) { |
| #ifdef ATTITUDE_USE_ACC_SENSORS |
| angle[PITCH] = getAngleEstimateFromAcc(PITCH); |
| angle[ROLL] = getAngleEstimateFromAcc(ROLL); |
| attitude[PITCH] = getAngleEstimateFromAcc(PITCH); |
| attitude[ROLL] = getAngleEstimateFromAcc(ROLL); |
| #else |
| angle[PITCH] = angle[ROLL] = 0; |
| attitude[PITCH] = attitude[ROLL] = 0; |
| #endif |
| } |
| 184,14 → 126,7 |
| // reset gyro integrals to acc guessing |
| setStaticAttitudeAngles(); |
| yawAngleDiff = 0; |
| // update compass course to current heading |
| compassCourse = magneticHeading; |
| // Inititialize YawGyroIntegral value with current compass heading |
| yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
| attitude_resetHeadingToMagnetic(); |
| // Servo_On(); //enable servo output |
| } |
| 224,22 → 159,28 |
| * changed accordingly. |
| */ |
| void trigAxisCoupling(void) { |
| int16_t rollAngleInDegrees = angle[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
| int16_t pitchAngleInDegrees = angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
| int16_t rollAngleInDegrees = attitude[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
| int16_t pitchAngleInDegrees = attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
| int16_t cospitch = cos_360(pitchAngleInDegrees); |
| int16_t cosroll = cos_360(rollAngleInDegrees); |
| int16_t sinroll = sin_360(rollAngleInDegrees); |
| ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate |
| * sinroll) >> LOG_MATH_UNIT_FACTOR); |
| ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll |
| - (int32_t) yawRate * sinroll) >> LOG_MATH_UNIT_FACTOR); |
| ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
| + (int32_t)yawRate * cosroll) >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees)) >> LOG_MATH_UNIT_FACTOR); |
| ACRate[ROLL] = rate_ATT[ROLL] |
| + (((((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
| >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees)) |
| >> LOG_MATH_UNIT_FACTOR); |
| ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
| ACYawRate = |
| ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
| / cospitch; |
| ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
| ACYawRate = |
| ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
| / cospitch; |
| } |
| // 480 usec with axis coupling - almost no time without. |
| 258,26 → 199,19 |
| /* |
| * Yaw |
| * Calculate yaw gyro integral (~ to rotation angle) |
| * Limit yawGyroHeading proportional to 0 deg to 360 deg |
| * Limit heading proportional to 0 deg to 360 deg |
| */ |
| yawGyroHeading += ACYawRate; |
| yawAngleDiff += yawRate; |
| if (yawGyroHeading >= YAWOVER360) { |
| yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
| } else if (yawGyroHeading < 0) { |
| yawGyroHeading += YAWOVER360; |
| } |
| heading += ACYawRate; |
| intervalWrap(&heading, YAWOVER360); |
| /* |
| * Pitch axis integration and range boundary wrap. |
| */ |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| angle[axis] += ACRate[axis]; |
| if (angle[axis] > PITCHROLLOVER180) { |
| angle[axis] -= PITCHROLLOVER360; |
| } else if (angle[axis] <= -PITCHROLLOVER180) { |
| angle[axis] += PITCHROLLOVER360; |
| attitude[axis] += ACRate[axis]; |
| if (attitude[axis] > PITCHROLLOVER180) { |
| attitude[axis] -= PITCHROLLOVER360; |
| } else if (attitude[axis] <= -PITCHROLLOVER180) { |
| attitude[axis] += PITCHROLLOVER360; |
| } |
| } |
| } |
| 299,35 → 233,35 |
| uint8_t ca = controlActivity >> 8; |
| uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
| if (highControlActivity) { |
| debugOut.digital[1] |= DEBUG_ACC0THORDER; |
| } else { |
| debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
| } |
| if (highControlActivity) { |
| debugOut.digital[1] |= DEBUG_ACC0THORDER; |
| } else { |
| debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
| } |
| if (accVector <= dynamicParams.maxAccVector) { |
| debugOut.digital[0] &= ~ DEBUG_ACC0THORDER; |
| debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
| uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
| int32_t accDerived; |
| /* |
| if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| } |
| if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| } |
| if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| */ |
| if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| */ |
| if (highControlActivity) { // reduce effect during stick control activity |
| permilleAcc /= 4; |
| if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity |
| if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity |
| permilleAcc /= 4; |
| } |
| } |
| } |
| /* |
| * Add to each sum: The amount by which the angle is changed just below. |
| 336,10 → 270,10 |
| accDerived = getAngleEstimateFromAcc(axis); |
| debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
| // 1000 * the correction amount that will be added to the gyro angle in next line. |
| temp = angle[axis]; |
| angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
| temp = attitude[axis]; |
| attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp |
| + (int32_t) permilleAcc * accDerived) / 1000L; |
| correctionSum[axis] += angle[axis] - temp; |
| correctionSum[axis] += attitude[axis] - temp; |
| } |
| } else { |
| debugOut.analog[9] = 0; |
| 372,7 → 306,7 |
| timer = DRIFTCORRECTION_TIME; |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| // Take the sum of corrections applied, add it to delta |
| if (correctionSum[axis] >=0) |
| if (correctionSum[axis] >= 0) |
| round = DRIFTCORRECTION_TIME / 2; |
| else |
| round = -DRIFTCORRECTION_TIME / 2; |
| 383,7 → 317,6 |
| // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
| // debugOut.analog[28 + axis] = driftComp[axis]; |
| correctionSum[axis] = 0; |
| } |
| } |
| 390,16 → 323,84 |
| } |
| void calculateAccVector(void) { |
| int16_t temp; |
| temp = filteredAcc[0] >> 3; |
| accVector = temp * temp; |
| temp = filteredAcc[1] >> 3; |
| accVector += temp * temp; |
| temp = filteredAcc[2] >> 3; |
| accVector += temp * temp; |
| //debugOut.analog[18] = accVector; |
| int16_t temp; |
| temp = filteredAcc[0] >> 3; |
| accVector = temp * temp; |
| temp = filteredAcc[1] >> 3; |
| accVector += temp * temp; |
| temp = filteredAcc[2] >> 3; |
| accVector += temp * temp; |
| //debugOut.analog[18] = accVector; |
| } |
| void attitude_resetHeadingToMagnetic(void) { |
| if (commands_isCalibratingCompass()) |
| return; |
| // Compass is off, skip. |
| if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
| return; |
| // Compass is invalid, skip. |
| if (magneticHeading < 0) |
| return; |
| heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
| targetHeading = heading; |
| debugOut.digital[0] ^= DEBUG_COMPASS; |
| } |
| void correctHeadingToMagnetic(void) { |
| int32_t error; |
| debugOut.analog[27] = heading; |
| if (commands_isCalibratingCompass()) |
| return; |
| // Compass is off, skip. |
| // Naaah this is assumed. |
| // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
| // return; |
| // Compass is invalid, skip. |
| if (magneticHeading < 0) |
| return; |
| // Spinning fast, skip |
| if (abs(yawRate) > 128) |
| return; |
| // Otherwise invalidated, skip |
| if (ignoreCompassTimer) { |
| ignoreCompassTimer--; |
| return; |
| } |
| // TODO: Find computational cost of this. |
| error = (magneticHeading*GYRO_DEG_FACTOR_YAW - heading) % GYRO_DEG_FACTOR_YAW; |
| // We only correct errors larger than the resolution of the compass, or else we would keep rounding the |
| // better resolution of the gyros to the worse resolution of the compass all the time. |
| // The correction should really only serve to compensate for gyro drift. |
| if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
| int32_t correction = (error * (int32_t)dynamicParams.compassYawEffect) >> 8; |
| // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
| // and to the target heading (the direction to which it maneuvers to point). That means, this correction has |
| // no effect on control at all!!! It only has effect on the values of the two variables. However, these values |
| // could have effect on control elsewhere, like in compassControl.c . |
| heading += correction; |
| intervalWrap(&heading, YAWOVER360); |
| targetHeading += correction; |
| intervalWrap(&targetHeading, YAWOVER360); |
| debugOut.digital[1] ^= DEBUG_COMPASS; |
| } |
| /************************************************************************ |
| * Main procedure. |
| ************************************************************************/ |
| 416,80 → 417,9 |
| // We are done reading variables from the analog module. |
| // Interrupt-driven sensor reading may restart. |
| startAnalogConversionCycle(); |
| } |
| void updateCompass(void) { |
| int16_t w, v, r, correction, error; |
| if (compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) { |
| if (controlMixer_testCompassCalState()) { |
| compassCalState++; |
| if (compassCalState < 5) |
| beepNumber(compassCalState); |
| else |
| beep(1000); |
| } |
| } else { |
| // get maximum attitude angle |
| w = abs(angle[PITCH] / 512); |
| v = abs(angle[ROLL] / 512); |
| if (v > w) |
| w = v; |
| correction = w / 8 + 1; |
| // calculate the deviation of the yaw gyro heading and the compass heading |
| if (magneticHeading < 0) |
| error = 0; // disable yaw drift compensation if compass heading is undefined |
| else if (abs(yawRate) > 128) { // spinning fast |
| error = 0; |
| } else { |
| // compassHeading - yawGyroHeading, on a -180..179 deg interval. |
| error = ((540 + magneticHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) |
| % 360) - 180; |
| } |
| if (!ignoreCompassTimer && w < 25) { |
| yawGyroDrift += error; |
| // Basically this gets set if we are in "fix" mode, and when starting. |
| if (updateCompassCourse) { |
| beep(200); |
| yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
| compassCourse = magneticHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
| updateCompassCourse = 0; |
| } |
| } |
| yawGyroHeading += (error * 8) / correction; |
| /* |
| w = (w * dynamicParams.CompassYawEffect) / 32; |
| w = dynamicParams.CompassYawEffect - w; |
| */ |
| w = dynamicParams.compassYawEffect - (w * dynamicParams.compassYawEffect) |
| / 32; |
| // As readable formula: |
| // w = dynamicParams.CompassYawEffect * (1-w/32); |
| if (w >= 0) { // maxAttitudeAngle < 32 |
| if (!ignoreCompassTimer) { |
| /*v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;*/ |
| v = 64 + controlActivity / 100; |
| // yawGyroHeading - compassCourse on a -180..179 degree interval. |
| r |
| = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
| % 360) - 180; |
| v = (r * w) / v; // align to compass course |
| // limit yaw rate |
| w = 3 * dynamicParams.compassYawEffect; |
| if (v > w) |
| v = w; |
| else if (v < -w) |
| v = -w; |
| yawAngleDiff += v; |
| } else { // wait a while |
| ignoreCompassTimer--; |
| } |
| } else { // ignore compass at extreme attitudes for a while |
| ignoreCompassTimer = 500; |
| } |
| if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
| correctHeadingToMagnetic(); |
| } |
| } |
| /branches/dongfang_FC_rewrite/attitude.h |
|---|
| 79,6 → 79,11 |
| */ |
| #define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR)) |
| #define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L) |
| #define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
| #define YAWOVER180 (GYRO_DEG_FACTOR_YAW * 180L) |
| #define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L) |
| /* |
| * Rotation rates |
| */ |
| 88,25 → 93,24 |
| /* |
| * Attitudes calculated by numerical integration of gyro rates |
| */ |
| extern int32_t angle[2], yawAngleDiff; |
| extern int32_t attitude[2]; |
| // extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
| // This is really a flight module thing, but it should be corrected along |
| // when the yaw angle is corrected from the compass, and that happens here. |
| // extern int32_t yawAngleDiff; |
| /* |
| * Compass navigation |
| */ |
| extern int16_t magneticHeading; |
| extern int16_t compassCourse; |
| // extern int16_t compassOffCourse; |
| extern uint8_t compassCalState; |
| extern int32_t yawGyroHeading; |
| extern int16_t yawGyroHeadingInDeg; |
| extern uint8_t updateCompassCourse; |
| //extern int16_t headingInDegrees; |
| extern int32_t heading; |
| extern uint16_t ignoreCompassTimer; |
| extern uint16_t accVector; |
| void updateCompass(void); |
| extern int32_t targetHeading; |
| /* |
| * Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements, |
| * to help canceling out drift and vibration noise effects. The dynamic offsets themselves |
| 143,4 → 147,6 |
| */ |
| void calculateFlightAttitude(void); |
| void attitude_resetHeadingToMagnetic(void); |
| #endif //_ATTITUDE_H |
| /branches/dongfang_FC_rewrite/attitudeControl.c |
|---|
| 3,6 → 3,7 |
| #include "uart0.h" |
| #include "configuration.h" |
| #include "dongfangMath.h" |
| #include "controlMixer.h" |
| // For scope debugging only! |
| #include "output.h" |
| 13,13 → 14,14 |
| // Takes 380 - 400 usec. Way too slow. |
| // With static MINPROJECTION: 220 usec. |
| uint16_t AC_getThrottle(uint16_t throttle) { |
| void AC_getPRTY(int16_t* PRTY) { |
| int16_t throttle = PRTY[CONTROL_THROTTLE]; |
| int32_t projection; |
| uint8_t effect = dynamicParams.attitudeControl; // Userparam 3 |
| int16_t deltaThrottle, y; |
| int16_t rollAngleInDegrees = angle[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
| int16_t pitchAngleInDegrees = angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
| int16_t rollAngleInDegrees = attitude[ROLL]/GYRO_DEG_FACTOR_PITCHROLL; |
| int16_t pitchAngleInDegrees = attitude[PITCH]/GYRO_DEG_FACTOR_PITCHROLL; |
| projection = (int32_t) cos_360(pitchAngleInDegrees) * (int32_t) cos_360(rollAngleInDegrees); |
| projection >>= 8; |
| 38,5 → 40,5 |
| } |
| deltaThrottle = ((int32_t)y * effect) >> 6; |
| // debugOut.analog[8] = deltaThrottle; |
| return throttle + deltaThrottle; |
| PRTY[CONTROL_THROTTLE] = throttle; |
| } |
| /branches/dongfang_FC_rewrite/attitudeControl.h |
|---|
| 1,5 → 1,5 |
| #ifndef _ATTITUDECONTROL_H |
| #define _ATTITUDECONTROL_H |
| #include <inttypes.h> |
| uint16_t AC_getThrottle(uint16_t throttle); |
| void AC_getPRTY(int16_t* PRTY); |
| #endif |
| /branches/dongfang_FC_rewrite/commands.c |
|---|
| 57,6 → 57,8 |
| #include "attitude.h" |
| #include "output.h" |
| uint8_t compassCalState = 0; |
| void commands_handleCommands(void) { |
| /* |
| * Get the current command (start/stop motors, calibrate), if any. |
| 86,10 → 88,8 |
| flight_setNeutral(); |
| controlMixer_setNeutral(); |
| beepNumber(getActiveParamSet()); |
| } else if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE |
| | CFG_GPS_ACTIVE) && argument == 7) { |
| } else if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && argument == 7) { |
| // If right stick is centered and down |
| // TODO: Out of here! State machine instead. |
| compassCalState = 1; |
| beep(1000); |
| } |
| 123,3 → 123,18 |
| MKFlags &= ~(MKFLAG_MOTOR_RUN); |
| } |
| } |
| /* |
| * if (controlMixer_testCompassCalState()) { |
| compassCalState++; |
| if (compassCalState < 5) |
| beepNumber(compassCalState); |
| else |
| beep(1000); |
| } |
| * |
| */ |
| uint8_t commands_isCalibratingCompass(void) { |
| return RC_testCompassCalState(); |
| } |
| /branches/dongfang_FC_rewrite/commands.h |
|---|
| 2,8 → 2,6 |
| #define _COMMANDS_H |
| #include <inttypes.h> |
| void commands_handleCommands(void); |
| /* |
| * An enumeration over the start motors, stop motors, calibrate gyros |
| * and calibreate acc. meters commands. |
| 14,4 → 12,9 |
| #define COMMAND_GYROCAL 2 |
| #define COMMAND_ACCCAL 4 |
| extern uint8_t compassCalState; |
| void commands_handleCommands(void); |
| uint8_t commands_isCalibratingCompass(void); |
| #endif |
| /branches/dongfang_FC_rewrite/compassControl.c |
|---|
| 0,0 → 1,19 |
| #include <inttypes.h> |
| #include "controlMixer.h" |
| #include "attitude.h" |
| #include <stdlib.h> |
| // The only possibility in a compass control is to aim the head at some compass heading. |
| // One way could be: When a switch is turned on, we capture the current heading in a variable. |
| // From then on, if the current heading is to the right of the captured heading, we yaw left etc. |
| // If the yaw at input is nonzero, we could also temporarily disable this and instead re-capture |
| // the current heading.. |
| int32_t magneticTargetHeading; |
| void CC_periodicTaskAndPRTY(int16_t* PRTY) { |
| int16_t currentYaw = PRTY[CONTROL_YAW]; |
| if (abs(currentYaw) < 10) { |
| magneticTargetHeading = magneticHeading; |
| } |
| } |
| /branches/dongfang_FC_rewrite/compassControl.h |
|---|
| 0,0 → 1,10 |
| #ifndef _COMPASS_H |
| #define _COMPASSRC_H |
| #include <inttypes.h> |
| extern int32_t magneticTargetHeading; |
| void CC_periodicTaskAndPRTY(int16_t* PRTY); |
| #endif |
| /branches/dongfang_FC_rewrite/configuration.c |
|---|
| 55,7 → 55,8 |
| #include "configuration.h" |
| #include "sensors.h" |
| #include "rc.h" |
| #include "uart0.h" |
| //#include "uart0.h" |
| #include "output.h" |
| int16_t variables[VARIABLE_COUNT]; |
| ParamSet_t staticParams; |
| 81,8 → 82,6 |
| void configuration_applyVariablesToParams(void) { |
| uint8_t i; |
| debugOut.analog[20] = variables[0]; |
| #define SET_POT_MM(b,a,min,max) {if (a>=255-VARIABLE_COUNT) b=variables[a+VARIABLE_COUNT-255]; else b=a; if(b<=min) b=min; else if(b>=max) b=max;} |
| #define SET_POT(b,a) {if (a>=255-VARIABLE_COUNT) b=variables[a+VARIABLE_COUNT-255]; else b=a;} |
| SET_POT_MM(dynamicParams.gyroP, staticParams.gyroP, 5, 200); |
| 280,7 → 279,9 |
| staticParams.outputFlash[1].timing = 15; |
| staticParams.outputDebugMask = 8; |
| staticParams.outputFlags = 16|8|4; |
| staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | OUTPUTFLAGS_USE_ONBOARD_LEDS; |
| staticParams.naviMode = 200; // free. |
| } |
| /***************************************************/ |
| /branches/dongfang_FC_rewrite/controlMixer.c |
|---|
| 1,54 → 1,3 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + Nur für den privaten Gebrauch |
| // + www.MikroKopter.com |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
| // + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zulässig ist. |
| // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
| // + bzgl. der Nutzungsbedingungen aufzunehmen. |
| // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
| // + Verkauf von Luftbildaufnahmen, usw. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
| // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
| // + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt und genannt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <stdlib.h> |
| #include "controlMixer.h" |
| #include "rc.h" |
| 55,6 → 4,7 |
| #include "heightControl.h" |
| #include "attitudeControl.h" |
| #include "externalControl.h" |
| #include "compassControl.h" |
| #include "naviControl.h" |
| #include "configuration.h" |
| #include "attitude.h" |
| 133,9 → 83,6 |
| uint8_t rcQ = RC_getSignalQuality(); |
| uint8_t ecQ = EC_getSignalQuality(); |
| if (rcQ < SIGNAL_GOOD) debugOut.digital[0] |= DEBUG_SIGNAL; else debugOut.digital[0] &= ~DEBUG_SIGNAL; |
| if (ecQ < SIGNAL_GOOD) debugOut.digital[1] |= DEBUG_SIGNAL; else debugOut.digital[1] &= ~DEBUG_SIGNAL; |
| // This needs not be the only correct solution... |
| return rcQ > ecQ ? rcQ : ecQ; |
| } |
| 166,42 → 113,45 |
| } |
| /* |
| * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
| * Update the variables indicating stick position from the sum of R/C, GPS and external control |
| * and whatever other controls we invented in the meantime... |
| * Update variables. |
| * Decode commands but do not execute them. |
| */ |
| int16_t tempPRTY[4]; |
| void controlMixer_periodicTask(void) { |
| // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
| // TODO: If no signal --> zero. |
| int16_t tempThrottle; |
| // HC_periodicTask(); |
| // takes almost no time... |
| RC_periodicTask(); |
| // takes almost no time... |
| EC_periodicTask(); |
| // takes about 80 usec. |
| HC_periodicTask(); |
| int16_t* RC_PRTY = RC_getPRTY(); |
| int16_t* EC_PRTY = EC_getPRTY(); |
| debugOut.analog[29] = RC_PRTY[CONTROL_PITCH]; |
| debugOut.analog[30] = RC_PRTY[CONTROL_ROLL]; |
| // This will init the values (not just add to them). |
| RC_periodicTaskAndPRTY(tempPRTY); |
| int16_t naviSticks[] = {RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH], RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]}; |
| // Add external control to RC |
| EC_periodicTaskAndPRTY(tempPRTY); |
| navigation_periodicTask(&naviSticks); |
| // Add navigations control to RC and external control. |
| navigation_periodicTaskAndPRTY(tempPRTY); |
| updateControlAndMeasureControlActivity(CONTROL_PITCH, naviSticks[CONTROL_PITCH]); |
| updateControlAndMeasureControlActivity(CONTROL_ROLL, naviSticks[CONTROL_ROLL]); |
| updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]); |
| // Add compass control (could also have been before navi, they are independent) |
| CC_periodicTaskAndPRTY(tempPRTY); |
| // Add height control (could also have been before navi and/or compass, they are independent) |
| HC_periodicTaskAndPRTY(tempPRTY); |
| // Add attitude control (could also have been before navi and/or compass, they are independent) |
| AC_getPRTY(tempPRTY); |
| // Commit results to global variable and also measure control activity. |
| updateControlAndMeasureControlActivity(CONTROL_PITCH, tempPRTY[CONTROL_PITCH]); |
| updateControlAndMeasureControlActivity(CONTROL_ROLL, tempPRTY[CONTROL_ROLL]); |
| updateControlAndMeasureControlActivity(CONTROL_YAW, tempPRTY[CONTROL_YAW]); |
| dampenControlActivity(); |
| debugOut.analog[17] = controlActivity/10; |
| tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]); |
| controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle); |
| // Update variables ("potis"). |
| if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
| controlMixer_updateVariables(); |
| controlMixer_didReceiveSignal = 1; |
| 224,7 → 174,7 |
| */ |
| /* |
| * Record maxima |
| * Record maxima. Predecessor of the control activity stuff. |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) { |
| maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
| 235,6 → 185,7 |
| } |
| */ |
| // Decode commands. |
| uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
| : COMMAND_NONE; |
| uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() |
| 254,12 → 205,4 |
| isCommandRepeated = 0; |
| lastCommand = COMMAND_NONE; |
| } |
| //if (rcCommand != COMMAND_NONE) debugOut.digital[0] |= DEBUG_SIGNAL; else debugOut.digital[0] &= ~DEBUG_SIGNAL; |
| //if (isCommandRepeated) debugOut.digital[1] |= DEBUG_SIGNAL; else debugOut.digital[1] &= ~DEBUG_SIGNAL; |
| } |
| // TODO: Integrate into command system. |
| uint8_t controlMixer_testCompassCalState(void) { |
| return RC_testCompassCalState(); |
| } |
| /branches/dongfang_FC_rewrite/controlMixer.h |
|---|
| 122,6 → 122,3 |
| */ |
| uint8_t controlMixer_getArgument(void); |
| uint8_t controlMixer_isCommandRepeated(void); |
| // TODO: Abstract away if possible. |
| uint8_t controlMixer_testCompassCalState(void); |
| /branches/dongfang_FC_rewrite/directGPSNaviControl.c |
|---|
| 15,6 → 15,7 |
| #include "isqrt.h" |
| #include "attitude.h" |
| #include "dongfangMath.h" |
| #include "attitude.h" |
| typedef enum { |
| GPS_FLIGHT_MODE_UNDEF, |
| 50,11 → 51,11 |
| flightMode = GPS_FLIGHT_MODE_HOME; |
| } else { |
| if (dynamicParams.naviMode < 50) |
| flightMode = GPS_FLIGHT_MODE_FREE; |
| flightMode = GPS_FLIGHT_MODE_HOME; |
| else if (dynamicParams.naviMode < 180) |
| flightMode = GPS_FLIGHT_MODE_AID; |
| else |
| flightMode = GPS_FLIGHT_MODE_HOME; |
| flightMode = GPS_FLIGHT_MODE_FREE; |
| } |
| if (flightMode != flightModeOld) { |
| 93,9 → 94,9 |
| } |
| // checks nick and roll sticks for manual control |
| uint8_t navi_isManuallyControlled(int16_t* sticks) { |
| if (sticks[CONTROL_PITCH] < staticParams.naviStickThreshold |
| && sticks[CONTROL_ROLL] < staticParams.naviStickThreshold) |
| uint8_t navi_isManuallyControlled(int16_t* PRTY) { |
| if (PRTY[CONTROL_PITCH] < staticParams.naviStickThreshold |
| && PRTY[CONTROL_ROLL] < staticParams.naviStickThreshold) |
| return 0; |
| else |
| return 1; |
| 134,7 → 135,7 |
| // calculates the GPS control stick values from the deviation to target position |
| // if the pointer to the target positin is NULL or is the target position invalid |
| // then the P part of the controller is deactivated. |
| void navi_PIDController(GPS_Pos_t *pTargetPos, int16_t* sticks) { |
| void navi_PIDController(GPS_Pos_t *pTargetPos, int16_t *PRTY) { |
| static int32_t PID_Nick, PID_Roll; |
| int32_t coscompass, sincompass; |
| int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
| 230,8 → 231,8 |
| // in the flight.c. Therefore a positive north deviation/velocity should result in a positive |
| // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll. |
| coscompass = cos_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
| sincompass = sin_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
| coscompass = -cos_360(heading / GYRO_DEG_FACTOR_YAW); |
| sincompass = -sin_360(heading / GYRO_DEG_FACTOR_YAW); |
| PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
| PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
| 239,12 → 240,9 |
| // limit resulting GPS control vector |
| navi_limitXY(&PID_Nick, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN); |
| debugOut.analog[27] = -PID_Nick; |
| debugOut.analog[28] = -PID_Roll; |
| PRTY[CONTROL_PITCH] += PID_Nick; |
| PRTY[CONTROL_ROLL] += PID_Roll; |
| sticks[CONTROL_PITCH] -= PID_Nick; |
| sticks[CONTROL_ROLL] -= PID_Roll; |
| } else { // invalid GPS data or bad compass reading |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| 252,7 → 250,7 |
| } |
| } |
| void navigation_periodicTask(int16_t* sticks) { |
| void navigation_periodicTaskAndPRTY(int16_t* PRTY) { |
| static uint8_t GPS_P_Delay = 0; |
| static uint16_t beep_rythm = 0; |
| 296,7 → 294,7 |
| case GPS_FLIGHT_MODE_AID: |
| if (holdPosition.status != INVALID) { |
| if (navi_isManuallyControlled(sticks)) { // MK controlled by user |
| if (navi_isManuallyControlled(PRTY)) { // MK controlled by user |
| // update hold point to current gps position |
| navi_writeCurrPositionTo(&holdPosition); |
| // disable gps control |
| 306,9 → 304,9 |
| // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
| GPS_P_Delay++; |
| navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position |
| navi_PIDController(NULL, sticks); // activates only the D-Part |
| navi_PIDController(NULL, PRTY); // activates only the D-Part |
| } else |
| navi_PIDController(&holdPosition, sticks); // activates the P&D-Part |
| navi_PIDController(&holdPosition, PRTY); // activates the P&D-Part |
| } |
| } else // invalid Hold Position |
| { // try to catch a valid hold position from gps data input |
| 321,10 → 319,10 |
| // update hold point to current gps position |
| // to avoid a flight back if home comming is deactivated |
| navi_writeCurrPositionTo(&holdPosition); |
| if (navi_isManuallyControlled(sticks)) // MK controlled by user |
| if (navi_isManuallyControlled(PRTY)) // MK controlled by user |
| { |
| } else {// GPS control active |
| navi_PIDController(&homePosition, sticks); |
| navi_PIDController(&homePosition, PRTY); |
| } |
| } else { |
| // bad home position |
| 332,11 → 330,11 |
| // try to hold at least the position as a fallback option |
| if (holdPosition.status != INVALID) { |
| if (navi_isManuallyControlled(sticks)) { |
| if (navi_isManuallyControlled(PRTY)) { |
| // MK controlled by user |
| } else { |
| // GPS control active |
| navi_PIDController(&holdPosition, sticks); |
| navi_PIDController(&holdPosition, PRTY); |
| } |
| } else { // try to catch a valid hold position |
| navi_writeCurrPositionTo(&holdPosition); |
| 365,5 → 363,3 |
| debugOut.analog[11] = GPSInfo.satnum; |
| } |
| /branches/dongfang_FC_rewrite/dongfangMath.c |
|---|
| 232,3 → 232,11 |
| result = pgm_read_word(&TAN_TABLE[(uint8_t) arg]); |
| return (sign == 1) ? result : -result; |
| } |
| void intervalWrap(int32_t *number, int32_t limit) { |
| if (*number >= limit) { |
| *number -= limit; // 360 deg. wrap |
| } else if (*number < 0) { |
| *number += limit; |
| } |
| } |
| /branches/dongfang_FC_rewrite/dongfangMath.h |
|---|
| 20,3 → 20,5 |
| int16_t sin_360(int16_t arg); |
| int16_t cos_360(int16_t arg); |
| int16_t tan_360(int16_t arg); |
| void intervalWrap(int32_t *number, int32_t limit); |
| /branches/dongfang_FC_rewrite/externalControl.c |
|---|
| 4,64 → 4,57 |
| ExternalControl_t externalControl; |
| uint8_t externalControlActive; |
| // int16_t EC_PRTY[4]; |
| // TODO: Who is going to call this |
| int16_t EC_PRTY[4]; |
| // TODO: Who is going to call this |
| void EC_setNeutral(void) { |
| // if necessary. From main.c. |
| externalControl.config = 0; |
| externalControl.pitch = 0; |
| externalControl.roll = 0; |
| externalControl.yaw = 0; |
| externalControl.throttle = 0; |
| // if necessary. From main.c. |
| externalControl.config = 0; |
| externalControl.pitch = 0; |
| externalControl.roll = 0; |
| externalControl.yaw = 0; |
| externalControl.throttle = 0; |
| // From main.c. What does it do?? |
| externalControl.digital[0] = 0x55; |
| // From main.c. What does it do?? |
| externalControl.digital[0] = 0x55; |
| } |
| int16_t* EC_getPRTY(void) { |
| return EC_PRTY; |
| void EC_periodicTaskAndPRTY(int16_t* PRTY) { |
| if (externalControlActive) { |
| externalControlActive--; |
| PRTY[CONTROL_PITCH] += externalControl.pitch * (int16_t) staticParams.stickP; |
| PRTY[CONTROL_ROLL] += externalControl.roll * (int16_t) staticParams.stickP; |
| PRTY[CONTROL_THROTTLE] += externalControl.throttle; |
| PRTY[CONTROL_YAW] += externalControl.yaw; // No stickP or similar?????? |
| } |
| } |
| uint8_t EC_getArgument(void) { |
| return externalControl.config; |
| return externalControl.config; |
| } |
| uint8_t EC_getCommand(void) { |
| return externalControl.free; |
| return externalControl.free; |
| } |
| // not implemented. |
| int16_t EC_getVariable(uint8_t varNum) { |
| return 0; |
| return 0; |
| } |
| void EC_periodicTask() { |
| if (externalControlActive) { |
| externalControlActive--; |
| EC_PRTY[CONTROL_PITCH] = externalControl.pitch * (int16_t) staticParams.stickP; |
| EC_PRTY[CONTROL_ROLL] = externalControl.roll * (int16_t) staticParams.stickP; |
| EC_PRTY[CONTROL_THROTTLE] = externalControl.throttle; |
| EC_PRTY[CONTROL_YAW] = externalControl.yaw; // No stickP or similar?????? |
| } else { |
| EC_PRTY[CONTROL_PITCH] = EC_PRTY[CONTROL_ROLL] = EC_PRTY[CONTROL_THROTTLE] = EC_PRTY[CONTROL_YAW] = 0; |
| } |
| } |
| uint8_t EC_getSignalQuality(void) { |
| if (externalControlActive > 80) |
| // Configured and heard from recently |
| return SIGNAL_GOOD; |
| if (externalControlActive > 80) |
| // Configured and heard from recently |
| return SIGNAL_GOOD; |
| if (externalControlActive) |
| // Configured and heard from |
| return SIGNAL_OK; |
| if (externalControlActive) |
| // Configured and heard from |
| return SIGNAL_OK; |
| if (!(externalControl.config & 0x01 && dynamicParams.externalControl > 128)) |
| // External control is not even configured. |
| return NO_SIGNAL; |
| if (!(externalControl.config & 0x01 && dynamicParams.externalControl > 128)) |
| // External control is not even configured. |
| return NO_SIGNAL; |
| // Configured but expired. |
| return SIGNAL_LOST; |
| // Configured but expired. |
| return SIGNAL_LOST; |
| } |
| /branches/dongfang_FC_rewrite/externalControl.h |
|---|
| 20,8 → 20,7 |
| extern ExternalControl_t externalControl; |
| extern uint8_t externalControlActive; |
| void EC_periodicTask(void); |
| int16_t* EC_getPRTY(void); |
| void EC_periodicTaskAndPRTY(int16_t* PRTY); |
| uint8_t EC_getArgument(void); |
| uint8_t EC_getCommand(void); |
| int16_t EC_getVariable(uint8_t varNum); |
| /branches/dongfang_FC_rewrite/flight.c |
|---|
| 212,8 → 212,9 |
| PDPartYaw = 0; |
| if (isFlying == 250) { |
| // HC_setGround(); |
| updateCompassCourse = 1; |
| yawAngleDiff = 0; |
| attitude_resetHeadingToMagnetic(); |
| // Set target heading to the one just gotten off compass. |
| targetHeading = heading; |
| } |
| } else { |
| // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
| 228,29 → 229,20 |
| /************************************************************************/ |
| /* Yawing */ |
| /************************************************************************/ |
| /* |
| if (abs(controls[CONTROL_YAW]) > 4 * staticParams.stickYawP) { // yaw stick is activated |
| ignoreCompassTimer = 1000; |
| if (!(staticParams.bitConfig & CFG_COMPASS_FIX)) { |
| updateCompassCourse = 1; |
| //targetHeading = heading; |
| // YGBSM!!! |
| } |
| } |
| */ |
| // yawControlRate = controlYaw; |
| // Trim drift of yawAngleDiff with controlYaw. |
| // TODO: We want NO feedback of control related stuff to the attitude related stuff. |
| // This seems to be used as: Difference desired <--> real heading. |
| yawAngleDiff -= controls[CONTROL_YAW]; |
| targetHeading -= controls[CONTROL_YAW]; |
| // limit the effect |
| CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
| debugOut.analog[28] = targetHeading; |
| /************************************************************************/ |
| /* Compass is currently not supported. */ |
| /************************************************************************/ |
| if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
| updateCompass(); |
| } |
| #if defined (USE_NAVICTRL) |
| /************************************************************************/ |
| /* GPS is currently not supported. */ |
| 271,13 → 263,21 |
| /************************************************************************/ |
| // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| PDPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
| PDPart[axis] = attitude[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
| PDPart[axis] += ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)); |
| PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
| CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
| } |
| PDPartYaw = (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
| int32_t headingDiff = heading - targetHeading; // apparently yaw is reverse on output. |
| if (headingDiff > YAWOVER180) headingDiff -= YAWOVER360; |
| else if (headingDiff <= -YAWOVER180) headingDiff += YAWOVER360; |
| // TODO: Not quite right: We want to limit targetHeading to be max. 50000 from heading. This is the wrong var. fixed. |
| CHECK_MIN_MAX(headingDiff, -50000, 50000); |
| debugOut.analog[29] = headingDiff; |
| PDPartYaw = (int32_t) (headingDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
| PDPartYaw += (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING); |
| // limit control feedback |
| 432,8 → 432,8 |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if (!(--debugDataTimer)) { |
| debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
| debugOut.analog[0] = angle[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg |
| debugOut.analog[1] = angle[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg |
| debugOut.analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
| debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg |
| debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg |
| debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW; |
| } |
| } |
| /branches/dongfang_FC_rewrite/flight.h |
|---|
| 12,16 → 12,6 |
| #define PITCH 0 |
| #define ROLL 1 |
| /* |
| * The output BLC throttles can be set in an [0..256[ interval. Some staticParams limits |
| * (min, throttle, max throttle etc) are in a [0..256[ interval. |
| * The calculation of motor throttle values from sensor and control information (basically |
| * flight.c) take place in an [0..1024[ interval for better precision. |
| * This is the conversion factor. |
| * --> Replaced back again by CONTROL_SCALING. Even though the 2 things are not quite the |
| * same, they are unseperable anyway. |
| */ |
| void flight_control(void); |
| void transmitMotorThrottleData(void); |
| void flight_setNeutral(void); |
| /branches/dongfang_FC_rewrite/heightControl.c |
|---|
| 3,6 → 3,7 |
| #include "attitude.h" |
| #include "uart0.h" |
| #include "configuration.h" |
| #include "controlMixer.h" |
| // for digital / LED debug. |
| #include "output.h" |
| 84,11 → 85,11 |
| // takes 180-200 usec (with integral term). That is too heavy!!! |
| // takes 100 usec without integral term. |
| uint16_t HC_getThrottle(uint16_t throttle) { |
| void HC_periodicTaskAndPRTY(int16_t* PRTY) { |
| HC_periodicTask(); |
| int16_t throttle = PRTY[CONTROL_THROTTLE]; |
| int32_t height = analog_getHeight(); |
| int32_t heightError = rampedTargetHeight - height; |
| //static int32_t lastHeight; |
| int16_t dHeight = analog_getDHeight(); |
| //lastHeight = height; |
| 175,7 → 176,7 |
| */ |
| return throttle; |
| PRTY[CONTROL_THROTTLE] = throttle; |
| } |
| /* |
| /branches/dongfang_FC_rewrite/heightControl.h |
|---|
| 1,3 → 1,2 |
| void HC_periodicTask(void); |
| uint16_t HC_getThrottle(uint16_t throttle); |
| void HC_periodicTaskAndPRTY(int16_t* PRTY); |
| void HC_setGround(void); |
| /branches/dongfang_FC_rewrite/makefile |
|---|
| 19,18 → 19,18 |
| EXT = DIRECT_GPS |
| #EXT = |
| GYRO=ENC-03_FC1.3 |
| GYRO_HW_NAME=ENC |
| GYRO_HW_FACTOR=1.304f |
| #GYRO=ENC-03_FC1.3 |
| #GYRO_HW_NAME=ENC |
| #GYRO_HW_FACTOR=1.304f |
| #GYRO_PITCHROLL_CORRECTION=1.0f |
| #GYRO_YAW_CORRECTION=1.0f |
| GYRO=ADXRS610_FC2.0 |
| GYRO_HW_NAME=ADXR |
| GYRO_HW_FACTOR=1.2288f |
| GYRO_PITCHROLL_CORRECTION=1.0f |
| GYRO_YAW_CORRECTION=1.0f |
| #GYRO=ADXRS610_FC2.0 |
| #GYRO_HW_NAME=ADXR |
| #GYRO_HW_FACTOR=1.2288f |
| #GYRO_PITCHROLL_CORRECTION=1.0f |
| #GYRO_YAW_CORRECTION=1.0f |
| #GYRO=invenSense |
| #GYRO_HW_NAME=Isense |
| #GYRO_HW_FACTOR=0.6827f |
| 129,7 → 129,7 |
| ########################################################################################################## |
| # List C source files here. (C dependencies are automatically generated.) |
| SRC = main.c uart0.c printf_P.c timer0.c timer2.c analog.c menu.c output.c controlMixer.c |
| SRC += externalControl.c dongfangMath.c twimaster.c rc.c attitude.c flight.c |
| SRC += externalControl.c compassControl.c dongfangMath.c twimaster.c rc.c attitude.c flight.c |
| SRC += eeprom.c heightControl.c configuration.c attitudeControl.c commands.c $(GYRO).c |
| ifeq ($(EXT), DIRECT_GPS) |
| /branches/dongfang_FC_rewrite/menu.c |
|---|
| 85,9 → 85,9 |
| /* Clear LCD Buffer */ |
| /************************************/ |
| void LCD_clear(void) { |
| uint8_t i; |
| for (i = 0; i < DISPLAYBUFFSIZE; i++) |
| displayBuff[i] = ' '; |
| uint8_t i; |
| for (i = 0; i < DISPLAYBUFFSIZE; i++) |
| displayBuff[i] = ' '; |
| } |
| /************************************/ |
| 95,203 → 95,229 |
| /************************************/ |
| // Display with 20 characters in 4 lines |
| void LCD_printMenu(void) { |
| if (remoteKeys & KEY1) { |
| if (menuItem) |
| menuItem--; |
| else |
| menuItem = maxMenuItem; |
| } |
| if (remoteKeys & KEY1) { |
| if (menuItem) |
| menuItem--; |
| else |
| menuItem = maxMenuItem; |
| } |
| if (remoteKeys & KEY2) { |
| if (menuItem == maxMenuItem) |
| menuItem = 0; |
| else |
| menuItem++; |
| } |
| if ((remoteKeys & KEY1) && (remoteKeys & KEY2)) |
| menuItem = 0; |
| if (remoteKeys & KEY2) { |
| if (menuItem == maxMenuItem) |
| menuItem = 0; |
| else |
| menuItem++; |
| } |
| if ((remoteKeys & KEY1) && (remoteKeys & KEY2)) |
| menuItem = 0; |
| LCD_clear(); |
| LCD_clear(); |
| if (menuItem > maxMenuItem) |
| menuItem = maxMenuItem; |
| // print menu item number in the upper right corner |
| if (menuItem < 10) { |
| LCD_printfxy(17,0,"[%i]", menuItem); |
| } else { |
| LCD_printfxy(16,0,"[%i]", menuItem); |
| } |
| if (menuItem > maxMenuItem) |
| menuItem = maxMenuItem; |
| // print menu item number in the upper right corner |
| if (menuItem < 10) { |
| LCD_printfxy(17, 0, "[%i]", menuItem); |
| } else { |
| LCD_printfxy(16, 0, "[%i]", menuItem); |
| } |
| switch (menuItem) { |
| case 0:// Version Info Menu Item |
| LCD_printfxy(0,0,"+ MikroKopter +") |
| ; |
| LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d%c",boardRelease/10,boardRelease%10,VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH+'a') |
| ; |
| LCD_printfxy(0,2,"Setting: %d %s", getActiveParamSet(), mixerMatrix.name) |
| ; |
| if (I2CTimeout < 6) { |
| LCD_printfxy(0,3,"I2C Error!!!"); |
| } else if (missingMotor) { |
| LCD_printfxy(0,3,"Missing BL-Ctrl:%d", missingMotor); |
| } else LCD_printfxy(0,3,"(c) Holger Buss"); |
| break; |
| /* |
| case 1:// Height Control Menu Item |
| if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) { |
| LCD_printfxy(0,0,"Height: %5i",ReadingHeight); |
| LCD_printfxy(0,1,"Set Point: %5i",SetPointHeight); |
| LCD_printfxy(0,2,"Air Press.:%5i",0); |
| LCD_printfxy(0,3,"Offset :%5i",0); |
| } |
| else |
| { |
| LCD_printfxy(0,1,"No "); |
| LCD_printfxy(0,2,"Height Control"); |
| } |
| break; |
| */ |
| case 2:// Attitude Menu Item |
| LCD_printfxy(0,0,"Attitude"); |
| LCD_printfxy(0,1,"Nick: %5i", angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL); |
| LCD_printfxy(0,2,"Roll: %5i", angle[ROLL ] / GYRO_DEG_FACTOR_PITCHROLL); |
| LCD_printfxy(0,3,"Heading(M):%5i", magneticHeading); |
| break; |
| case 3:// Remote Control Channel Menu Item |
| LCD_printfxy(0,0,"C1:%4i C2:%4i ",PPM_in[1],PPM_in[2]); |
| LCD_printfxy(0,1,"C3:%4i C4:%4i ",PPM_in[3],PPM_in[4]); |
| LCD_printfxy(0,2,"C5:%4i C6:%4i ",PPM_in[5],PPM_in[6]); |
| LCD_printfxy(0,3,"C7:%4i C8:%4i ",PPM_in[7],PPM_in[8]); |
| break; |
| case 4:// Remote Control Mapping Menu Item |
| LCD_printfxy(0,0,"Ni:%4i Ro:%4i ",PPM_in[channelMap.channels[CH_PITCH]], PPM_in[channelMap.channels[CH_ROLL]]); |
| LCD_printfxy(0,1,"Gs:%4i Ya:%4i ",PPM_in[channelMap.channels[CH_THROTTLE]],PPM_in[channelMap.channels[CH_YAW]]); |
| LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[channelMap.channels[CH_POTS]], PPM_in[channelMap.channels[CH_POTS+1]]); |
| LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[channelMap.channels[CH_POTS+2]], PPM_in[channelMap.channels[CH_POTS+3]]); |
| break; |
| /* |
| case 5:// Gyro Sensor Menu Item |
| LCD_printfxy(0,0,"Gyro - Sensor"); |
| switch(BoardRelease) { |
| case 10: |
| LCD_printfxy(0,1,"Nick %4i (%3i.%i)", AdValueGyroNick - HiResNickOffset / HIRES_GYRO_AMPLIFY, HiResNickOffset / HIRES_GYRO_AMPLIFY, HiResNickOffset % HIRES_GYRO_AMPLIFY); |
| LCD_printfxy(0,2,"Roll %4i (%3i.%i)", AdValueGyroRoll - HiResRollOffset / HIRES_GYRO_AMPLIFY, HiResRollOffset / HIRES_GYRO_AMPLIFY, HiResRollOffset % HIRES_GYRO_AMPLIFY); |
| LCD_printfxy(0,3,"Yaw %4i (%3i)", AdValueGyroYaw , YawOffset); |
| break; |
| case 11: |
| case 12: |
| case 20: // divice Offests by 2 becuse 2 samples are added in adc isr |
| LCD_printfxy(0,1,"Nick %4i (%3i.%i)",0, HiResNickOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResNickOffset % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7) |
| LCD_printfxy(0,2,"Roll %4i (%3i.%i)",0, HiResRollOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResRollOffset % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7) |
| LCD_printfxy(0,3,"Yaw %4i (%3i)",YawOffset - AdValueGyroYaw , YawOffset/2); |
| break; |
| switch (menuItem) { |
| case 0: // Version Info Menu Item |
| LCD_printfxy(0, 0, "+ MikroKopter +"); |
| LCD_printfxy( |
| 0, |
| 1, |
| "HW:V%d.%d SW:%d.%d%c", |
| boardRelease/10, boardRelease%10, VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH+'a'); |
| LCD_printfxy(0, 2, "Setting: %d %s", getActiveParamSet(), mixerMatrix.name); |
| if (I2CTimeout < 6) { |
| LCD_printfxy(0, 3, "I2C Error!!!"); |
| } else if (missingMotor) { |
| LCD_printfxy(0, 3, "Missing BL-Ctrl:%d", missingMotor); |
| } else |
| LCD_printfxy(0, 3, "(c) Holger Buss"); |
| break; |
| /* |
| case 1:// Height Control Menu Item |
| if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) { |
| LCD_printfxy(0,0,"Height: %5i",ReadingHeight); |
| LCD_printfxy(0,1,"Set Point: %5i",SetPointHeight); |
| LCD_printfxy(0,2,"Air Press.:%5i",0); |
| LCD_printfxy(0,3,"Offset :%5i",0); |
| } |
| else |
| { |
| LCD_printfxy(0,1,"No "); |
| LCD_printfxy(0,2,"Height Control"); |
| } |
| break; |
| */ |
| case 2: // Attitude Menu Item |
| LCD_printfxy(0, 0, "Attitude"); |
| LCD_printfxy(0, 1, "Nick: %5i", |
| attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL); |
| LCD_printfxy(0, 2, "Roll: %5i", |
| attitude[ROLL ] / GYRO_DEG_FACTOR_PITCHROLL); |
| LCD_printfxy(0, 3, "Heading(M):%5i", magneticHeading); |
| break; |
| case 3: // Remote Control Channel Menu Item |
| LCD_printfxy(0, 0, "C1:%4i C2:%4i ", PPM_in[1], PPM_in[2]); |
| LCD_printfxy(0, 1, "C3:%4i C4:%4i ", PPM_in[3], PPM_in[4]); |
| LCD_printfxy(0, 2, "C5:%4i C6:%4i ", PPM_in[5], PPM_in[6]); |
| LCD_printfxy(0, 3, "C7:%4i C8:%4i ", PPM_in[7], PPM_in[8]); |
| break; |
| case 4: // Remote Control Mapping Menu Item |
| LCD_printfxy( |
| 0, |
| 0, |
| "Ni:%4i Ro:%4i ", |
| PPM_in[channelMap.channels[CH_PITCH]], PPM_in[channelMap.channels[CH_ROLL]]); |
| LCD_printfxy( |
| 0, |
| 1, |
| "Gs:%4i Ya:%4i ", |
| PPM_in[channelMap.channels[CH_THROTTLE]], PPM_in[channelMap.channels[CH_YAW]]); |
| LCD_printfxy( |
| 0, |
| 2, |
| "P1:%4i P2:%4i ", |
| PPM_in[channelMap.channels[CH_POTS]], PPM_in[channelMap.channels[CH_POTS+1]]); |
| LCD_printfxy( |
| 0, |
| 3, |
| "P3:%4i P4:%4i ", |
| PPM_in[channelMap.channels[CH_POTS+2]], PPM_in[channelMap.channels[CH_POTS+3]]); |
| break; |
| /* |
| case 5:// Gyro Sensor Menu Item |
| LCD_printfxy(0,0,"Gyro - Sensor"); |
| switch(BoardRelease) { |
| case 10: |
| LCD_printfxy(0,1,"Nick %4i (%3i.%i)", AdValueGyroNick - HiResNickOffset / HIRES_GYRO_AMPLIFY, HiResNickOffset / HIRES_GYRO_AMPLIFY, HiResNickOffset % HIRES_GYRO_AMPLIFY); |
| LCD_printfxy(0,2,"Roll %4i (%3i.%i)", AdValueGyroRoll - HiResRollOffset / HIRES_GYRO_AMPLIFY, HiResRollOffset / HIRES_GYRO_AMPLIFY, HiResRollOffset % HIRES_GYRO_AMPLIFY); |
| LCD_printfxy(0,3,"Yaw %4i (%3i)", AdValueGyroYaw , YawOffset); |
| break; |
| case 11: |
| case 12: |
| case 20: // divice Offests by 2 becuse 2 samples are added in adc isr |
| LCD_printfxy(0,1,"Nick %4i (%3i.%i)",0, HiResNickOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResNickOffset % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7) |
| LCD_printfxy(0,2,"Roll %4i (%3i.%i)",0, HiResRollOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResRollOffset % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7) |
| LCD_printfxy(0,3,"Yaw %4i (%3i)",YawOffset - AdValueGyroYaw , YawOffset/2); |
| break; |
| case 13: |
| default: // divice Offests by 2 becuse 2 samples are added in adc isr |
| LCD_printfxy(0,1,"Nick %4i (%3i.%i)(%3i)",0, HiResNickOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResNickOffset % (HIRES_GYRO_AMPLIFY * 2))/2, 0); // division by 2 to push the reminder below 10 (15/2 = 7) |
| LCD_printfxy(0,2,"Roll %4i (%3i.%i)(%3i)",0, HiResRollOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResRollOffset % (HIRES_GYRO_AMPLIFY * 2))/2, 0); // division by 2 to push the reminder below 10 (15/2 = 7) |
| LCD_printfxy(0,3,"Yaw %4i (%3i)(%3i)",YawOffset - AdValueGyroYaw , YawOffset/2, 0); |
| break; |
| } |
| break; |
| case 6:// Acceleration Sensor Menu Item |
| LCD_printfxy(0,0,"ACC - Sensor"); |
| LCD_printfxy(0,1,"Nick %4i (%3i)",0,0); // factor 2 because of adding 2 samples in ADC ISR |
| LCD_printfxy(0,2,"Roll %4i (%3i)",0,0); // factor 2 because of adding 2 samples in ADC ISR |
| LCD_printfxy(0,3,"Height %4i (%3i)",0,0); |
| break; |
| */ |
| case 7:// Battery Voltage / Remote Control Level |
| LCD_printfxy(0,1,"Voltage: %3i.%1iV",UBat/10, UBat%10); |
| LCD_printfxy(0,2,"RC-Level: %5i",RCQuality); |
| break; |
| case 8:// Compass Menu Item |
| LCD_printfxy(0,0,"Compass "); |
| LCD_printfxy(0,1,"Course: %5i", compassCourse); |
| LCD_printfxy(0,2,"Heading: %5i", magneticHeading); |
| LCD_printfxy(0,3,"OffCourse: %5i", ((540 + magneticHeading - compassCourse) % 360) - 180); |
| break; |
| case 9:// Poti Menu Item |
| LCD_printfxy(0,0,"Po1: %3i Po5: %3i" ,variables[0], variables[4]); //PPM24-Extesion |
| LCD_printfxy(0,1,"Po2: %3i Po6: %3i" ,variables[1], variables[5]); //PPM24-Extesion |
| LCD_printfxy(0,2,"Po3: %3i Po7: %3i" ,variables[2], variables[6]); //PPM24-Extesion |
| LCD_printfxy(0,3,"Po4: %3i Po8: %3i" ,variables[3], variables[7]); //PPM24-Extesion |
| break; |
| /* |
| case 10:// Servo Menu Item |
| LCD_printfxy(0,0,"Servo " ); |
| LCD_printfxy(0,1,"Setpoint %3i",dynamicParams.ServoNickControl); |
| LCD_printfxy(0,2,"Position: %3i",ServoNickValue); |
| LCD_printfxy(0,3,"Range:%3i-%3i",staticParams.ServoNickMin, staticParams.ServoNickMax); |
| break; |
| */ |
| case 11://Extern Control |
| LCD_printfxy(0,0,"ExternControl " ); |
| LCD_printfxy(0,1,"Ni:%4i Ro:%4i ", externalControl.pitch, externalControl.roll); |
| LCD_printfxy(0,2,"Gs:%4i Ya:%4i ", externalControl.throttle, externalControl.yaw); |
| LCD_printfxy(0,3,"Hi:%4i Cf:%4i ", externalControl.height, externalControl.config); |
| break; |
| case 13: |
| default: // divice Offests by 2 becuse 2 samples are added in adc isr |
| LCD_printfxy(0,1,"Nick %4i (%3i.%i)(%3i)",0, HiResNickOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResNickOffset % (HIRES_GYRO_AMPLIFY * 2))/2, 0); // division by 2 to push the reminder below 10 (15/2 = 7) |
| LCD_printfxy(0,2,"Roll %4i (%3i.%i)(%3i)",0, HiResRollOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResRollOffset % (HIRES_GYRO_AMPLIFY * 2))/2, 0); // division by 2 to push the reminder below 10 (15/2 = 7) |
| LCD_printfxy(0,3,"Yaw %4i (%3i)(%3i)",YawOffset - AdValueGyroYaw , YawOffset/2, 0); |
| break; |
| } |
| break; |
| case 6:// Acceleration Sensor Menu Item |
| LCD_printfxy(0,0,"ACC - Sensor"); |
| LCD_printfxy(0,1,"Nick %4i (%3i)",0,0); // factor 2 because of adding 2 samples in ADC ISR |
| LCD_printfxy(0,2,"Roll %4i (%3i)",0,0); // factor 2 because of adding 2 samples in ADC ISR |
| LCD_printfxy(0,3,"Height %4i (%3i)",0,0); |
| break; |
| */ |
| case 7: // Battery Voltage / Remote Control Level |
| LCD_printfxy(0, 1, "Voltage: %3i.%1iV", UBat/10, UBat%10); |
| LCD_printfxy(0, 2, "RC-Level: %5i", RCQuality); |
| break; |
| case 8: // Compass Menu Item |
| LCD_printfxy(0, 0, "Compass "); |
| LCD_printfxy(0, 0, "TODO: Impl. "); |
| break; |
| case 9: // Poti Menu Item |
| LCD_printfxy(0, 0, "Variables "); |
| LCD_printfxy(0, 0, "TODO: Impl. "); |
| break; |
| case 10: // Servo Menu Item |
| LCD_printfxy(0, 0, "Servos "); |
| LCD_printfxy(0, 0, "TODO: Impl. "); |
| break; |
| case 11: //Extern Control |
| LCD_printfxy(0, 0, "ExternControl "); |
| LCD_printfxy(0, 1, "Ni:%4i Ro:%4i ", |
| externalControl.pitch, externalControl.roll); |
| LCD_printfxy(0, 2, "Gs:%4i Ya:%4i ", |
| externalControl.throttle, externalControl.yaw); |
| LCD_printfxy(0, 3, "Hi:%4i Cf:%4i ", |
| externalControl.height, externalControl.config); |
| break; |
| case 12://BL Communication errors |
| LCD_printfxy(0,0,"BL-Ctrl Errors " ); |
| LCD_printfxy(0,1," %3d %3d %3d %3d ",motor[0].error,motor[1].error,motor[2].error,motor[3].error); |
| LCD_printfxy(0,2," %3d %3d %3d %3d ",motor[4].error,motor[5].error,motor[6].error,motor[7].error); |
| LCD_printfxy(0,3," %3d %3d %3d %3d ",motor[8].error,motor[9].error,motor[10].error,motor[11].error); |
| break; |
| case 12: //BL Communication errors |
| LCD_printfxy(0, 0, "BL-Ctrl Errors "); |
| LCD_printfxy(0, 1, " %3d %3d %3d %3d ", |
| motor[0].error, motor[1].error, motor[2].error, motor[3].error); |
| LCD_printfxy(0, 2, " %3d %3d %3d %3d ", |
| motor[4].error, motor[5].error, motor[6].error, motor[7].error); |
| LCD_printfxy(0, 3, " %3d %3d %3d %3d ", |
| motor[8].error, motor[9].error, motor[10].error, motor[11].error); |
| break; |
| case 13://BL Overview |
| LCD_printfxy(0,0,"BL-Ctrl found " ); |
| LCD_printfxy(0,1," %c %c %c %c ",motor[0].present + '-',motor[1].present + '-',motor[2].present + '-',motor[3].present + '-'); |
| LCD_printfxy(0,2," %c %c %c %c ",motor[4].present + '-',motor[5].present + '-',motor[6].present + '-',motor[7].present + '-'); |
| LCD_printfxy(0,3," %c - - - ",motor[8].present + '-'); |
| if (motor[9].present) |
| LCD_printfxy(4,3,"10"); |
| if (motor[10].present) |
| LCD_printfxy(8,3,"11"); |
| if (motor[11].present) |
| LCD_printfxy(12,3,"12"); |
| break; |
| case 13: //BL Overview |
| LCD_printfxy(0, 0, "BL-Ctrl found "); |
| LCD_printfxy( |
| 0, |
| 1, |
| " %c %c %c %c ", |
| motor[0].present + '-', motor[1].present + '-', motor[2].present + '-', motor[3].present + '-'); |
| LCD_printfxy( |
| 0, |
| 2, |
| " %c %c %c %c ", |
| motor[4].present + '-', motor[5].present + '-', motor[6].present + '-', motor[7].present + '-'); |
| LCD_printfxy(0, 3, " %c - - - ", motor[8].present + '-'); |
| if (motor[9].present) |
| LCD_printfxy(4, 3, "10"); |
| if (motor[10].present) |
| LCD_printfxy(8, 3, "11"); |
| if (motor[11].present) |
| LCD_printfxy(12, 3, "12"); |
| break; |
| #if (defined (USE_NAVICTRL)) |
| case 14://GPS Lat/Lon coords |
| if (GPSInfo.status == INVALID) { |
| LCD_printfxy(0,0,"No GPS data!"); |
| } else { |
| switch (GPSInfo.satfix) |
| { |
| case SATFIX_NONE: |
| LCD_printfxy(0,0,"Sats: %d Fix: No", GPSInfo.satnum); |
| break; |
| case SATFIX_2D: |
| LCD_printfxy(0,0,"Sats: %d Fix: 2D", GPSInfo.satnum); |
| break; |
| case SATFIX_3D: |
| LCD_printfxy(0,0,"Sats: %d Fix: 3D", GPSInfo.satnum); |
| break; |
| default: |
| LCD_printfxy(0,0,"Sats: %d Fix: ??", GPSInfo.satnum); |
| break; |
| } |
| int16_t i1,i2,i3; |
| i1 = (int16_t)(GPSInfo.longitude/10000000L); |
| i2 = abs((int16_t)((GPSInfo.longitude%10000000L)/10000L)); |
| i3 = abs((int16_t)(((GPSInfo.longitude%10000000L)%10000L)/10L)); |
| LCD_printfxy(0,1,"Lon: %d.%03d%03d deg",i1, i2, i3); |
| i1 = (int16_t)(GPSInfo.latitude/10000000L); |
| i2 = abs((int16_t)((GPSInfo.latitude%10000000L)/10000L)); |
| i3 = abs((int16_t)(((GPSInfo.latitude%10000000L)%10000L)/10L)); |
| LCD_printfxy(0,2,"Lat: %d.%03d%03d deg",i1, i2, i3); |
| i1 = (int16_t)(GPSInfo.altitude/1000L); |
| i2 = abs((int16_t)(GPSInfo.altitude%1000L)); |
| LCD_printfxy(0,3,"Alt: %d.%03d m",i1, i2); |
| } |
| break; |
| case 14: //GPS Lat/Lon coords |
| if (GPSInfo.status == INVALID) { |
| LCD_printfxy(0,0,"No GPS data!"); |
| } else { |
| switch (GPSInfo.satfix) |
| { |
| case SATFIX_NONE: |
| LCD_printfxy(0,0,"Sats: %d Fix: No", GPSInfo.satnum); |
| break; |
| case SATFIX_2D: |
| LCD_printfxy(0,0,"Sats: %d Fix: 2D", GPSInfo.satnum); |
| break; |
| case SATFIX_3D: |
| LCD_printfxy(0,0,"Sats: %d Fix: 3D", GPSInfo.satnum); |
| break; |
| default: |
| LCD_printfxy(0,0,"Sats: %d Fix: ??", GPSInfo.satnum); |
| break; |
| } |
| int16_t i1,i2,i3; |
| i1 = (int16_t)(GPSInfo.longitude/10000000L); |
| i2 = abs((int16_t)((GPSInfo.longitude%10000000L)/10000L)); |
| i3 = abs((int16_t)(((GPSInfo.longitude%10000000L)%10000L)/10L)); |
| LCD_printfxy(0,1,"Lon: %d.%03d%03d deg",i1, i2, i3); |
| i1 = (int16_t)(GPSInfo.latitude/10000000L); |
| i2 = abs((int16_t)((GPSInfo.latitude%10000000L)/10000L)); |
| i3 = abs((int16_t)(((GPSInfo.latitude%10000000L)%10000L)/10L)); |
| LCD_printfxy(0,2,"Lat: %d.%03d%03d deg",i1, i2, i3); |
| i1 = (int16_t)(GPSInfo.altitude/1000L); |
| i2 = abs((int16_t)(GPSInfo.altitude%1000L)); |
| LCD_printfxy(0,3,"Alt: %d.%03d m",i1, i2); |
| } |
| break; |
| #endif |
| default: |
| maxMenuItem = menuItem - 1; |
| menuItem = 0; |
| break; |
| } |
| remoteKeys = 0; |
| default: |
| maxMenuItem = menuItem - 1; |
| menuItem = 0; |
| break; |
| } |
| remoteKeys = 0; |
| } |
| /branches/dongfang_FC_rewrite/naviControl.h |
|---|
| 4,6 → 4,6 |
| #include<inttypes.h> |
| void navi_setNeutral(void); |
| void navigation_periodicTask(int16_t* naviSticks); |
| void navigation_periodicTaskAndPRTY(int16_t* PRTY); |
| #endif |
| /branches/dongfang_FC_rewrite/output.h |
|---|
| 48,7 → 48,7 |
| #define DEBUG_HEIGHT_DIFF 2 |
| #define DEBUG_HOVERTHROTTLE 4 |
| #define DEBUG_ACC0THORDER 8 |
| #define DEBUG_SIGNAL 16 |
| #define DEBUG_COMPASS 16 |
| #define DEBUG_PRESSURERANGE 32 |
| #define DEBUG_CLIP 64 |
| #define DEBUG_SENSORLIMIT 128 |
| /branches/dongfang_FC_rewrite/rc.c |
|---|
| 1,53 → 1,3 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + Nur f�r den privaten Gebrauch |
| // + www.MikroKopter.com |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation), |
| // + dass eine Nutzung (auch auszugsweise) nur f�r den privaten (nicht-kommerziellen) Gebrauch zul�ssig ist. |
| // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
| // + bzgl. der Nutzungsbedingungen aufzunehmen. |
| // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen, |
| // + Verkauf von Luftbildaufnahmen, usw. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht, |
| // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
| // + auf anderen Webseiten oder sonstigen Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zul�ssig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <stdlib.h> |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| 58,12 → 8,10 |
| #include "configuration.h" |
| #include "commands.h" |
| // The channel array is now 0-based. |
| // The channel array is 0-based! |
| volatile int16_t PPM_in[MAX_CHANNELS]; |
| volatile int16_t PPM_diff[MAX_CHANNELS]; |
| volatile uint8_t newPPMData = 1; |
| volatile uint8_t RCQuality; |
| int16_t RC_PRTY[4]; |
| uint8_t lastRCCommand = COMMAND_NONE; |
| uint8_t commandTimer = 0; |
| 140,8 → 88,7 |
| The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is |
| the syncronization gap. |
| */ |
| ISR(TIMER1_CAPT_vect) |
| { // typical rate of 1 ms to 2 ms |
| ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms |
| int16_t signal = 0, tmp; |
| static int16_t index; |
| static uint16_t oldICR1 = 0; |
| 157,12 → 104,6 |
| //sync gap? (3.52 ms < signal < 25.6 ms) |
| if ((signal > 1100) && (signal < 8000)) { |
| // if a sync gap happens and there where at least 4 channels decoded before |
| // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array. |
| if (index >= 3) { |
| newPPMData = 0; // Null means NewData for the first 4 channels |
| } |
| // synchronize channel index |
| index = 0; |
| } else { // within the PPM frame |
| if (index < MAX_CHANNELS) { // PPM24 supports 12 channels |
| 237,30 → 178,23 |
| } |
| /* |
| * This must be called (as the only thing) for each control loop cycle (488 Hz). |
| * Get Pitch, Roll, Throttle, Yaw values |
| */ |
| void RC_periodicTask() { |
| void RC_periodicTaskAndPRTY(int16_t* PRTY) { |
| int16_t tmp1, tmp2; |
| if (RCQuality) { |
| RCQuality--; |
| if (newPPMData-- == 0) { |
| RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP |
| + RCDiff(CH_PITCH) * staticParams.stickD; |
| RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP |
| + RCDiff(CH_ROLL) * staticParams.stickD; |
| RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) |
| * staticParams.stickThrottleD + 120; |
| if (RC_PRTY[CONTROL_THROTTLE] < 0) |
| RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
| tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
| // exponential stick sensitivity in yawing rate |
| tmp2 = (int32_t) staticParams.stickYawP * ((int32_t) tmp1 * abs(tmp1)) |
| / 512L; // expo y = ax + bx^2 |
| tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
| RC_PRTY[CONTROL_YAW] = tmp2; |
| } |
| PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD; |
| PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD; |
| PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120; |
| if (PRTY[CONTROL_THROTTLE] < 0) PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
| tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
| // exponential stick sensitivity in yawing rate |
| tmp2 = (int32_t) staticParams.stickYawP * ((int32_t) tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
| tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
| PRTY[CONTROL_YAW] = tmp2; |
| uint8_t command = RC_getStickCommand(); |
| if (lastRCCommand == command) { |
| // Keep timer from overrunning. |
| if (commandTimer < COMMAND_TIMER) |
| 270,20 → 204,11 |
| lastRCCommand = command; |
| commandTimer = 0; |
| } |
| } else { // Bad signal |
| RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0; |
| } |
| debugOut.analog[18] = RCQuality; |
| } |
| /* |
| * Get Pitch, Roll, Throttle, Yaw values |
| */ |
| int16_t* RC_getPRTY(void) { |
| return RC_PRTY; |
| } |
| /* |
| * Get other channel value |
| */ |
| int16_t RC_getVariable(uint8_t varNum) { |
| 384,53 → 309,16 |
| } |
| /* |
| uint8_t RC_getLooping(uint8_t looping) { |
| // static uint8_t looping = 0; |
| if (RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig |
| & CFG_LOOP_LEFT) { |
| looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT); |
| } else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL) |
| < staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
| looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT)); |
| } |
| if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold |
| && staticParams.BitConfig & CFG_LOOP_RIGHT) { |
| looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT); |
| } else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL) |
| > -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
| looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT)); |
| } |
| if (RCChannel(CH_PITCH) > staticParams.LoopThreshold |
| && staticParams.BitConfig & CFG_LOOP_UP) { |
| looping |= (LOOPING_PITCH_AXIS | LOOPING_UP); |
| } else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH) |
| < staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
| looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP)); |
| } |
| if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold |
| && staticParams.BitConfig & CFG_LOOP_DOWN) { |
| looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN); |
| } else if ((looping & LOOPING_DOWN) && RCChannel(CH_PITCH) |
| > -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
| looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN)); |
| } |
| return looping; |
| } |
| */ |
| * For each time the stick is pulled, returns true. |
| */ |
| uint8_t RC_testCompassCalState(void) { |
| static uint8_t stick = 1; |
| static uint8_t stickPulled = 1; |
| // if pitch is centered or top set stick to zero |
| if (RCChannel(CH_PITCH) > -20) |
| stick = 0; |
| stickPulled = 0; |
| // if pitch is down trigger to next cal state |
| if ((RCChannel(CH_PITCH) < -70) && !stick) { |
| stick = 1; |
| if ((RCChannel(CH_PITCH) < -70) && !stickPulled) { |
| stickPulled = 1; |
| return 1; |
| } |
| return 0; |
| /branches/dongfang_FC_rewrite/rc.h |
|---|
| 30,8 → 30,8 |
| uint8_t RC_hasNewRCData (void); |
| */ |
| void RC_periodicTask(void); |
| int16_t* RC_getPRTY(void); |
| // void RC_periodicTask(void); |
| void RC_periodicTaskAndPRTY(int16_t* PRTY); |
| uint8_t RC_getArgument(void); |
| uint8_t RC_getCommand(void); |
| int16_t RC_getVariable(uint8_t varNum); |
| /branches/dongfang_FC_rewrite/timer2.c |
|---|
| 154,7 → 154,7 |
| } servo_t;*/ |
| int16_t calculateStabilizedServoAxis(uint8_t axis) { |
| int32_t value = angle[axis] / 64L; // between -500000 to 500000 extreme limits. Just about |
| int32_t value = attitude[axis] / 64L; // between -500000 to 500000 extreme limits. Just about |
| // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
| // That is a divisor of about 1<<14. Same conclusion as H&I. |
| value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
| /branches/dongfang_FC_rewrite/uart0.c |
|---|
| 13,6 → 13,7 |
| #include "externalControl.h" |
| #include "output.h" |
| #include "attitude.h" |
| #include "commands.h" |
| #ifdef USE_DIRECT_GPS |
| #include "mk3mag.h" |
| 96,10 → 97,10 |
| "HeightPdThrottle", |
| "HeightIdThrottle", //25 |
| "HeightDdThrottle", |
| "NaviStickPitch ", |
| "NaviStickRoll ", |
| "StickPitch ", |
| "StickRoll ", //30 |
| "HeadingInDegrees", |
| "TargetHeadingDeg", |
| "Diff ", |
| " ", //30 |
| "navi mode " |
| }; |
| 654,9 → 655,9 |
| if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) |
| && txd_complete) { |
| sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D)); |
| data3D.anglePitch = (int16_t) (angle[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1° |
| data3D.angleRoll = (int16_t) (angle[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1° |
| data3D.heading = (int16_t) (yawGyroHeading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1° |
| data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1° |
| data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1° |
| data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1° |
| data3D_timer = setDelay(data3D_interval); |
| request_data3D = FALSE; |
| } |
| 669,8 → 670,8 |
| #ifdef USE_DIRECT_GPS |
| if((checkDelay(toMk3MagTimer)) && txd_complete) { |
| toMk3Mag.attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
| toMk3Mag.attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
| toMk3Mag.attitude[0] = (int16_t)(attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg |
| toMk3Mag.attitude[1] = (int16_t)(attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg |
| toMk3Mag.userParam[0] = dynamicParams.userParams[0]; |
| toMk3Mag.userParam[1] = dynamicParams.userParams[1]; |
| toMk3Mag.calState = compassCalState; |