/branches/dongfang_FC_rewrite/analog.c |
---|
6,6 → 6,7 |
#include "attitude.h" |
#include "sensors.h" |
#include "printf_P.h" |
#include "mk3mag.h" |
// for Delay functions |
#include "timer0.h" |
45,6 → 46,7 |
int16_t gyro_ATT[2]; |
int16_t gyroD[2]; |
int16_t yawGyro; |
int16_t magneticHeading; |
int32_t groundPressure; |
490,6 → 492,7 |
analog_updateAccelerometers(); |
analog_updateAirPressure(); |
analog_updateBatteryVoltage(); |
debugOut.analog[12] = magneticHeading = volatileMagneticHeading; |
} |
void analog_setNeutral() { |
/branches/dongfang_FC_rewrite/analog.h |
---|
192,11 → 192,14 |
*/ |
extern int32_t filteredAirPressure; |
extern int16_t magneticHeading; |
/* |
* Flag: Interrupt handler has done all A/D conversion and processing. |
*/ |
extern volatile uint8_t analogDataReady; |
void analog_init(void); |
/* |
/branches/dongfang_FC_rewrite/attitude.c |
---|
53,20 → 53,13 |
//int readingHeight = 0; |
// Yaw angle and compass stuff. |
int32_t headingError; |
// This is updated/written from MM3. Negative angle indicates invalid data. |
int16_t magneticHeading = -1; |
// This is NOT updated from MM3. Negative angle indicates invalid data. |
// 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; |
uint16_t ignoreCompassTimer = 500; |
uint16_t ignoreCompassTimer = 0;// 500; |
int32_t heading; // Yaw Gyro Integral supported by compass |
int16_t yawGyroDrift; |
203,6 → 196,11 |
*/ |
heading += ACYawRate; |
intervalWrap(&heading, YAWOVER360); |
headingError += ACYawRate; |
debugOut.analog[27] = heading / 100; |
/* |
* Pitch axis integration and range boundary wrap. |
*/ |
346,7 → 344,8 |
return; |
heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
targetHeading = heading; |
//targetHeading = heading; |
headingError = 0; |
debugOut.digital[0] ^= DEBUG_COMPASS; |
} |
354,10 → 353,10 |
void correctHeadingToMagnetic(void) { |
int32_t error; |
debugOut.analog[27] = heading; |
if (commands_isCalibratingCompass()) |
if (commands_isCalibratingCompass()) { |
debugOut.analog[29] = 1; |
return; |
} |
// Compass is off, skip. |
// Naaah this is assumed. |
365,21 → 364,28 |
// return; |
// Compass is invalid, skip. |
if (magneticHeading < 0) |
if (magneticHeading < 0) { |
debugOut.analog[29] = 2; |
return; |
} |
// Spinning fast, skip |
if (abs(yawRate) > 128) |
if (abs(yawRate) > 128) { |
debugOut.analog[29] = 3; |
return; |
} |
// Otherwise invalidated, skip |
if (ignoreCompassTimer) { |
ignoreCompassTimer--; |
debugOut.analog[29] = 4; |
return; |
} |
// TODO: Find computational cost of this. |
error = (magneticHeading*GYRO_DEG_FACTOR_YAW - heading) % GYRO_DEG_FACTOR_YAW; |
error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading); |
if (error <= -YAWOVER180) error += YAWOVER360; |
else if (error > YAWOVER180) error -= YAWOVER360; |
// 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. |
386,7 → 392,8 |
// 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; |
int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
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 |
395,10 → 402,11 |
heading += correction; |
intervalWrap(&heading, YAWOVER360); |
targetHeading += correction; |
intervalWrap(&targetHeading, YAWOVER360); |
debugOut.digital[1] ^= DEBUG_COMPASS; |
// If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all |
// when the compass corrects the heading - it only corrects numbers!) we want to add: |
// This will however cause drift to remain uncorrected! |
// headingError += correction; |
debugOut.analog[29] = 0; |
} |
/************************************************************************ |
/branches/dongfang_FC_rewrite/attitude.h |
---|
117,13 → 117,11 |
/* |
* Compass navigation |
*/ |
extern int16_t magneticHeading; |
//extern int16_t headingInDegrees; |
extern int32_t heading; |
extern uint16_t ignoreCompassTimer; |
extern uint16_t accVector; |
extern int32_t targetHeading; |
extern int32_t headingError; |
/* |
/branches/dongfang_FC_rewrite/configuration.c |
---|
87,7 → 87,7 |
SET_POT_MM(dynamicParams.gyroP, staticParams.gyroP, 5, 200); |
SET_POT(dynamicParams.gyroI, staticParams.gyroI); |
SET_POT(dynamicParams.gyroD, staticParams.gyroD); |
SET_POT(dynamicParams.compassYawEffect,staticParams.compassYawEffect); |
SET_POT(dynamicParams.compassFixedHeading,staticParams.compassFixedHeading); |
SET_POT(dynamicParams.externalControl, staticParams.externalControl); |
SET_POT(dynamicParams.dynamicStability,staticParams.dynamicStability); |
246,14 → 246,12 |
// staticParams.driftCompDivider = |
// staticParams.driftCompLimit = |
staticParams.axisCoupling1 = 90; |
staticParams.axisCoupling2 = 67; |
staticParams.axisCouplingYawCorrection = 0; |
staticParams.dynamicStability = 50; |
staticParams.maxAccVector = 10; |
staticParams.IFactor = 32; |
staticParams.yawIFactor = 100; |
staticParams.compassYawEffect = 128; |
staticParams.compassYawCorrection = 64; |
staticParams.compassFixedHeading = 0; |
staticParams.levelCorrection[0] = staticParams.levelCorrection[1] = 128; |
// Servos |
/branches/dongfang_FC_rewrite/configuration.h |
---|
42,7 → 42,7 |
/*PMM*/uint8_t gyroP; |
/* P */uint8_t gyroI; |
/* P */uint8_t gyroD; |
/* P */uint8_t compassYawEffect; |
/* P */uint8_t compassFixedHeading; |
// Control |
/* P */uint8_t externalControl; |
134,9 → 134,9 |
uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung) |
uint8_t driftCompLimit; // limit for gyrodrift compensation |
uint8_t axisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
uint8_t axisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
uint8_t axisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
// uint8_t axisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
// uint8_t axisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
// uint8_t axisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
uint8_t levelCorrection[2]; |
146,18 → 146,25 |
uint8_t gyroD; |
uint8_t attitudeControl; |
uint8_t stickP; |
uint8_t stickD; |
uint8_t stickYawP; |
uint8_t stickThrottleD; |
uint8_t minThrottle; |
uint8_t maxThrottle; |
uint8_t externalControl; // for serial Control |
uint8_t motorSmoothing; |
uint8_t dynamicStability; // PID limit for Attitude controller |
uint8_t IFactor; |
uint8_t yawIFactor; |
uint8_t compassYawEffect; |
uint8_t compassYawCorrection; |
uint8_t compassFixedHeading; |
uint8_t batteryVoltageWarning; |
uint8_t emergencyThrottle; |
uint8_t emergencyFlightDuration; |
217,7 → 224,7 |
#define CFG_SIMPLE_HC_HOLD_SWITCH (1<<1) |
#define CFG_HEADING_HOLD (1<<2) |
#define CFG_COMPASS_ACTIVE (1<<3) |
#define CFG_COMPASS_FIX (1<<4) |
#define CFG_UNUSED (1<<4) |
#define CFG_GPS_ACTIVE (1<<5) |
#define CFG_AXIS_COUPLING_ACTIVE (1<<6) |
#define CFG_GYRO_SATURATION_PREVENTION (1<<7) |
/branches/dongfang_FC_rewrite/controlMixer.c |
---|
170,6 → 170,7 |
AC_getPRTY(tempPRTY); |
// Commit results to global variable and also measure control activity. |
controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE]; |
updateControlAndMeasureControlActivity(CONTROL_PITCH, tempPRTY[CONTROL_PITCH]); |
updateControlAndMeasureControlActivity(CONTROL_ROLL, tempPRTY[CONTROL_ROLL]); |
updateControlAndMeasureControlActivity(CONTROL_YAW, tempPRTY[CONTROL_YAW]); |
/branches/dongfang_FC_rewrite/directGPSNaviControl.c |
---|
63,7 → 63,7 |
flightModeOld = flightMode; |
} |
debugOut.analog[31] = flightMode; |
//debugOut.analog[31] = flightMode; |
} |
// --------------------------------------------------------------------------------- |
/branches/dongfang_FC_rewrite/flight.c |
---|
214,7 → 214,7 |
// HC_setGround(); |
attitude_resetHeadingToMagnetic(); |
// Set target heading to the one just gotten off compass. |
targetHeading = heading; |
// targetHeading = heading; |
} |
} else { |
// Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
239,10 → 239,6 |
} |
*/ |
targetHeading -= controls[CONTROL_YAW]; |
debugOut.analog[28] = targetHeading; |
#if defined (USE_NAVICTRL) |
/************************************************************************/ |
/* GPS is currently not supported. */ |
255,7 → 251,6 |
#endif |
// end part 1: 750-800 usec. |
// start part 3: 350 - 400 usec. |
#define SENSOR_LIMIT (4096 * 4) |
/************************************************************************/ |
/* Calculate control feedback from angle (gyro integral) */ |
266,18 → 261,18 |
PDPart[axis] = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); // P-Part - Proportional to Integral |
PDPart[axis] += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 5); |
PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
CHECK_MIN_MAX(PDPart[axis], -6L*GYRO_DEG_FACTOR_PITCHROLL, 6L*GYRO_DEG_FACTOR_PITCHROLL); |
} |
int32_t headingDiff = heading - targetHeading; // apparently yaw is reverse on output. |
if (headingDiff > YAWOVER180) headingDiff -= YAWOVER360; |
else if (headingDiff <= -YAWOVER180) headingDiff += YAWOVER360; |
#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW) |
// This is where control affects the target heading. It also (later) directly controls yaw. |
headingError -= controls[CONTROL_YAW]; |
debugOut.analog[28] = headingError / 100; |
if (headingError < -YAW_I_LIMIT) headingError = -YAW_I_LIMIT; |
if (headingError > YAW_I_LIMIT) headingError = YAW_I_LIMIT; |
// 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) / (GYRO_DEG_FACTOR_PITCHROLL << 3); |
PDPartYaw = (int32_t)(headingError * yawIFactor) / (GYRO_DEG_FACTOR_PITCHROLL << 3); |
// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on. |
PDPartYaw += (int32_t)(yawRate * yawPFactor) / (GYRO_DEG_FACTOR_PITCHROLL >> 6); |
// limit control feedback |
360,7 → 355,7 |
// Add (P, D) parts minus stick pos. to the scaled-down I part. |
term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch |
term[axis] += (dynamicParams.levelCorrection[axis] - 128); |
/* |
/* |
* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |
* The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity |
* (max. pitch or roll term is the throttle value). |
/branches/dongfang_FC_rewrite/mk3mag.c |
---|
1,65 → 1,13 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 <avr/io.h> |
#include <stdlib.h> |
#include <inttypes.h> |
#include "timer0.h" |
#include "attitude.h" |
#include "eeprom.h" |
#include "mk3mag.h" |
// For the DebougOut alone. |
#include "output.h" |
uint8_t PWMTimeout = 12; |
// This will updated in interrupt handler. Should not be processed by main, other than just atomic copy. |
volatile uint16_t volatileMagneticHeading; |
ToMk3Mag_t toMk3Mag; |
/*********************************************/ |
101,7 → 49,7 |
if (PWMCount > 400) { |
if (PWMTimeout) |
PWMTimeout--; // decrement timeout |
magneticHeading = -1; // unknown heading |
volatileMagneticHeading = -1; // unknown heading |
PWMCount = 0; // reset PWM Counter |
} |
} else { // pwm is low |
108,9 → 56,9 |
// ignore pwm values values of 0 and higher than 37 ms; |
if ((PWMCount) && (PWMCount < 362)) { // 362 * 102.4us = 37.0688 ms |
if (PWMCount < 10) |
magneticHeading = 0; |
volatileMagneticHeading = 0; |
else { |
magneticHeading = ((uint32_t) (PWMCount - 10) * 1049L) / 1024; // correct timebase and offset |
volatileMagneticHeading = ((uint32_t) (PWMCount - 10) * 1049L) / 1024; // correct timebase and offset |
//DebugOut.Digital[1] ^= DEBUG_MK3MAG; // correct signal recd. |
} |
/* |
134,7 → 82,5 |
beepDelay = setDelay(100); |
} |
} |
debugOut.analog[12] = magneticHeading; |
} |
/branches/dongfang_FC_rewrite/mk3mag.h |
---|
9,6 → 9,7 |
} ToMk3Mag_t; |
extern ToMk3Mag_t toMk3Mag; |
extern volatile uint16_t volatileMagneticHeading; |
// Initialization |
void MK3MAG_init(void); |
/branches/dongfang_FC_rewrite/rc.c |
---|
119,14 → 119,14 |
else |
RCQuality = 200; |
} |
// If signal is the same as before +/- 1, just keep it there. |
if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) { |
// If signal is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff. |
// if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) { |
// In addition, if the signal is very close to 0, just set it to 0. |
if (signal >= -1 && signal <= 1) { |
tmp = 0; |
} else { |
tmp = PPM_in[index]; |
} |
if (signal >= -1 && signal <= 1) { |
tmp = 0; |
//} else { |
// tmp = PPM_in[index]; |
// } |
} else |
tmp = signal; |
// calculate signal difference on good signal level |
/branches/dongfang_FC_rewrite/uart0.c |
---|
98,10 → 98,10 |
"HeightIdThrottle", //25 |
"HeightDdThrottle", |
"HeadingInDegrees", |
"TargetHeadingDeg", |
"Diff ", |
" ", //30 |
"navi mode " |
"0 ", |
"1 ", |
"2 ", //30 |
"3 " |
}; |
/****************************************************************/ |
510,9 → 510,9 |
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) && (pRxData[2] == sizeof(staticParams))) // check for setting to be in range and version of settings |
{ |
memcpy(&staticParams, (uint8_t*) &pRxData[2], sizeof(staticParams)); |
memcpy(&staticParams, (uint8_t*) &pRxData[3], sizeof(staticParams)); |
paramSet_writeToEEProm(pRxData[0]); |
tempchar[0] = getActiveParamSet(); |
beepNumber(tempchar[0]); |
/branches/dongfang_FC_rewrite/uart0.h |
---|
1,10 → 1,8 |
#ifndef _UART0_H |
#define _UART0_H |
#define RXD_BUFFER_LEN 150 |
// must be at least 4('#'+Addr+'CmdID'+'\r')+ (80 * 4)/3 = 111 bytes |
#define TXD_BUFFER_LEN 150 |
#define RXD_BUFFER_LEN 150 |
#define TXD_BUFFER_LEN 180 |
#define RXD_BUFFER_LEN 180 |
#include <inttypes.h> |
/branches/dongfang_FC_rewrite/ubx.c |
---|
96,7 → 96,7 |
GPSInfo.veleast = ubxVelNed.VEL_E; |
GPSInfo.velnorth = ubxVelNed.VEL_N; |
GPSInfo.veltop = -ubxVelNed.VEL_D; |
GPSInfo.veltop = -ubxVelNed.VEL_D; |
GPSInfo.velground = ubxVelNed.gSpeed; |
GPSInfo.status = NEWDATA; |