| /branches/dongfang_FC_rewrite/analog.c |
|---|
| 500,18 → 500,26 |
| // If AIRPRESSURE_OVERSAMPLING is an odd number we only want to add half the double sample. |
| // The 2 cases above (end of range) are ignored for this. |
| debugOut.digital[1] &= ~DEBUG_SENSORLIMIT; |
| if (pressureMeasurementCount == AIRPRESSURE_OVERSAMPLING - 1) |
| airPressureSum += simpleAirPressure / 2; |
| else |
| airPressureSum += simpleAirPressure; |
| airPressureSum += simpleAirPressure; |
| } |
| // 2 samples were added. |
| pressureMeasurementCount += 2; |
| if (pressureMeasurementCount >= AIRPRESSURE_OVERSAMPLING) { |
| // Assumption here: AIRPRESSURE_OVERSAMPLING is even (well we all know it's 14 haha...) |
| if (pressureMeasurementCount == AIRPRESSURE_OVERSAMPLING) { |
| // The best oversampling count is 14.5. We add a quarter of the double ADC value to get the final half. |
| airPressureSum += simpleAirPressure >> 2; |
| lastFilteredAirPressure = filteredAirPressure; |
| filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER - 1) |
| + airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER; |
| if (!staticParams.airpressureWindowLength) { |
| filteredAirPressure = (filteredAirPressure * (staticParams.airpressureFilterConstant - 1) |
| + airPressureSum + staticParams.airpressureFilterConstant / 2) / staticParams.airpressureFilterConstant; |
| } else { |
| // use windowed. |
| filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength; |
| } |
| pressureMeasurementCount = airPressureSum = 0; |
| } |
| //int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH]; |
| 520,7 → 528,7 |
| windowedAirPressure += simpleAirPressure; |
| windowedAirPressure -= airPressureWindow[windowPtr]; |
| airPressureWindow[windowPtr] = simpleAirPressure; |
| windowPtr = (windowPtr+1) % MAX_AIRPRESSURE_WINDOW_LENGTH; |
| windowPtr = (windowPtr+1) % staticParams.airpressureWindowLength; |
| } |
| } |
| /branches/dongfang_FC_rewrite/analog.h |
|---|
| 110,7 → 110,7 |
| division by the constant takes place in the flight attitude code, the |
| constant is there. |
| The control loop executes every 2ms, and for each iteration |
| The control loop executes at 488Hz, and for each iteration |
| gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL]. |
| Assuming a constant rotation rate v and a zero initial gyroIntegral |
| (for this explanation), we get: |
| 119,18 → 119,18 |
| N * gyro / HIRES_GYRO_INTEGRATION_FACTOR = |
| N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR |
| where N is the number of summations; N = t/2ms. |
| where N is the number of summations; N = t*488. |
| For one degree of rotation: t*v = 1: |
| gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR. |
| gyroIntegralXXXX = t * 488 * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR. |
| This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor. |
| Examples: |
| FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304 |
| FC2.0: I=2, H=2.048, HIRES_GYRO_INTEGRATION_FACTOR=13 --> integralDegreeFactor = 1260 |
| My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365 |
| FC1.3: I=4, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=1 --> integralDegreeFactor = 2545 |
| FC2.0: I=4, H=1.2288, HIRES_GYRO_INTEGRATION_FACTOR=1 --> integralDegreeFactor = 2399 |
| My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1333 |
| */ |
| /* |
| 295,7 → 295,7 |
| void analog_calibrateAcc(void); |
| void analog_setGround(); |
| void analog_setGround(void); |
| int32_t analog_getHeight(void); |
| int16_t analog_getDHeight(void); |
| /branches/dongfang_FC_rewrite/attitude.c |
|---|
| 380,7 → 380,7 |
| CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
| // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
| debugOut.analog[28 + axis] = driftComp[axis]; |
| // debugOut.analog[28 + axis] = driftComp[axis]; |
| correctionSum[axis] = 0; |
| } |
| /branches/dongfang_FC_rewrite/attitude.h |
|---|
| 142,4 → 142,5 |
| * Main routine, called from the flight loop. |
| */ |
| void calculateFlightAttitude(void); |
| #endif //_ATTITUDE_H |
| /branches/dongfang_FC_rewrite/configuration.c |
|---|
| 1,28 → 1,28 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + Nur für den privaten Gebrauch |
| // + 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. |
| // + 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, |
| // + 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 |
| // + 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" |
| // + 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 |
| // + 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 |
| // + 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 |
| // + mit unserer Zustimmung zul�ssig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 211,6 → 211,8 |
| void setOtherDefaults(void) { |
| // Height Control |
| staticParams.airpressureFilterConstant = 8; |
| staticParams.airpressureWindowLength = 0; |
| staticParams.airpressureAccZCorrection = 128+56; |
| staticParams.heightP = 10; |
| staticParams.heightD = 30; |
| /branches/dongfang_FC_rewrite/configuration.h |
|---|
| 158,7 → 158,8 |
| uint8_t emergencyFlightDuration; |
| // Height Control |
| uint8_t airpressureFilter; |
| uint8_t airpressureFilterConstant; |
| uint8_t airpressureWindowLength; // 0 means: Use filter. |
| uint8_t airpressureAccZCorrection; |
| uint8_t heightP; |
| uint8_t heightI; |
| 192,7 → 193,7 |
| #define MKFLAG_CALIBRATE (1<<2) |
| #define MKFLAG_START (1<<3) |
| #define MKFLAG_EMERGENCY_FLIGHT (1<<4) |
| #define MKFLAG_RESERVE1 (1<<5) |
| #define MKFLAG_LOWBAT (1<<5) |
| #define MKFLAG_RESERVE2 (1<<6) |
| #define MKFLAG_RESERVE3 (1<<7) |
| /branches/dongfang_FC_rewrite/eeprom.h |
|---|
| 24,7 → 24,7 |
| #define EEPROM_ADR_PARAMSET_BEGIN 256 |
| #define CHANNELMAP_REVISION 0 |
| #define EEPARAM_REVISION 2 |
| #define EEPARAM_REVISION 3 |
| #define EEMIXER_REVISION 0 |
| #define SENSOROFFSET_REVISION 0 |
| /branches/dongfang_FC_rewrite/flight.c |
|---|
| 424,11 → 424,11 |
| // if (i < 4) debugOut.analog[22 + i] = throttle; |
| if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) { |
| motor[i].SetPoint = throttle; |
| motor[i].throttle = throttle; |
| } else if (motorTestActive) { |
| motor[i].SetPoint = motorTest[i]; |
| motor[i].throttle = motorTest[i]; |
| } else { |
| motor[i].SetPoint = 0; |
| motor[i].throttle = 0; |
| } |
| } |
| 442,7 → 442,5 |
| debugOut.analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| debugOut.analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| debugOut.analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
| debugOut.analog[16] = gyroPFactor; |
| } |
| } |
| /branches/dongfang_FC_rewrite/flight.h |
|---|
| 22,18 → 22,6 |
| * same, they are unseperable anyway. |
| */ |
| // looping params |
| // extern long TurnOver180Nick, TurnOver180Roll; |
| // external control |
| // extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw; |
| // extern int16_t naviAccPitch, naviAccRoll, naviCntAcc; |
| // TODO: Rip em up, replace by a flight-state machine. |
| // OK first step: Move to control mixer, where the state machine will be located anyway. |
| // extern volatile uint8_t MKFlags; |
| // extern uint16_t isFlying; |
| void flight_control(void); |
| void transmitMotorThrottleData(void); |
| void flight_setNeutral(void); |
| /branches/dongfang_FC_rewrite/heightControl.c |
|---|
| 77,7 → 77,6 |
| } |
| // height, in meters (so the division factor is: 100) |
| // debugOut.analog[30] = filteredAirPressure / 10; |
| debugOut.analog[30] = (117100 - filteredAirPressure) / 100; |
| // Calculated 0 alt number: 108205 |
| // Experimental 0 alt number: 117100 |
| 95,10 → 94,14 |
| if (heightError > 0) { |
| debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
| debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
| } else if (heightError < 0) { |
| } else { |
| debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
| } |
| if (dHeight > 0) { |
| debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
| } else { |
| debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
| } |
| // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
| 106,32 → 109,39 |
| #define IHEIGHT_SCALE 24 |
| // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256 |
| int16_t dThrottle = (iHeight * (int32_t)staticParams.heightI) >> (IHEIGHT_SCALE); |
| int16_t dThrottleI = (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE); |
| // dt = (iHeight * staticParams.heightI) >> (IHEIGHT_SCALE) = staticParams.heightControlMaxIntegral |
| // (iHeight * staticParams.heightI) = staticParams.heightControlMaxIntegral << (IHEIGHT_SCALE) |
| // iHeight = staticParams.heightControlMaxIntegral << (IHEIGHT_SCALE) /staticParams.heightI |
| if (dThrottle > staticParams.heightControlMaxIntegral) { |
| dThrottle = staticParams.heightControlMaxIntegral; |
| iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / staticParams.heightI; |
| } else if (dThrottle < -staticParams.heightControlMaxIntegral) { |
| dThrottle = -staticParams.heightControlMaxIntegral; |
| iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / staticParams.heightI; |
| if (dThrottleI > staticParams.heightControlMaxIntegral) { |
| dThrottleI = staticParams.heightControlMaxIntegral; |
| iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI; |
| } else if (dThrottleI < -staticParams.heightControlMaxIntegral) { |
| dThrottleI = -staticParams.heightControlMaxIntegral; |
| iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI; |
| } |
| debugOut.analog[27] = (iHeight * (int32_t)staticParams.heightI) >> (IHEIGHT_SCALE); |
| int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10; |
| int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7; |
| dThrottle += ((heightError * staticParams.heightP) >> 10) + ((dHeight * staticParams.heightD) >> 7); |
| debugOut.analog[24] = dThrottleP; |
| debugOut.analog[25] = dThrottleI; |
| debugOut.analog[26] = dThrottleD; |
| debugOut.analog[27] = dynamicParams.heightP; |
| debugOut.analog[28] = dynamicParams.heightI; |
| debugOut.analog[29] = dynamicParams.heightD; |
| int16_t dThrottle = dThrottleI + dThrottleP + dThrottleD; |
| if (dThrottle > staticParams.heightControlMaxThrottleChange) |
| dThrottle = staticParams.heightControlMaxThrottleChange; |
| else if (dThrottle < -staticParams.heightControlMaxThrottleChange) |
| dThrottle = -staticParams.heightControlMaxThrottleChange; |
| debugOut.analog[19] = rampedTargetHeight; |
| debugOut.analog[21] = dThrottle; |
| debugOut.analog[26] = height; |
| debugOut.analog[19] = throttle; |
| debugOut.analog[20] = dThrottle; |
| debugOut.analog[21] = height; |
| debugOut.analog[22] = rampedTargetHeight; |
| debugOut.analog[23] = heightError; |
| if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) { |
| if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) |
| 155,16 → 165,16 |
| stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER |
| - 1) + throttle) / HOVERTHROTTLEFILTER; |
| /* |
| if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 |
| && stronglyFilteredHeightDiff > -3) { |
| hoverThrottle = stronglyFilteredThrottle; |
| debugOut.digital[0] |= DEBUG_HOVERTHROTTLE; |
| // DebugOut.Analog[18] = hoverThrottle; |
| } else |
| debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE; |
| */ |
| //debugOut.analog[20] = dThrottle; |
| return throttle; |
| } |
| /branches/dongfang_FC_rewrite/menu.c |
|---|
| 1,28 → 1,28 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + Nur für den privaten Gebrauch |
| // + 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. |
| // + 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, |
| // + 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 |
| // + 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" |
| // + 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 |
| // + 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 |
| // + 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 |
| // + mit unserer Zustimmung zul�ssig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 234,21 → 234,21 |
| 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); |
| 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(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) |
| if (motor[10].present) |
| LCD_printfxy(8,3,"11"); |
| if (motor[11].Present) |
| if (motor[11].present) |
| LCD_printfxy(12,3,"12"); |
| break; |
| /branches/dongfang_FC_rewrite/naviControl.h |
|---|
| 6,90 → 6,3 |
| //void NC_calibrate(void); |
| //uint8_t NC_getSignalQuality (void); |
| //void NC_setNeutral(void); |
| // ######################## SPI - FlightCtrl ################### |
| #ifndef _NAVICONTROL_H |
| #define _NAVICONTROL_H |
| //#include <util/delay.h> |
| #include <inttypes.h> |
| #define SPI_PROTOCOL_COMP 1 |
| #define SPI_CMD_USER 10 |
| #define SPI_CMD_STICK 11 |
| #define SPI_CMD_MISC 12 |
| #define SPI_CMD_PARAMETER1 13 |
| #define SPI_CMD_VERSION 14 |
| typedef struct { |
| uint8_t Sync1; |
| uint8_t Sync2; |
| uint8_t Command; |
| int16_t IntegralPitch; |
| int16_t IntegralRoll; |
| int16_t AccPitch; |
| int16_t AccRoll; |
| int16_t GyroHeading; |
| int16_t GyroPitch; |
| int16_t GyroRoll; |
| int16_t GyroYaw; |
| union { |
| int8_t sByte[12]; |
| uint8_t Byte[12]; |
| int16_t Int[6]; |
| int32_t Long[3]; |
| float Float[3]; |
| } Param; |
| uint8_t Chksum; |
| } __attribute__((packed)) ToNaviCtrl_t; |
| #define SPI_CMD_OSD_DATA 100 |
| #define SPI_CMD_GPS_POS 101 |
| #define SPI_CMD_GPS_TARGET 102 |
| #define SPI_KALMAN 103 |
| typedef struct { |
| uint8_t Command; |
| int16_t GPSStickPitch; |
| int16_t GPSStickRoll; |
| int16_t GPS_Yaw; |
| int16_t CompassHeading; |
| int16_t Status; |
| uint16_t BeepTime; |
| union { |
| int8_t Byte[12]; |
| int16_t Int[6]; |
| int32_t Long[3]; |
| float Float[3]; |
| } Param; |
| uint8_t Chksum; |
| } __attribute__((packed)) FromNaviCtrl_t; |
| typedef struct { |
| uint8_t Major; |
| uint8_t Minor; |
| uint8_t Patch; |
| uint8_t Compatible; |
| // unsigned char Hardware; |
| } __attribute__((packed)) SPI_VersionInfo_t; |
| extern ToNaviCtrl_t toNaviCtrl; |
| extern FromNaviCtrl_t fromNaviCtrl; |
| typedef struct { |
| int8_t KalmanK; |
| int8_t KalmanMaxDrift; |
| int8_t KalmanMaxFusion; |
| uint8_t SerialDataOkay; |
| } __attribute__((packed)) NCData_t; |
| extern uint8_t NCDataOkay; |
| extern uint8_t NCSerialDataOkay; |
| void SPI_MasterInit(void); |
| void SPI_StartTransmitPacket(void); |
| void SPI_TransmitByte(void); |
| // new: |
| // extern void UpdateSPI_Buffer(void); |
| #endif //_NAVICONTROL_H |
| /branches/dongfang_FC_rewrite/spi.c |
|---|
| 1,28 → 1,28 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + Nur für den privaten Gebrauch |
| // + 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. |
| // + 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, |
| // + 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 |
| // + 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" |
| // + 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 |
| // + 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 |
| // + 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 |
| // + mit unserer Zustimmung zul�ssig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 63,13 → 63,10 |
| #include "flight.h" |
| //----------------------------------------- |
| #define DDR_SPI DDRB |
| #define DD_SS PB4 |
| #define DD_SCK PB7 |
| #define DD_MOSI PB5 |
| #define DD_MISO PB6 |
| // for compatibility reasons gcc3.x <-> gcc4.x |
| // dongfang: Lets try to abandon GCC3. |
| /* |
| #ifndef SPCR |
| #define SPCR SPCR0 |
| #endif |
| 114,7 → 111,13 |
| #ifndef SPI2X |
| #define SPI2X SPI2X0 |
| #endif |
| */ |
| // ------------------------- |
| #define DDR_SPI DDRB |
| #define DD_SS PB4 |
| #define DD_SCK PB7 |
| #define DD_MOSI PB5 |
| #define DD_MISO PB6 |
| #define SLAVE_SELECT_DDR_PORT DDRC |
| #define SLAVE_SELECT_PORT PORTC |
| 169,27 → 172,28 |
| SPI_TxBuffer = (uint8_t *) &toNaviCtrl; // set pointer to tx-buffer |
| SPITransferCompleted = 1; |
| // initialize data packet to NaviControl |
| toNaviCtrl.Sync1 = SPI_TXSYNCBYTE1; |
| toNaviCtrl.Sync2 = SPI_TXSYNCBYTE2; |
| toNaviCtrl.sync1 = SPI_TXSYNCBYTE1; |
| toNaviCtrl.sync2 = SPI_TXSYNCBYTE2; |
| toNaviCtrl.Command = SPI_CMD_USER; |
| toNaviCtrl.IntegralPitch = 0; |
| toNaviCtrl.IntegralRoll = 0; |
| NCSerialDataOkay = 0; |
| NCDataOkay = 0; |
| toNaviCtrl.command = SPI_CMD_USER; |
| toNaviCtrl.integralPitch = 0; |
| toNaviCtrl.integralRoll = 0; |
| naviCtrlData.serialDataOkay = 0; |
| //NCSerialDataOkay = 0; |
| //NCDataOkay = 0; |
| SPI_RxDataValid = 0; |
| SPI_VersionInfo.Major = VERSION_MAJOR; |
| SPI_VersionInfo.Minor = VERSION_MINOR; |
| SPI_VersionInfo.Patch = VERSION_PATCH; |
| SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE; |
| //SPI_VersionInfo.Major = VERSION_MAJOR; |
| //SPI_VersionInfo.Minor = VERSION_MINOR; |
| //SPI_VersionInfo.Patch = VERSION_PATCH; |
| //SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE; |
| } |
| /**********************************************************/ |
| /* Update Data transferd by the SPI from/to NaviCtrl */ |
| /* Update Data transfered by the SPI from/to NaviCtrl */ |
| /**********************************************************/ |
| void UpdateSPI_Buffer(void) { |
| void updateSPI_Buffer(void) { |
| uint8_t i; |
| int16_t tmp; |
| cli(); |
| 196,35 → 200,37 |
| // stop all interrupts to avoid writing of new data during update of that packet. |
| // update content of packet to NaviCtrl |
| toNaviCtrl.IntegralPitch = (int16_t) ((10 * angle[PITCH]) |
| / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| toNaviCtrl.IntegralRoll = (int16_t) ((10 * angle[ROLL]) |
| / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| toNaviCtrl.GyroHeading = (int16_t) ((10 * yawGyroHeading) |
| / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
| toNaviCtrl.GyroPitch = rate_ATT[PITCH]; |
| toNaviCtrl.GyroRoll = rate_ATT[ROLL]; |
| toNaviCtrl.GyroYaw = yawRate; |
| toNaviCtrl.AccPitch = (10 * getAngleEstimateFromAcc(PITCH)) |
| / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
| toNaviCtrl.AccRoll = (10 * getAngleEstimateFromAcc(ROLL)) |
| / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
| // Scaling? |
| toNaviCtrl.integralPitch = (angle[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1� |
| // Scaling? |
| toNaviCtrl.integralRoll = (angle[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1� |
| // Scaling? |
| toNaviCtrl.gyroHeading = (yawGyroHeading / (GYRO_DEG_FACTOR_YAW / 10)); // convert to multiple of 0.1� |
| // Scaling? |
| toNaviCtrl.gyroPitch = rate_ATT[PITCH]; |
| // Scaling? |
| toNaviCtrl.gyroRoll = rate_ATT[ROLL]; |
| // Scaling? |
| toNaviCtrl.gyroYaw = yawRate; |
| // Scaling? |
| toNaviCtrl.accPitch = getAngleEstimateFromAcc(PITCH) / (GYRO_DEG_FACTOR_PITCHROLL/10); // convert to multiple of 0.1� |
| // Scaling? |
| toNaviCtrl.AccRoll = getAngleEstimateFromAcc(ROLL) / (GYRO_DEG_FACTOR_PITCHROLL/10); |
| // TODO: What are these little bastards? |
| averageAcc[PITCH] = averageAcc[ROLL] = averageAccCount = 0; |
| switch (toNaviCtrl.Command) { |
| switch (toNaviCtrl.command) { |
| case SPI_CMD_USER: |
| for (i = 0; i < sizeof(dynamicParams.UserParams); i++) { |
| toNaviCtrl.Param.Byte[i] = dynamicParams.UserParams[i]; |
| for (i = 0; i < sizeof(dynamicParams.userParams); i++) { |
| toNaviCtrl.param.asByte[i] = dynamicParams.userParams[i]; |
| } |
| toNaviCtrl.Param.Byte[8] = MKFlags; |
| MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); // calibrate and start are temporal states that are cleared immediately after transmitting |
| toNaviCtrl.Param.Byte[9] = (uint8_t) UBat; |
| toNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning; |
| toNaviCtrl.Param.Byte[11] = getActiveParamSet(); |
| toNaviCtrl.Param.Byte[9] = getActiveParamSet(); |
| toNaviCtrl.Param.Byte[10] = 10; //EE_Parameter.ComingHomeAltitude; |
| toNaviCtrl.Param.Byte[11] = 0; //FC_StatusFlags2; |
| break; |
| case SPI_CMD_PARAMETER1: |
| /branches/dongfang_FC_rewrite/spi.h |
|---|
| 1,4 → 1,3 |
| // ######################## SPI - FlightCtrl ################### |
| #ifndef _SPI_H |
| #define _SPI_H |
| 11,69 → 10,108 |
| #define SPI_CMD_MISC 12 |
| #define SPI_CMD_PARAMETER1 13 |
| #define SPI_CMD_VERSION 14 |
| typedef struct { |
| uint8_t Sync1; |
| uint8_t Sync2; |
| uint8_t Command; |
| int16_t IntegralPitch; |
| int16_t IntegralRoll; |
| int16_t AccPitch; |
| int16_t AccRoll; |
| int16_t GyroHeading; |
| int16_t GyroPitch; |
| int16_t GyroRoll; |
| int16_t GyroYaw; |
| union { |
| int8_t sByte[12]; |
| uint8_t Byte[12]; |
| int16_t Int[6]; |
| int32_t Long[3]; |
| float Float[3]; |
| } Param; |
| uint8_t Chksum; |
| }__attribute__((packed)) ToNaviCtrl_t; |
| #define SPI_CMD_OSD_DATA 100 |
| #define SPI_CMD_GPS_POS 101 |
| #define SPI_CMD_GPS_TARGET 102 |
| #define SPI_KALMAN 103 |
| #define SPI_NCCMD_KALMAN 103 |
| #define SPI_NCCMD_VERSION 104 |
| #define SPI_NCCMD_GPSINFO 105 |
| #define SPI_NCCMD_HOTT_DATA 106 |
| #define SPI_MISC 107 |
| // Satfix types for GPSData.SatFix |
| #define SATFIX_NONE 0x00 |
| #define SATFIX_DEADRECKOING 0x01 |
| #define SATFIX_2D 0x02 |
| #define SATFIX_3D 0x03 |
| #define SATFIX_GPS_DEADRECKOING 0x04 |
| #define SATFIX_TIMEONLY 0x05 |
| // Flags for interpretation of the GPSData.Flags |
| #define FLAG_GPSFIXOK 0x01 // (i.e. within DOP & ACC Masks) |
| #define FLAG_DIFFSOLN 0x02 // (is DGPS used) |
| #define FLAG_WKNSET 0x04 // (is Week Number valid) |
| #define FLAG_TOWSET 0x08 // (is Time of Week valid) |
| #define FLAG_GPS_NAVIGATION_ACTIVE 0x10 // NC to FC -> NC is ready to navigate |
| typedef struct { |
| uint8_t Command; |
| uint8_t sync1; |
| uint8_t sync2; |
| uint8_t command; |
| int16_t integralPitch; |
| int16_t integralRoll; |
| int16_t accPitch; |
| int16_t accRoll; |
| int16_t gyroHeading; |
| int16_t gyroPitch; |
| int16_t gyroRoll; |
| int16_t gyroYaw; |
| uint8_t FCStatus; |
| union { |
| int8_t asSignedByte[12]; |
| uint8_t asByte[12]; |
| int16_t asInt[6]; |
| int32_t asLong[3]; |
| float asFloat[3]; |
| } param; |
| uint8_t chksum; |
| }__attribute__((packed)) ToNaviCtrl_t; |
| typedef struct { |
| uint8_t command; |
| int16_t GPSStickPitch; |
| int16_t GPSStickRoll; |
| int16_t GPS_Yaw; |
| int16_t CompassHeading; |
| int16_t Status; |
| uint16_t BeepTime; |
| int16_t GPSStickYaw; |
| int16_t accErrorPitch; |
| int16_t accErrorRoll; |
| int16_t magVectorZ; |
| int16_t compassHeading; |
| int16_t status; |
| uint16_t beepTime; |
| union { |
| int8_t Byte[12]; |
| int16_t Int[6]; |
| int32_t Long[3]; |
| float Float[3]; |
| } Param; |
| uint8_t Chksum; |
| int8_t asSignedByte[12]; |
| uint8_t asByte[12]; |
| int16_t asSignedInt[6]; |
| uint16_t asInt[6]; |
| int32_t asSignedLong[3]; |
| uint32_t asLong[3]; |
| float asFloat[3]; |
| } param; |
| uint8_t chksum; |
| }__attribute__((packed)) FromNaviCtrl_t; |
| typedef struct { |
| uint8_t Major; |
| uint8_t Minor; |
| uint8_t Patch; |
| uint8_t Compatible; |
| // unsigned char Hardware; |
| uint8_t major; |
| uint8_t minor; |
| uint8_t patch; |
| uint8_t compatible; |
| uint8_t hardware; |
| }__attribute__((packed)) SPI_VersionInfo_t; |
| // Some of this is defined for compatibility with H&I NaviCtrl rather than necessity of the data... |
| typedef struct { |
| int8_t kalmanK; |
| int8_t kalmanMaxDrift; |
| int8_t kalmanMaxFusion; |
| uint8_t serialDataOkay; |
| int8_t GPS_Z; |
| }__attribute__((packed)) NCData_t; |
| typedef struct { |
| unsigned char flags; // Status Flags |
| unsigned char numOfSats; // number of satelites |
| unsigned char satFix; // type of satfix |
| unsigned char speed; // m/sek |
| unsigned int homeDistance; // distance to Home in dm |
| int homeBearing; // bearing to home in deg |
| }__attribute__((packed)) GPSInfo_t; |
| extern SPI_VersionInfo_t naviCtrlVersion; |
| extern ToNaviCtrl_t toNaviCtrl; |
| extern FromNaviCtrl_t fromNaviCtrl; |
| extern NCData_t naviCtrlData; |
| extern GPSInfo_t GPSInfo; |
| typedef struct { |
| int8_t KalmanK; |
| int8_t KalmanMaxDrift; |
| int8_t KalmanMaxFusion; |
| uint8_t SerialDataOkay; |
| }__attribute__((packed)) NCData_t; |
| extern uint8_t NCDataOkay; |
| extern uint8_t NCSerialDataOkay; |
| 80,7 → 118,5 |
| void SPI_MasterInit(void); |
| void SPI_StartTransmitPacket(void); |
| void SPI_TransmitByte(void); |
| // new: |
| // extern void UpdateSPI_Buffer(void); |
| #endif //_SPI_H |
| /branches/dongfang_FC_rewrite/twimaster.c |
|---|
| 1,28 → 1,28 |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 04.2007 Holger Buss |
| // + Nur für den privaten Gebrauch |
| // + 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. |
| // + 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, |
| // + 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 |
| // + 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" |
| // + 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 |
| // + 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 |
| // + 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 |
| // + mit unserer Zustimmung zul�ssig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 99,10 → 99,9 |
| motor_read = 0; |
| for (i = 0; i < MAX_MOTORS; i++) { |
| motor[i].SetPoint = 0; |
| motor[i].Present = 0; |
| motor[i].Error = 0; |
| motor[i].MaxPWM = 0; |
| motor[i].throttle = 0; |
| motor[i].present = 0; |
| motor[i].maxPWM = 0; |
| } |
| SREG = sreg; |
| 206,14 → 205,14 |
| I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode |
| break; |
| case 1: // Send Data to Slave |
| I2C_WriteByte(motor[motor_write].SetPoint); // transmit rotation rate setpoint |
| I2C_WriteByte(motor[motor_write].throttle); // transmit throttle value. |
| break; |
| case 2: // repeat case 0+1 for all motors |
| if (TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received |
| if (!missing_motor) |
| missing_motor = motor_write + 1; |
| if (++motor[motor_write].Error == 0) |
| motor[motor_write].Error = 255; // increment error counter and handle overflow |
| if (++motor[motor_write].error == 0) |
| motor[motor_write].error = 255; // increment error counter and handle overflow |
| } |
| I2C_Stop(TWI_STATE_MOTOR_TX); |
| I2CTimeout = 10; |
| 224,13 → 223,13 |
| case 3: |
| if (TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received |
| // no response from the addressed slave received |
| motor[motor_read].Present = 0; |
| motor[motor_read].present = 0; |
| motor_read++; // next motor |
| if (motor_read >= MAX_MOTORS) |
| motor_read = 0; // restart reading of first motor if we have reached the last one |
| I2C_Stop(TWI_STATE_MOTOR_TX); |
| } else { |
| motor[motor_read].Present = ('1' - '-') + motor_read; |
| motor[motor_read].present = ('1' - '-') + motor_read; |
| I2C_ReceiveByte(); //Transmit 1st byte |
| } |
| missingMotor = missing_motor; |
| 237,12 → 236,12 |
| missing_motor = 0; |
| break; |
| case 4: //Read 1st byte and transmit 2nd Byte |
| motor[motor_read].Current = TWDR; |
| motor[motor_read].current = TWDR; |
| I2C_ReceiveLastByte(); // nack |
| break; |
| case 5: |
| //Read 2nd byte |
| motor[motor_read].MaxPWM = TWDR; |
| motor[motor_read].maxPWM = TWDR; |
| motor_read++; // next motor |
| if (motor_read >= MAX_MOTORS) |
| motor_read = 0; // restart reading of first motor if we have reached the last one |
| 293,7 → 292,7 |
| printf("\n\rFound BL-Ctrl: "); |
| for (i = 0; i < MAX_MOTORS; i++) { |
| motor[i].SetPoint = 0; |
| motor[i].throttle = 0; |
| } |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| 304,13 → 303,13 |
| for (i = 0; i < MAX_MOTORS; i++) { |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| _delay_ms(2); |
| if (motor[i].Present) |
| if (motor[i].present) |
| printf("%d ",i+1); |
| } |
| for (i = 0; i < MAX_MOTORS; i++) { |
| if (!motor[i].Present && mixerMatrix.motor[i][MIX_THROTTLE] > 0) |
| if (!motor[i].present && mixerMatrix.motor[i][MIX_THROTTLE] > 0) |
| printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
| motor[i].Error = 0; |
| motor[i].error = 0; |
| } |
| } |
| /branches/dongfang_FC_rewrite/twimaster.h |
|---|
| 12,11 → 12,11 |
| extern uint8_t missingMotor; |
| typedef struct { |
| uint8_t SetPoint; // written by attitude controller |
| uint8_t Present; // 0 if BL was found |
| uint8_t Error; // I2C error counter |
| uint8_t Current; // read byck from BL |
| uint8_t MaxPWM; // read back from BL |
| uint8_t throttle; // written by attitude controller |
| uint8_t present; // 0 if BL was found |
| uint8_t error; // I2C error counter |
| uint8_t current; // read back from BL |
| uint8_t maxPWM; // read back from BL |
| }__attribute__((packed)) motorData_t; |
| extern motorData_t motor[MAX_MOTORS]; |
| /branches/dongfang_FC_rewrite/uart0.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. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include <avr/wdt.h> |
| 137,20 → 85,20 |
| "Roll Term ", |
| "Yaw Term ", |
| "Throttle Term ", //15 |
| "gyroP ", |
| "- ", |
| "ControlAct/10 ", |
| "Acc. Vector ", |
| "targetheight ", |
| "var0 ", //20 |
| "dHeightThrottle ", |
| "M1 ", |
| "M2 ", |
| "M3 ", |
| "M4 ", //25 |
| "heightTrottleIn ", |
| "HeightdThrottle ", //20 |
| "Height ", |
| "HeightI", |
| "DriftCompPitch ", |
| "DriftCompRoll ", |
| "TargetHeight ", |
| "heightError ", |
| "HeightPdThrottle", |
| "HeightIdThrottle", //25 |
| "HeightDdThrottle", |
| "HeightPFactor ", |
| "HeightIFactor ", |
| "HeightDFactor ", |
| "Altitude ", //30 |
| "AirpressADC " }; |
| 559,8 → 507,7 |
| case 's': // save settings |
| if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off |
| { |
| if ((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] |
| == EEPARAM_REVISION)) // check for setting to be in range and version of settings |
| if ((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EEPARAM_REVISION)) // check for setting to be in range and version of settings |
| { |
| memcpy(&staticParams, (uint8_t*) &pRxData[2], sizeof(staticParams)); |
| paramSet_writeToEEProm(pRxData[0]); |
| 643,7 → 590,7 |
| } |
| /************************************************************************/ |
| /* Routine für die Serielle Ausgabe */ |
| /* Routine f�r die Serielle Ausgabe */ |
| /************************************************************************/ |
| int16_t uart_putchar(int8_t c) { |
| if (c == '\n') |
| 682,7 → 629,7 |
| request_display1 = FALSE; |
| } |
| if (request_debugLabel != 0xFF) { // Texte für die Analogdaten |
| if (request_debugLabel != 0xFF) { // Texte f�r die Analogdaten |
| uint8_t label[16]; // local sram buffer |
| memcpy_P(label, ANALOG_LABEL[request_debugLabel], 16); // read lable from flash to sram buffer |
| sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_debugLabel, |
| 690,7 → 637,7 |
| request_debugLabel = 0xFF; |
| } |
| if (confirmFrame && txd_complete) { // Datensatz ohne checksum bestätigen |
| if (confirmFrame && txd_complete) { // Datensatz ohne checksum best�tigen |
| sendOutData('B', FC_ADDRESS, 1, (uint8_t*) &confirmFrame, sizeof(confirmFrame)); |
| confirmFrame = 0; |
| } |