1,63 → 1,9 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + Nur f�r den privaten Gebrauch |
// + www.MikroKopter.com |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zul�ssig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt und genannt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zul�ssig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
/************************************************************************/ |
/* Flight Attitude */ |
/************************************************************************/ |
|
#include <stdlib.h> |
#include <avr/io.h> |
|
#include "attitude.h" |
#include "dongfangMath.h" |
#include "commands.h" |
|
// For scope debugging only! |
#include "rc.h" |
102,9 → 48,9 |
* Gyro integrals. These are the rotation angles of the airframe compared to the |
* horizontal plane, yaw relative to yaw at start. |
*/ |
int32_t angle[2], yawAngleDiff; |
int32_t attitude[2]; |
|
int readingHeight = 0; |
//int readingHeight = 0; |
|
// Yaw angle and compass stuff. |
|
112,23 → 58,19 |
int16_t magneticHeading = -1; |
|
// This is NOT updated from MM3. Negative angle indicates invalid data. |
int16_t compassCourse = -1; |
// int16_t headingInDegrees = -1; |
|
int32_t targetHeading; |
|
// The difference between the above 2 (heading - course) on a -180..179 degree interval. |
// Not necessary. Never read anywhere. |
// int16_t compassOffCourse = 0; |
|
uint8_t updateCompassCourse = 0; |
uint8_t compassCalState = 0; |
uint16_t ignoreCompassTimer = 500; |
|
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
int32_t heading; // Yaw Gyro Integral supported by compass |
int16_t yawGyroDrift; |
|
#define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L) |
#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
#define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L) |
|
int16_t correctionSum[2] = { 0, 0 }; |
|
// For NaviCTRL use. |
156,16 → 98,16 |
|
int32_t getAngleEstimateFromAcc(uint8_t axis) { |
//int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
return (int32_t)GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis];// + correctionTerm; |
return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; // + correctionTerm; |
// return 342L * filteredAcc[axis]; |
} |
|
void setStaticAttitudeAngles(void) { |
#ifdef ATTITUDE_USE_ACC_SENSORS |
angle[PITCH] = getAngleEstimateFromAcc(PITCH); |
angle[ROLL] = getAngleEstimateFromAcc(ROLL); |
attitude[PITCH] = getAngleEstimateFromAcc(PITCH); |
attitude[ROLL] = getAngleEstimateFromAcc(ROLL); |
#else |
angle[PITCH] = angle[ROLL] = 0; |
attitude[PITCH] = attitude[ROLL] = 0; |
#endif |
} |
|
184,14 → 126,7 |
|
// reset gyro integrals to acc guessing |
setStaticAttitudeAngles(); |
yawAngleDiff = 0; |
|
// update compass course to current heading |
compassCourse = magneticHeading; |
|
// Inititialize YawGyroIntegral value with current compass heading |
yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
|
attitude_resetHeadingToMagnetic(); |
// Servo_On(); //enable servo output |
} |
|
224,22 → 159,28 |
* changed accordingly. |
*/ |
void trigAxisCoupling(void) { |
int16_t rollAngleInDegrees = angle[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
int16_t pitchAngleInDegrees = angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
int16_t rollAngleInDegrees = attitude[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
int16_t pitchAngleInDegrees = attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
|
int16_t cospitch = cos_360(pitchAngleInDegrees); |
int16_t cosroll = cos_360(rollAngleInDegrees); |
int16_t sinroll = sin_360(rollAngleInDegrees); |
|
ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate |
* sinroll) >> LOG_MATH_UNIT_FACTOR); |
ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll |
- (int32_t) yawRate * sinroll) >> LOG_MATH_UNIT_FACTOR); |
|
ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
+ (int32_t)yawRate * cosroll) >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees)) >> LOG_MATH_UNIT_FACTOR); |
ACRate[ROLL] = rate_ATT[ROLL] |
+ (((((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
>> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees)) |
>> LOG_MATH_UNIT_FACTOR); |
|
ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
ACYawRate = |
((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
/ cospitch; |
|
ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
ACYawRate = |
((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
/ cospitch; |
} |
|
// 480 usec with axis coupling - almost no time without. |
258,26 → 199,19 |
/* |
* Yaw |
* Calculate yaw gyro integral (~ to rotation angle) |
* Limit yawGyroHeading proportional to 0 deg to 360 deg |
* Limit heading proportional to 0 deg to 360 deg |
*/ |
yawGyroHeading += ACYawRate; |
yawAngleDiff += yawRate; |
|
if (yawGyroHeading >= YAWOVER360) { |
yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
} else if (yawGyroHeading < 0) { |
yawGyroHeading += YAWOVER360; |
} |
|
heading += ACYawRate; |
intervalWrap(&heading, YAWOVER360); |
/* |
* Pitch axis integration and range boundary wrap. |
*/ |
for (axis = PITCH; axis <= ROLL; axis++) { |
angle[axis] += ACRate[axis]; |
if (angle[axis] > PITCHROLLOVER180) { |
angle[axis] -= PITCHROLLOVER360; |
} else if (angle[axis] <= -PITCHROLLOVER180) { |
angle[axis] += PITCHROLLOVER360; |
attitude[axis] += ACRate[axis]; |
if (attitude[axis] > PITCHROLLOVER180) { |
attitude[axis] -= PITCHROLLOVER360; |
} else if (attitude[axis] <= -PITCHROLLOVER180) { |
attitude[axis] += PITCHROLLOVER360; |
} |
} |
} |
299,35 → 233,35 |
uint8_t ca = controlActivity >> 8; |
uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
|
if (highControlActivity) { |
debugOut.digital[1] |= DEBUG_ACC0THORDER; |
} else { |
debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
} |
if (highControlActivity) { |
debugOut.digital[1] |= DEBUG_ACC0THORDER; |
} else { |
debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
} |
|
if (accVector <= dynamicParams.maxAccVector) { |
debugOut.digital[0] &= ~ DEBUG_ACC0THORDER; |
|
debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
|
uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
int32_t accDerived; |
|
/* |
if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
permilleAcc /= 2; |
debugFullWeight = 0; |
} |
if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
permilleAcc /= 2; |
debugFullWeight = 0; |
} |
|
if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
permilleAcc /= 2; |
debugFullWeight = 0; |
*/ |
if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
permilleAcc /= 2; |
debugFullWeight = 0; |
*/ |
|
if (highControlActivity) { // reduce effect during stick control activity |
permilleAcc /= 4; |
if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity |
if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity |
permilleAcc /= 4; |
} |
} |
} |
|
/* |
* Add to each sum: The amount by which the angle is changed just below. |
336,10 → 270,10 |
accDerived = getAngleEstimateFromAcc(axis); |
debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
// 1000 * the correction amount that will be added to the gyro angle in next line. |
temp = angle[axis]; |
angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
temp = attitude[axis]; |
attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp |
+ (int32_t) permilleAcc * accDerived) / 1000L; |
correctionSum[axis] += angle[axis] - temp; |
correctionSum[axis] += attitude[axis] - temp; |
} |
} else { |
debugOut.analog[9] = 0; |
372,7 → 306,7 |
timer = DRIFTCORRECTION_TIME; |
for (axis = PITCH; axis <= ROLL; axis++) { |
// Take the sum of corrections applied, add it to delta |
if (correctionSum[axis] >=0) |
if (correctionSum[axis] >= 0) |
round = DRIFTCORRECTION_TIME / 2; |
else |
round = -DRIFTCORRECTION_TIME / 2; |
383,7 → 317,6 |
// DebugOut.Analog[11 + axis] = correctionSum[axis]; |
// DebugOut.Analog[16 + axis] = correctionSum[axis]; |
// debugOut.analog[28 + axis] = driftComp[axis]; |
|
correctionSum[axis] = 0; |
} |
} |
390,16 → 323,84 |
} |
|
void calculateAccVector(void) { |
int16_t temp; |
temp = filteredAcc[0] >> 3; |
accVector = temp * temp; |
temp = filteredAcc[1] >> 3; |
accVector += temp * temp; |
temp = filteredAcc[2] >> 3; |
accVector += temp * temp; |
//debugOut.analog[18] = accVector; |
int16_t temp; |
temp = filteredAcc[0] >> 3; |
accVector = temp * temp; |
temp = filteredAcc[1] >> 3; |
accVector += temp * temp; |
temp = filteredAcc[2] >> 3; |
accVector += temp * temp; |
//debugOut.analog[18] = accVector; |
} |
|
void attitude_resetHeadingToMagnetic(void) { |
if (commands_isCalibratingCompass()) |
return; |
|
// Compass is off, skip. |
if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
return; |
|
// Compass is invalid, skip. |
if (magneticHeading < 0) |
return; |
|
heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
targetHeading = heading; |
|
debugOut.digital[0] ^= DEBUG_COMPASS; |
} |
|
void correctHeadingToMagnetic(void) { |
int32_t error; |
|
debugOut.analog[27] = heading; |
|
if (commands_isCalibratingCompass()) |
return; |
|
// Compass is off, skip. |
// Naaah this is assumed. |
// if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
// return; |
|
// Compass is invalid, skip. |
if (magneticHeading < 0) |
return; |
|
// Spinning fast, skip |
if (abs(yawRate) > 128) |
return; |
|
// Otherwise invalidated, skip |
if (ignoreCompassTimer) { |
ignoreCompassTimer--; |
return; |
} |
|
// TODO: Find computational cost of this. |
error = (magneticHeading*GYRO_DEG_FACTOR_YAW - heading) % GYRO_DEG_FACTOR_YAW; |
|
// We only correct errors larger than the resolution of the compass, or else we would keep rounding the |
// better resolution of the gyros to the worse resolution of the compass all the time. |
// The correction should really only serve to compensate for gyro drift. |
if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
|
int32_t correction = (error * (int32_t)dynamicParams.compassYawEffect) >> 8; |
|
// The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
// and to the target heading (the direction to which it maneuvers to point). That means, this correction has |
// no effect on control at all!!! It only has effect on the values of the two variables. However, these values |
// could have effect on control elsewhere, like in compassControl.c . |
heading += correction; |
intervalWrap(&heading, YAWOVER360); |
|
targetHeading += correction; |
intervalWrap(&targetHeading, YAWOVER360); |
|
debugOut.digital[1] ^= DEBUG_COMPASS; |
} |
|
/************************************************************************ |
* Main procedure. |
************************************************************************/ |
416,80 → 417,9 |
// We are done reading variables from the analog module. |
// Interrupt-driven sensor reading may restart. |
startAnalogConversionCycle(); |
} |
|
void updateCompass(void) { |
int16_t w, v, r, correction, error; |
|
if (compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) { |
if (controlMixer_testCompassCalState()) { |
compassCalState++; |
if (compassCalState < 5) |
beepNumber(compassCalState); |
else |
beep(1000); |
} |
} else { |
// get maximum attitude angle |
w = abs(angle[PITCH] / 512); |
v = abs(angle[ROLL] / 512); |
if (v > w) |
w = v; |
correction = w / 8 + 1; |
// calculate the deviation of the yaw gyro heading and the compass heading |
if (magneticHeading < 0) |
error = 0; // disable yaw drift compensation if compass heading is undefined |
else if (abs(yawRate) > 128) { // spinning fast |
error = 0; |
} else { |
// compassHeading - yawGyroHeading, on a -180..179 deg interval. |
error = ((540 + magneticHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) |
% 360) - 180; |
} |
if (!ignoreCompassTimer && w < 25) { |
yawGyroDrift += error; |
// Basically this gets set if we are in "fix" mode, and when starting. |
if (updateCompassCourse) { |
beep(200); |
yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
compassCourse = magneticHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
updateCompassCourse = 0; |
} |
} |
yawGyroHeading += (error * 8) / correction; |
|
/* |
w = (w * dynamicParams.CompassYawEffect) / 32; |
w = dynamicParams.CompassYawEffect - w; |
*/ |
w = dynamicParams.compassYawEffect - (w * dynamicParams.compassYawEffect) |
/ 32; |
|
// As readable formula: |
// w = dynamicParams.CompassYawEffect * (1-w/32); |
|
if (w >= 0) { // maxAttitudeAngle < 32 |
if (!ignoreCompassTimer) { |
/*v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;*/ |
v = 64 + controlActivity / 100; |
// yawGyroHeading - compassCourse on a -180..179 degree interval. |
r |
= ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
% 360) - 180; |
v = (r * w) / v; // align to compass course |
// limit yaw rate |
w = 3 * dynamicParams.compassYawEffect; |
if (v > w) |
v = w; |
else if (v < -w) |
v = -w; |
yawAngleDiff += v; |
} else { // wait a while |
ignoreCompassTimer--; |
} |
} else { // ignore compass at extreme attitudes for a while |
ignoreCompassTimer = 500; |
} |
if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
correctHeadingToMagnetic(); |
} |
} |
|