| /branches/dongfang_FC_rewrite/attitude.c |
|---|
| 220,6 → 220,8 |
| * - it corrects the differential of the integral = the gyro offsets. |
| * That should only be necessary with drifty gyros like ENC-03. |
| ************************************************************************/ |
| #define LOG_DIVIDER 12 |
| #define DIVIDER (1L << LOG_DIVIDER) |
| void correctIntegralsByAcc0thOrder(void) { |
| // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
| // are less than ....., or reintroduce Kalman. |
| 227,55 → 229,45 |
| uint8_t axis; |
| int32_t temp; |
| uint8_t ca = controlActivity >> 8; |
| uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
| uint16_t ca = controlActivity >> 6; |
| uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance; |
| if (!controlActivityWeighted) controlActivityWeighted = 1; |
| uint8_t accVectorWeighted = accVector / staticParams.zerothOrderCorrectionAccTolerance; |
| if (!accVectorWeighted) accVectorWeighted = 1; |
| if (highControlActivity) { |
| debugOut.digital[1] |= DEBUG_ACC0THORDER; |
| } else { |
| debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
| } |
| uint8_t accPart = staticParams.zerothOrderCorrection; |
| int32_t accDerived; |
| if (accVector <= dynamicParams.maxAccVector) { |
| debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
| debugOut.analog[14] = controlActivity; |
| debugOut.analog[15] = accVector; |
| 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; |
| */ |
| debugOut.analog[20] = controlActivityWeighted; |
| debugOut.analog[21] = accVectorWeighted; |
| debugOut.analog[24] = accVector; |
| if (highControlActivity) { // reduce effect during stick control activity |
| permilleAcc /= 4; |
| 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. |
| */ |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| 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 = attitude[axis]; |
| attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp |
| + (int32_t) permilleAcc * accDerived) / 1000L; |
| correctionSum[axis] += attitude[axis] - temp; |
| } |
| } else { |
| // experiment: Kill drift compensation updates when not flying smooth. |
| // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
| debugOut.digital[0] |= DEBUG_ACC0THORDER; |
| accPart /= controlActivityWeighted; |
| accPart /= accVectorWeighted; |
| /* |
| * Add to each sum: The amount by which the angle is changed just below. |
| */ |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| 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 = attitude[axis]; |
| attitude[axis] = ((int32_t) (DIVIDER - accPart) * temp + (int32_t)accPart * accDerived) >> LOG_DIVIDER; |
| correctionSum[axis] += attitude[axis] - temp; |
| } |
| } |
| 351,7 → 343,7 |
| int32_t error; |
| if (commands_isCalibratingCompass()) { |
| debugOut.analog[30] = -1; |
| //debugOut.analog[30] = -1; |
| return; |
| } |
| 362,13 → 354,13 |
| // Compass is invalid, skip. |
| if (magneticHeading < 0) { |
| debugOut.analog[30] = -2; |
| //debugOut.analog[30] = -2; |
| return; |
| } |
| // Spinning fast, skip |
| if (abs(yawRate) > 128) { |
| debugOut.analog[30] = -3; |
| // debugOut.analog[30] = -3; |
| return; |
| } |
| 375,11 → 367,11 |
| // Otherwise invalidated, skip |
| if (ignoreCompassTimer) { |
| ignoreCompassTimer--; |
| debugOut.analog[30] = -4; |
| //debugOut.analog[30] = -4; |
| return; |
| } |
| debugOut.analog[30] = magneticHeading; |
| //debugOut.analog[30] = magneticHeading; |
| // TODO: Find computational cost of this. |
| error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading); |
| 395,10 → 387,9 |
| //debugOut.analog[30] = correction; |
| // 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 . |
| // and to the heading error (the angle of yaw that the copter is off the set heading). |
| heading += correction; |
| headingError += correction; |
| intervalWrap(&heading, YAWOVER360); |
| // If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all |
| /branches/dongfang_FC_rewrite/configuration.c |
|---|
| 56,12 → 56,13 |
| #include "sensors.h" |
| #include "rc.h" |
| #include "output.h" |
| #include "flight.h" |
| int16_t variables[VARIABLE_COUNT]; |
| ParamSet_t staticParams; |
| channelMap_t channelMap; |
| mixerMatrix_t mixerMatrix; |
| volatile dynamicParam_t dynamicParams; |
| volatile DynamicParams_t dynamicParams; |
| uint8_t CPUType = ATMEGA644; |
| uint8_t boardRelease = 13; |
| 73,59 → 74,28 |
| uint16_t isFlying = 0; |
| volatile uint8_t MKFlags = 0; |
| /************************************************************************ |
| * Map the parameter to pot values |
| * Replacing this code by the code below saved almost 1 kbyte. |
| ************************************************************************/ |
| const MMXLATION XLATIONS[] = { |
| {offsetof(ParamSet_t, levelCorrection[0]), offsetof(DynamicParams_t, levelCorrection[0]),0,255}, |
| {offsetof(ParamSet_t, levelCorrection[1]), offsetof(DynamicParams_t, levelCorrection[1]),0,255}, |
| {offsetof(ParamSet_t, gyroP), offsetof(DynamicParams_t, gyroP),0,255}, |
| {offsetof(ParamSet_t, gyroI), offsetof(DynamicParams_t, gyroI),0,255}, |
| {offsetof(ParamSet_t, gyroD), offsetof(DynamicParams_t, gyroD),0,255}, |
| {offsetof(ParamSet_t, attitudeControl), offsetof(DynamicParams_t, attitudeControl),0,255}, |
| {offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255}, |
| {offsetof(ParamSet_t, dynamicStability), offsetof(DynamicParams_t, dynamicStability),0,255}, |
| {offsetof(ParamSet_t, heightP), offsetof(DynamicParams_t, heightP),0,255}, |
| {offsetof(ParamSet_t, heightI), offsetof(DynamicParams_t, heightI),0,255}, |
| {offsetof(ParamSet_t, heightD), offsetof(DynamicParams_t, heightD),0,255}, |
| {offsetof(ParamSet_t, heightSetting), offsetof(DynamicParams_t, heightSetting),0,255}, |
| {offsetof(ParamSet_t, servoConfigurations[0].manualControl), offsetof(DynamicParams_t, servoManualControl[0]),0,255}, |
| {offsetof(ParamSet_t, servoConfigurations[1].manualControl), offsetof(DynamicParams_t, servoManualControl[1]),0,255}, |
| {offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255}, |
| {offsetof(ParamSet_t, outputFlash[1].timing), offsetof(DynamicParams_t, output1Timing),0,255}, |
| {offsetof(ParamSet_t, naviMode), offsetof(DynamicParams_t, naviMode),0,255}}; |
| void configuration_applyVariablesToParams(void) { |
| uint8_t i; |
| #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); |
| SET_POT(dynamicParams.gyroI, staticParams.gyroI); |
| SET_POT(dynamicParams.gyroD, staticParams.gyroD); |
| //SET_POT(dynamicParams.compassControlHeading,staticParams.compassControlHeading); |
| SET_POT(dynamicParams.externalControl, staticParams.externalControl); |
| SET_POT(dynamicParams.dynamicStability,staticParams.dynamicStability); |
| SET_POT(dynamicParams.maxAccVector,staticParams.maxAccVector); |
| SET_POT(dynamicParams.heightP, staticParams.heightP); |
| SET_POT(dynamicParams.heightI, staticParams.heightI); |
| SET_POT(dynamicParams.heightD, staticParams.heightD); |
| SET_POT(dynamicParams.heightSetting,staticParams.heightSetting); |
| SET_POT(dynamicParams.attitudeControl,staticParams.attitudeControl); |
| SET_POT(dynamicParams.servoManualControl[0], staticParams.servoConfigurations[0].manualControl); |
| SET_POT(dynamicParams.servoManualControl[1], staticParams.servoConfigurations[1].manualControl); |
| SET_POT_MM(dynamicParams.output0Timing, staticParams.outputFlash[0].timing,1,255); |
| SET_POT_MM(dynamicParams.output1Timing, staticParams.outputFlash[1].timing,1,255); |
| SET_POT(dynamicParams.levelCorrection[0], staticParams.levelCorrection[0]); |
| SET_POT(dynamicParams.levelCorrection[1], staticParams.levelCorrection[1]); |
| SET_POT(dynamicParams.naviMode, staticParams.naviMode); |
| for (i=0; i<sizeof(staticParams.userParams); i++) { |
| SET_POT(dynamicParams.userParams[i],staticParams.userParams[i]); |
| } |
| } |
| const XLATION XLATIONS[] = { |
| {offsetof(ParamSet_t, heightSetting), offsetof(dynamicParam_t, heightSetting)}, |
| }; |
| const MMXLATION MMXLATIONS[] = { |
| {offsetof(ParamSet_t, heightD), offsetof(dynamicParam_t, heightD),0,100}, |
| }; |
| uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) { |
| uint8_t result; |
| if (src>=251) result = variables[src-251]; |
| if (src>=(256-VARIABLE_COUNT)) result = variables[src-(256-VARIABLE_COUNT)]; |
| else result = src; |
| if (result < min) result = min; |
| else if (result > max) result = max; |
| 132,31 → 102,21 |
| return result; |
| } |
| void configuration_applyVariablesToParams_dead(void) { |
| void configuration_applyVariablesToParams(void) { |
| uint8_t i, src; |
| uint8_t* pointerToTgt; |
| for(i=0; i<sizeof(XLATIONS)/sizeof(XLATION); i++) { |
| src = *((uint8_t*)(&staticParams + XLATIONS[i].sourceIdx)); |
| pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx); |
| if (src < 255) { |
| *pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
| } |
| for(i=0; i<sizeof(XLATIONS)/sizeof(MMXLATION); i++) { |
| src = *((uint8_t*)(&staticParams) + XLATIONS[i].sourceIdx); |
| pointerToTgt = (uint8_t*)(&dynamicParams) + XLATIONS[i].targetIdx; |
| *pointerToTgt = configuration_applyVariableToParam(src, XLATIONS[i].min, XLATIONS[i].max); |
| } |
| for(i=0; i<sizeof(MMXLATIONS)/sizeof(MMXLATION); i++) { |
| src = *((uint8_t*)(&staticParams + MMXLATIONS[i].sourceIdx)); |
| pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx); |
| if (src < 255) { |
| *pointerToTgt = configuration_applyVariableToParam(src, MMXLATIONS[i].min, MMXLATIONS[i].max); |
| } |
| } |
| // User parameters are always variable. |
| for (i=0; i<sizeof(staticParams.userParams); i++) { |
| src = *((uint8_t*)(&staticParams + offsetof(ParamSet_t, userParams) + i)); |
| pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, userParams) + i); |
| if (src < 255) { |
| *pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
| } |
| src = *((uint8_t*)(&staticParams) + offsetof(ParamSet_t, userParams) + i); |
| pointerToTgt = (uint8_t*)(&dynamicParams) + offsetof(DynamicParams_t, userParams) + i; |
| *pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
| } |
| } |
| 176,7 → 136,6 |
| * tedious to have to modify the code for how to turn on and off LEDs when deploying |
| * on different HW version.... |
| */ |
| uint8_t getBoardRelease(void) { |
| uint8_t boardRelease = 13; |
| // the board release is coded via the pull up or down the 2 status LED |
| 209,6 → 168,24 |
| return boardRelease; |
| } |
| void configuration_setNormalFlightParameters(void) { |
| flight_setParameters(staticParams.IFactor, dynamicParams.gyroP, |
| staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI, |
| dynamicParams.gyroP, staticParams.yawIFactor); |
| } |
| void configuration_setFailsafeFlightParameters(void) { |
| flight_setParameters(0, 90, 120, 90, 120); |
| } |
| // Called after a change in configuration parameters, as a hook for modules to take over changes. |
| void configuration_paramSetDidChange(void) { |
| // This should be OK to do here as long as we don't change parameters during emergency flight. We don't. |
| configuration_setNormalFlightParameters(); |
| // Immediately load changes to output, and also signal the paramset change. |
| output_init(); |
| } |
| void setOtherDefaults(void) { |
| // Height Control |
| staticParams.airpressureFilterConstant = 8; |
| 246,7 → 223,8 |
| // staticParams.driftCompLimit = |
| staticParams.dynamicStability = 50; |
| staticParams.maxAccVector = 10; |
| staticParams.zerothOrderCorrectionAccTolerance = 60; |
| staticParams.zerothOrderCorrectionControlTolerance = 60; |
| staticParams.IFactor = 52; |
| staticParams.yawIFactor = 100; |
| staticParams.compassYawCorrection = 64; |
| /branches/dongfang_FC_rewrite/configuration.h |
|---|
| 47,7 → 47,6 |
| // Control |
| /* P */uint8_t externalControl; |
| /* P */uint8_t dynamicStability; |
| uint8_t maxAccVector; |
| // Height control |
| /*PMM*/uint8_t heightP; |
| 70,9 → 69,9 |
| uint8_t naviMode; |
| /* P */uint8_t userParams[8]; |
| } dynamicParam_t; |
| } DynamicParams_t; |
| extern volatile dynamicParam_t dynamicParams; |
| extern volatile DynamicParams_t dynamicParams; |
| typedef struct { |
| uint8_t sourceIdx, targetIdx; |
| 79,9 → 78,11 |
| uint8_t min, max; |
| } MMXLATION; |
| /* |
| typedef struct { |
| uint8_t sourceIdx, targetIdx; |
| } XLATION; |
| */ |
| typedef struct { |
| uint8_t channels[MAX_CHANNELS]; |
| 128,9 → 129,9 |
| uint8_t gyroDFilterConstant; |
| uint8_t accFilterConstant; |
| uint8_t maxAccVector; |
| uint8_t maxControlActivity; |
| uint8_t zerothOrderCorrection; |
| uint8_t zerothOrderCorrectionAccTolerance; |
| uint8_t zerothOrderCorrectionControlTolerance; |
| uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung) |
| uint8_t driftCompLimit; // limit for gyrodrift compensation |
| 269,8 → 270,13 |
| void paramSet_default(uint8_t setnumber); |
| void mixerMatrix_default(void); |
| void configuration_setNormalFlightParameters(void); |
| void configuration_setFailsafeFlightParameters(void); |
| void configuration_applyVariablesToParams(void); |
| uint8_t getCPUType(void); |
| uint8_t getBoardRelease(void); |
| // Called after a change in configuration parameters, as a hook for modules to take over changes. |
| void configuration_paramSetDidChange(void); |
| #endif // _CONFIGURATION_H |
| /branches/dongfang_FC_rewrite/controlMixer.c |
|---|
| 151,6 → 151,8 |
| debugOut.analog[16] = tempPRTY[CONTROL_PITCH]; |
| debugOut.analog[17] = tempPRTY[CONTROL_ROLL]; |
| debugOut.analog[18] = tempPRTY[CONTROL_THROTTLE]; |
| debugOut.analog[19] = tempPRTY[CONTROL_YAW]; |
| // Add external control to RC |
| EC_periodicTaskAndPRTY(tempPRTY); |
| 160,9 → 162,6 |
| navigation_periodicTaskAndPRTY(tempPRTY); |
| #endif |
| debugOut.analog[18] = tempPRTY[CONTROL_PITCH]; |
| debugOut.analog[19] = tempPRTY[CONTROL_ROLL]; |
| // Add compass control (could also have been before navi, they are independent) |
| CC_periodicTaskAndPRTY(tempPRTY); |
| 177,9 → 176,6 |
| AC_getPRTY(tempPRTY); |
| } |
| debugOut.analog[20] = tempPRTY[CONTROL_PITCH]; |
| debugOut.analog[21] = tempPRTY[CONTROL_ROLL]; |
| // Commit results to global variable and also measure control activity. |
| controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE]; |
| updateControlAndMeasureControlActivity(CONTROL_PITCH, tempPRTY[CONTROL_PITCH]); |
| /branches/dongfang_FC_rewrite/eeprom.c |
|---|
| 1,55 → 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. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #ifndef EEMEM |
| #define EEMEM __attribute__ ((section (".eeprom"))) |
| #endif |
| 141,8 → 89,9 |
| // setnumber [1..5] |
| uint8_t paramSet_readFromEEProm(uint8_t setnumber) { |
| uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*(sizeof(ParamSet_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD); |
| // output_init(); // what's that doing here?? |
| return readChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, offset, sizeof(ParamSet_t)); |
| uint8_t result = readChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, offset, sizeof(ParamSet_t)); |
| configuration_paramSetDidChange(); |
| return result; |
| } |
| /***************************************************/ |
| 152,8 → 101,6 |
| uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*(sizeof(ParamSet_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD); |
| writeChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, offset, sizeof(ParamSet_t)); |
| // set this parameter set to active set |
| setActiveParamSet(setnumber); |
| // output_init(); // what's that doing here?? |
| } |
| void paramSet_readOrDefault() { |
| 166,13 → 113,13 |
| paramSet_default(i); |
| paramSet_writeToEEProm(i); |
| } |
| // default-Setting is parameter set 3 |
| // default-Setting is parameter set 1 |
| setActiveParamSet(1); |
| paramSet_readFromEEProm(getActiveParamSet()); |
| // For some strange reason, the read will have no effect. |
| // Lets reset... |
| wdt_enable(WDTO_250MS); |
| // wdt_enable(WDTO_500MS); |
| } |
| printf("\n\r\rUsing Parameter Set %d", getActiveParamSet()); |
| } |
| 213,6 → 160,7 |
| printf("writing default channel map"); |
| channelMap_default(); |
| channelMap_writeToEEProm(); |
| wdt_enable(WDTO_500MS); |
| } |
| } |
| 249,8 → 197,8 |
| uint8_t getActiveParamSet(void) { |
| uint8_t setnumber; |
| setnumber = eeprom_read_byte(&EEPromArray[PID_ACTIVE_SET]); |
| if (setnumber > 4) { |
| setActiveParamSet(setnumber = 0); |
| if (setnumber > 5) { |
| setActiveParamSet(setnumber = 1); |
| } |
| return setnumber; |
| } |
| /branches/dongfang_FC_rewrite/failsafeControl.c |
|---|
| 7,21 → 7,14 |
| uint16_t emergencyFlightTime; |
| void setFailsafeFlightParameters(void) { |
| flight_setParameters(0, 90, 120, 90, 120); |
| void FC_setNeutral(void) { |
| configuration_setNormalFlightParameters(); |
| } |
| void setNormalFlightParameters(void) { |
| flight_setParameters(staticParams.IFactor, dynamicParams.gyroP, |
| staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI, |
| dynamicParams.gyroP, staticParams.yawIFactor); |
| } |
| void FC_periodicTaskAndPRTY(uint16_t* PRTY) { |
| void FC_setNeutral(void) { |
| setNormalFlightParameters(); |
| } |
| debugOut.analog[25] = emergencyFlightTime; |
| void FC_periodicTaskAndPRTY(uint16_t* PRTY) { |
| if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
| if (controlMixer_didReceiveSignal) |
| beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss. |
| 30,12 → 23,12 |
| if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) { |
| if (isFlying > 256) { |
| MKFlags |= MKFLAG_EMERGENCY_FLIGHT; // Set flag for emergency landing |
| setFailsafeFlightParameters(); |
| configuration_setFailsafeFlightParameters(); |
| // Set the time in whole seconds. |
| if (staticParams.emergencyFlightDuration > (65535 - F_MAINLOOP) / F_MAINLOOP) |
| if (staticParams.emergencyFlightDuration > (65535 - (uint16_t)F_MAINLOOP) / (uint16_t)F_MAINLOOP) |
| emergencyFlightTime = 0xffff; |
| else |
| emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * F_MAINLOOP; |
| emergencyFlightTime = (uint16_t)staticParams.emergencyFlightDuration * (uint16_t)F_MAINLOOP; |
| } |
| } else { |
| if (emergencyFlightTime) { |
| 53,7 → 46,7 |
| // Signal is OK. |
| if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
| MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing |
| setNormalFlightParameters(); |
| configuration_setNormalFlightParameters(); |
| } |
| } |
| /branches/dongfang_FC_rewrite/flight.c |
|---|
| 135,8 → 135,6 |
| else if (headingError > YAW_I_LIMIT) |
| headingError = YAW_I_LIMIT; |
| debugOut.analog[29] = headingError / 100; |
| PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4); |
| // Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on. |
| PDPart += (int32_t) (yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5); |
| /branches/dongfang_FC_rewrite/heightControl.c |
|---|
| 36,9 → 36,12 |
| if (height > maxHeightThisFlight) |
| maxHeightThisFlight = height; |
| // debugOut.analog[25] = dynamicParams.heightSetting; |
| if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) { |
| // If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle. |
| if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) { |
| // if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) { |
| if (dynamicParams.heightSetting >= 255/3) { |
| // Switch is ON |
| if (setHeightLatch <= LATCH_TIME) { |
| if (setHeightLatch == LATCH_TIME) { |
| 77,7 → 80,7 |
| } |
| // height, in meters (so the division factor is: 100) |
| debugOut.analog[24] = (117100 - filteredAirPressure) / 100; |
| // debugOut.analog[24] = (117100 - filteredAirPressure) / 100; |
| // Calculated 0 alt number: 108205 |
| // Experimental 0 alt number: 117100 |
| } |
| /branches/dongfang_FC_rewrite/makefile |
|---|
| 267,6 → 267,7 |
| #AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex |
| AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex $(FUSE_SETTINGS) |
| #AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep |
| AVRDUDE_READ_EEPROM = -U eeprom:r:eeprom.eep:i |
| # Make a dummy read in order to cycle the reset pin on the MCU. |
| AVRDUDE_RESET = -u -U lfuse:r:m |
| 405,6 → 406,10 |
| program: $(TARGET).hex $(TARGET).eep |
| $(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) |
| # Program the device. |
| readeep: |
| $(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_READ_EEPROM) |
| reset: |
| $(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_RESET) |
| /branches/dongfang_FC_rewrite/presentation.txt |
|---|
| 0,0 → 1,43 |
| Dongfang FC Rewrite |
| Goal: A neat, modular, analysable, reliable and easily extendable code base for Mikrokopter FC and later NC. |
| Experience and good design ideas from the original code are reused. |
| The project is personal. Only the parts that were of immediate use for myself were included. |
| The feature set is smaller than that of the original code, but the addition of features is much easier. It is more of a developer's firmware than a consumer's. |
| Features: |
| - Will run as compiled with any AVR-GCC version. |
| - Will run with any hardware version, and easily adapt to nonstandard hardware. Will run with or without ACC sensor, and with sensors in any orientation. |
| - Will run without a bootloader. |
| - The fc.c code monster has been killed and cut up. Global variables have been kept at a minimum. Abstractions are used where they should be. |
| - Controls have been abstracted and separated. RC, navigation, serial control, failsafe flight etc. are all in separate modules. |
| - NaviCtrl is not yet implemented (the strategy will probably be: First integrate to existing NaviCtrl code, then rewrite that as well). For simple requirements, navigation by directly connected GPS is possible. |
| Configuration and diags tool |
| Description of modules |
| ControlMixer: |
| The control mixer makes a composite control vector of four virtual RC sticks, based on the output of all features exercising control on the flight. The logic to mix the inputs is kept simple and free from side effects. |
| The component inputs include: RC, serial control, navigation, height control, compass-fixed heading control and failsafe emergency flight. |
| Commands: |
| Commands by RC stick gestures (calibrate, select parameterset, start and stop motors) have been abstracted into this module. Ultimately, serial control should also have commands capabilities. |
| Attitude: |
| All code related to determining the flight attitude of the MK (as opposed to determining control input and output) has been placed here. This relies heavily on data from the analog module. |
| Gyroscope calibration data is storen in EEPROM like accelerometer calibration data. There is thus no need to calibrate before each flight. |
| Flight: |
| This module calculates the motor output vector based on data from the attitude module, and composite stick input from the controlMixer module. This is the functionality fc.c minus all the things that were moved away. |
| Output: |
| The two LED outputs on the FC have a more advanced control and can be used as debugging signals etc. |
| Parameterset changes: |
| - The RC channel mapping is global and accessed through new serial commands. |
| - Gyro and accelerometer sensor attitudes added. These will also be made global at a later time. |
| - The parameter sets are incompatible with all MK-Tool versions and the dongfang tool must be used instead. The tool will currently only read and write to an XML paramerset format, but that is easy to overview, read, compare and write. |
| /branches/dongfang_FC_rewrite/uart0.c |
|---|
| 102,24 → 102,24 |
| "M2 ", |
| "M3 ", |
| "M4 ", |
| "pdpart ", |
| "control ", //15 |
| "controlActivity ", |
| "accVector ", //15 |
| "RC Stick P ", |
| "RC Stick R ", |
| "Stick P w GPS ", |
| "Stick R w GPS ", |
| "RC Stick T ", |
| "RC Stick Y ", |
| "Stick P w Emerg.", //20 |
| "Stick R w Emerg.", |
| "Height[dm] ", |
| "dHeight ", |
| "Altitude ", |
| " ", //25 |
| "Altitude ", |
| "EFT ", //25 |
| "naviPitch ", |
| "naviRoll ", |
| "i part contrib ", |
| "HeadingError ", |
| "Magnetic Heading", //30 |
| "GPS Datasets " |
| " ", |
| " ", //30 |
| " " |
| }; |
| /****************************************************************/ |
| 200,10 → 200,6 |
| versionInfo.protoMajor = VERSION_SERIAL_MAJOR; |
| versionInfo.protoMinor = VERSION_SERIAL_MINOR; |
| for (uint8_t i=0; i<32; i++) { |
| debugOut.analog[i] = i; |
| } |
| // restore global interrupt flags |
| SREG = sreg; |
| } |
| 290,8 → 286,8 |
| tmpchecksum += txd_buffer[i]; |
| } |
| tmpchecksum %= 4096; |
| txd_buffer[i++] = '=' + tmpchecksum >> 6; |
| txd_buffer[i++] = '=' + tmpchecksum & 0x3F; |
| txd_buffer[i++] = '=' + (tmpchecksum >> 6); |
| txd_buffer[i++] = '=' + (tmpchecksum & 0x3F); |
| txd_buffer[i++] = '\r'; |
| txd_complete = FALSE; |
| UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
| 514,7 → 510,11 |
| else if (pRxData[0] > 5) |
| pRxData[0] = 5; // limit to 5 |
| // load requested parameter set |
| debugOut.analog[30] = staticParams.GPSMininumSatellites; |
| paramSet_readFromEEProm(pRxData[0]); |
| tempchar[0] = pRxData[0]; |
| tempchar[1] = EEPARAM_REVISION; |
| tempchar[2] = sizeof(staticParams); |
| 530,6 → 530,8 |
| { |
| memcpy(&staticParams, (uint8_t*) &pRxData[3], sizeof(staticParams)); |
| paramSet_writeToEEProm(pRxData[0]); |
| setActiveParamSet(pRxData[0]); |
| configuration_paramSetDidChange(); |
| tempchar[0] = getActiveParamSet(); |
| beepNumber(tempchar[0]); |
| } else { |
| /branches/dongfang_FC_rewrite/ubx.c |
|---|
| 83,8 → 83,6 |
| // DebugOut.Digital ....blah... |
| if (GPSInfo.status != NEWDATA) { |
| GPSDatasetCounter++; |
| debugOut.analog[31] = GPSDatasetCounter; |
| GPSInfo.status = INVALID; |
| // NAV SOL |
| GPSInfo.flags = ubxSol.flags; |