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