Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2054 → Rev 2055

/branches/dongfang_FC_rewrite/analog.c
297,6 → 297,27
}
}
 
// Experimental gyro shake takeoff detect!
uint16_t gyroActivity = 0;
void measureGyroActivityAndUpdateGyro(uint8_t axis, int16_t newValue) {
int16_t tmp = gyro_ATT[axis];
gyro_ATT[axis] = newValue;
 
tmp -= newValue;
tmp = (tmp*tmp) >> 4;
 
if (gyroActivity + (uint16_t)tmp < 0x8000)
gyroActivity += tmp;
}
 
#define GADAMPING 10
void dampenGyroActivity(void) {
uint32_t tmp = gyroActivity;
tmp *= ((1<<GADAMPING)-1);
tmp >>= GADAMPING;
gyroActivity = tmp;
}
 
void analog_updateGyros(void) {
// for various filters...
int16_t tempOffsetGyro[2], tempGyro;
349,19 → 370,10
*/
rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR);
 
gyro_ATT[PITCH] = tempOffsetGyro[PITCH];
gyro_ATT[ROLL] = tempOffsetGyro[ROLL];
measureGyroActivityAndUpdateGyro(0, tempOffsetGyro[PITCH]);
measureGyroActivityAndUpdateGyro(1, tempOffsetGyro[ROLL]);
dampenGyroActivity();
 
debugOut.analog[22 + 0] = gyro_PID[0];
debugOut.analog[22 + 1] = gyro_PID[1];
 
debugOut.analog[24 + 0] = gyro_ATT[0];
debugOut.analog[24 + 1] = gyro_ATT[1];
 
// 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else.
// gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter;
// gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL]) / staticParams.attitudeGyroFilter;
 
// Yaw gyro.
if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW)
yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW];
427,6 → 439,8
// Even if the sample is off-range, use it.
simpleAirPressure = getSimplePressure(rawAirPressure);
debugOut.analog[6] = rawAirPressure;
debugOut.analog[7] = simpleAirPressure;
if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) {
// Danger: pressure near lower end of range. If the measurement saturates, the
490,7 → 504,7
analog_updateAirPressure();
analog_updateBatteryVoltage();
#ifdef USE_MK3MAG
debugOut.analog[12] = magneticHeading = volatileMagneticHeading;
magneticHeading = volatileMagneticHeading;
#endif
}
 
517,9 → 531,7
// Therefore run measurement for 100ms to achive stable readings
delay_ms_with_adc_measurement(100, 0);
 
// Rough estimate. Hmm no nothing happens at calibration anyway.
// airPressureSum = simpleAirPressure * (AIRPRESSURE_OVERSAMPLING/2);
// pressureMeasurementCount = 0;
gyroActivity = 0;
}
 
void analog_calibrateGyros(void) {
/branches/dongfang_FC_rewrite/analog.h
194,6 → 194,8
 
extern int16_t magneticHeading;
 
extern uint16_t gyroActivity;
 
/*
* Flag: Interrupt handler has done all A/D conversion and processing.
*/
/branches/dongfang_FC_rewrite/attitude.c
201,8 → 201,6
 
headingError += ACYawRate;
 
debugOut.analog[27] = heading / 100;
 
/*
* Pitch axis integration and range boundary wrap.
*/
268,7 → 266,7
*/
for (axis = PITCH; axis <= ROLL; axis++) {
accDerived = getAngleEstimateFromAcc(axis);
debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
//debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
// 1000 * the correction amount that will be added to the gyro angle in next line.
temp = attitude[axis];
attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp
276,8 → 274,6
correctionSum[axis] += attitude[axis] - temp;
}
} else {
debugOut.analog[9] = 0;
debugOut.analog[10] = 0;
// experiment: Kill drift compensation updates when not flying smooth.
// correctionSum[PITCH] = correctionSum[ROLL] = 0;
debugOut.digital[0] |= DEBUG_ACC0THORDER;
357,7 → 353,7
int32_t error;
 
if (commands_isCalibratingCompass()) {
debugOut.analog[29] = 1;
//debugOut.analog[29] = 1;
return;
}
 
368,13 → 364,13
 
// Compass is invalid, skip.
if (magneticHeading < 0) {
debugOut.analog[29] = 2;
//debugOut.analog[29] = 2;
return;
}
 
// Spinning fast, skip
if (abs(yawRate) > 128) {
debugOut.analog[29] = 3;
//debugOut.analog[29] = 3;
return;
}
 
381,7 → 377,7
// Otherwise invalidated, skip
if (ignoreCompassTimer) {
ignoreCompassTimer--;
debugOut.analog[29] = 4;
//debugOut.analog[29] = 4;
return;
}
 
396,7 → 392,7
if(abs(error) < GYRO_DEG_FACTOR_YAW) return;
 
int32_t correction = (error * staticParams.compassYawCorrection) >> 8;
debugOut.analog[30] = correction;
//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
409,7 → 405,7
// 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;
//debugOut.analog[29] = 0;
}
#endif
 
/branches/dongfang_FC_rewrite/commands.c
89,7 → 89,6
paramSet_readFromEEProm(getActiveParamSet());
analog_calibrateGyros();
attitude_setNeutral();
flight_setNeutral();
controlMixer_setNeutral();
beepNumber(getActiveParamSet());
}
108,7 → 107,6
// Run gyro and acc. meter calibration but do not repeat it.
analog_calibrateAcc();
attitude_setNeutral();
flight_setNeutral();
controlMixer_setNeutral();
beepNumber(getActiveParamSet());
}
/branches/dongfang_FC_rewrite/configuration.c
247,7 → 247,7
staticParams.dynamicStability = 50;
staticParams.maxAccVector = 10;
staticParams.IFactor = 50;
staticParams.IFactor = 52;
staticParams.yawIFactor = 100;
staticParams.compassYawCorrection = 64;
staticParams.compassP = 50;
/branches/dongfang_FC_rewrite/configuration.h
213,7 → 213,7
 
// MKFlags
#define MKFLAG_MOTOR_RUN (1<<0)
#define MKFLAG_FLY (1<<1)
//#define MKFLAG_FLY (1<<1)
#define MKFLAG_CALIBRATE (1<<2)
#define MKFLAG_START (1<<3)
#define MKFLAG_EMERGENCY_FLIGHT (1<<4)
/branches/dongfang_FC_rewrite/controlMixer.c
5,6 → 5,7
#include "attitudeControl.h"
#include "externalControl.h"
#include "compassControl.h"
#include "failsafeControl.h"
#include "naviControl.h"
#include "configuration.h"
#include "attitude.h"
44,24 → 45,14
 
void controlMixer_setNeutral() {
for (uint8_t i=0; i<VARIABLE_COUNT; i++) {
variables[i] = 0;
variables[i] = RC_getVariable(i);
}
EC_setNeutral();
HC_setGround();
FC_setNeutral(); // FC is FailsafeControl, not FlightCtrl.
}
 
/*
* Set the potientiometer values to the momentary values of the respective R/C channels.
* No slew rate limitation.
*/
void controlMixer_initVariables(void) {
uint8_t i;
for (i=0; i < VARIABLE_COUNT; i++) {
variables[i] = RC_getVariable(i);
}
}
 
/*
* Update potentiometer values with limited slew rate. Could be made faster if desired.
* TODO: It assumes R/C as source. Not necessarily true.
*/
100,7 → 91,7
controlActivity += tmp;
else controlActivity = 0xffff;
*/
if (controlActivity + (uint16_t)tmp < 32768)
if (controlActivity + (uint16_t)tmp < 0x8000)
controlActivity += tmp;
}
 
166,12 → 157,16
// Add compass control (could also have been before navi, they are independent)
CC_periodicTaskAndPRTY(tempPRTY);
 
// Add height control (could also have been before navi and/or compass, they are independent)
HC_periodicTaskAndPRTY(tempPRTY);
FC_periodicTaskAndPRTY(tempPRTY);
 
// Add attitude control (could also have been before navi and/or compass, they are independent)
AC_getPRTY(tempPRTY);
if (!MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
// Add height control (could also have been before navi and/or compass, they are independent)
HC_periodicTaskAndPRTY(tempPRTY);
 
// Add attitude control (could also have been before navi and/or compass, they are independent)
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]);
178,8 → 173,7
updateControlAndMeasureControlActivity(CONTROL_ROLL, tempPRTY[CONTROL_ROLL]);
updateControlAndMeasureControlActivity(CONTROL_YAW, tempPRTY[CONTROL_YAW]);
dampenControlActivity();
 
debugOut.analog[17] = controlActivity/10;
//debugOut.analog[17] = controlActivity/10;
// We can safely do this even with a bad signal - the variables will not have been updated then.
configuration_applyVariablesToParams();
/branches/dongfang_FC_rewrite/directGPSNaviControl.c
61,8 → 61,6
beep(100);
flightModeOld = flightMode;
}
 
//debugOut.analog[31] = flightMode;
}
 
// ---------------------------------------------------------------------------------
359,6 → 357,4
GPSInfo.status = PROCESSED;
break;
} // eof GPSInfo.status
 
debugOut.analog[11] = GPSInfo.satnum;
}
/branches/dongfang_FC_rewrite/eeprom.c
57,6 → 57,7
#include "eeprom.h"
#include "printf_P.h"
#include "output.h"
#include <avr/wdt.h>
#include <avr/eeprom.h>
 
// byte array in eeprom
167,6 → 168,9
}
// default-Setting is parameter set 3
setActiveParamSet(1);
// For some strange reason, the read will have no effect.
// Lets reset...
wdt_enable(WDTO_250MS);
}
printf("\n\r\rUsing Parameter Set %d", getActiveParamSet());
/branches/dongfang_FC_rewrite/flight.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 (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 "eeprom.h"
75,10 → 24,10
* value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
*/
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
 
uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control
uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
uint8_t invKi = 64;
uint8_t invKi;
int32_t IPart[2];
 
/************************************************************************/
/* Filter for motor value smoothing (necessary???) */
104,21 → 53,7
}
}
 
/************************************************************************/
/* Neutral Readings */
/************************************************************************/
void flight_setNeutral() {
MKFlags |= MKFLAG_CALIBRATE;
// not really used here any more.
/*
dynamicParams.KalmanK = -1;
dynamicParams.KalmanMaxDrift = 0;
dynamicParams.KalmanMaxFusion = 32;
*/
controlMixer_initVariables();
}
 
void setFlightParameters(uint8_t _invKi, uint8_t _gyroPFactor,
void flight_setParameters(uint8_t _invKi, uint8_t _gyroPFactor,
uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
invKi = _invKi;
gyroPFactor = _gyroPFactor;
127,18 → 62,18
yawIFactor = _yawIFactor;
}
 
void setNormalFlightParameters(void) {
setFlightParameters(
staticParams.IFactor,
dynamicParams.gyroP,
staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,
dynamicParams.gyroP,
staticParams.yawIFactor
);
void flight_setGround() {
// Just reset all I terms.
IPart[PITCH] = IPart[ROLL] = 0;
headingError = 0;
}
 
void setStableFlightParameters(void) {
setFlightParameters(0, 90, 120, 90, 120);
void flight_takeOff() {
HC_setGround();
#ifdef USE_MK3MAG
attitude_resetHeadingToMagnetic();
compass_setTakeoffHeading(heading);
#endif
}
 
/************************************************************************/
145,14 → 80,12
/* Main Flight Control */
/************************************************************************/
void flight_control(void) {
uint16_t tmp_int;
int16_t tmp_int;
// Mixer Fractions that are combined for Motor Control
int16_t yawTerm, throttleTerm, term[2];
 
// PID controller variables
int16_t PDPart;
static int32_t IPart[2] = {0, 0};
static uint16_t emergencyFlightTime;
static int8_t debugDataTimer = 1;
 
// High resolution motor values for smoothing of PID motor outputs
162,6 → 95,22
 
throttleTerm = controls[CONTROL_THROTTLE];
 
if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
// increment flight-time counter until overflow.
if (isFlying != 0xFFFF)
isFlying++;
}
/*
* When standing on the ground, do not apply I controls and zero the yaw stick.
* Probably to avoid integration effects that will cause the copter to spin
* or flip when taking off.
*/
if (isFlying < 256) {
flight_setGround();
if (isFlying == 250)
flight_takeOff();
}
 
// This check removed. Is done on a per-motor basis, after output matrix multiplication.
if (throttleTerm < staticParams.minThrottle + 10)
throttleTerm = staticParams.minThrottle + 10;
168,100 → 117,27
else if (throttleTerm > staticParams.maxThrottle - 20)
throttleTerm = (staticParams.maxThrottle - 20);
 
/************************************************************************/
/* RC-signal is bad */
/* This part could be abstracted, as having yet another control input */
/* to the control mixer: An emergency autopilot control. */
/************************************************************************/
// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
throttleTerm *= CONTROL_SCALING;
// TODO: We dont need to repeat this for every iteration!
 
if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
if (controlMixer_didReceiveSignal) beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss.
if (emergencyFlightTime) {
// continue emergency flight
emergencyFlightTime--;
if (isFlying > 256) {
// We're probably still flying. Descend slowly.
throttleTerm = staticParams.emergencyThrottle; // Set emergency throttle
MKFlags |= (MKFLAG_EMERGENCY_FLIGHT); // Set flag for emergency landing
setStableFlightParameters();
} else {
MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors.
}
} else {
// end emergency flight (just cut the motors???)
MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_FLIGHT);
}
} else {
// signal is acceptable
if (controlMixer_getSignalQuality() > SIGNAL_BAD) {
// Reset emergency landing control variables.
MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing
// The time is in whole seconds.
if (staticParams.emergencyFlightDuration > (65535-F_MAINLOOP)/F_MAINLOOP)
emergencyFlightTime = 0xffff;
else
emergencyFlightTime = (uint16_t)staticParams.emergencyFlightDuration * F_MAINLOOP;
}
 
// If some throttle is given, and the motor-run flag is on, increase the probability that we are flying.
if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
// increment flight-time counter until overflow.
if (isFlying != 0xFFFF)
isFlying++;
} else
/*
* When standing on the ground, do not apply I controls and zero the yaw stick.
* Probably to avoid integration effects that will cause the copter to spin
* or flip when taking off.
*/
if (isFlying < 256) {
IPart[PITCH] = IPart[ROLL] = 0;
if (isFlying == 250) {
HC_setGround();
#ifdef USE_MK3MAG
attitude_resetHeadingToMagnetic();
compass_setTakeoffHeading(heading);
#endif
// 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?
// Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe.
MKFlags |= (MKFLAG_FLY);
}
commands_handleCommands();
setNormalFlightParameters();
} // end else (not bad signal case)
// end part 1: 750-800 usec.
// start part 3: 350 - 400 usec.
// end part 1: 750-800 usec.
// start part 3: 350 - 400 usec.
#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
// This is where control affects the target heading. It also (later) directly controls 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;
if (headingError < -YAW_I_LIMIT)
headingError = -YAW_I_LIMIT;
if (headingError > YAW_I_LIMIT)
headingError = YAW_I_LIMIT;
 
PDPart = (int32_t)(headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
PDPart += (int32_t)(yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);
PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
PDPart += (int32_t) (yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);
 
/*
* Compose throttle term.
* If a Bl-Ctrl is missing, prevent takeoff.
*/
if (missingMotor) {
// if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
if (isFlying > 1 && isFlying < 50 && throttleTerm > 0)
isFlying = 1; // keep within lift off condition
throttleTerm = staticParams.minThrottle; // reduce gas to min to avoid lift of
}
// Lets not limit P and D.
// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
 
// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
throttleTerm *= CONTROL_SCALING;
 
/*
* Compose yaw term.
* The yaw term is limited: Absolute value is max. = the throttle term / 2.
271,7 → 147,7
*/
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value
yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING;
// Limit yawTerm
// Limit yawTerm
debugOut.digital[0] &= ~DEBUG_CLIP;
if (throttleTerm > MIN_YAWGAS) {
if (yawTerm < -throttleTerm / 2) {
292,6 → 168,7
}
 
tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
 
if (yawTerm < -(tmp_int - throttleTerm)) {
yawTerm = -(tmp_int - throttleTerm);
debugOut.digital[0] |= DEBUG_CLIP;
302,7 → 179,8
 
debugOut.digital[1] &= ~DEBUG_CLIP;
 
tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + abs(yawTerm) / 2)) >> 6;
tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + (abs(yawTerm) >> 1)) >> 6);
//tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
 
/************************************************************************/
/* Calculate control feedback from angle (gyro integral) */
310,19 → 188,19
/************************************************************************/
// The P-part is the P of the PID controller. That's the angle integrals (not rates).
for (axis = PITCH; axis <= ROLL; axis++) {
int16_t iDiff;
iDiff = PDPart = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 3);
PDPart += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
// In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
// In HH mode, the I part is summed from P and D of gyros minus stick.
if (gyroIFactor) {
int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 3);
PDPart += iDiff;
IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
} else {
IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
}
 
// With normal Ki, limit effect to +/- 205 (of 1024!!!)
// With normal Ki, limit I parts to +/- 205 (of about 1024)
if (IPart[axis] < -64000) {
IPart[axis] = -64000;
debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
331,9 → 209,10
debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
}
 
term[axis] = PDPart - controls[axis] + ((int32_t)IPart[axis] * invKi) >> 14;
term[axis] += (dynamicParams.levelCorrection[axis] - 128);
/*
term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * invKi) >> 14);
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).
341,8 → 220,10
*/
if (term[axis] < -tmp_int) {
debugOut.digital[1] |= DEBUG_CLIP;
term[axis] = -tmp_int;
} else if (term[axis] > tmp_int) {
debugOut.digital[1] |= DEBUG_CLIP;
term[axis] = tmp_int;
}
}
 
351,27 → 232,30
// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
debugOut.analog[3] = rate_ATT[PITCH];
debugOut.analog[4] = rate_ATT[ROLL];
debugOut.analog[5] = yawRate;
if (!(--debugDataTimer)) {
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
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;
 
debugOut.analog[6] = filteredAcc[PITCH];
debugOut.analog[7] = filteredAcc[ROLL];
debugOut.analog[8] = filteredAcc[Z];
debugOut.analog[3] = rate_ATT[PITCH];
debugOut.analog[4] = rate_ATT[ROLL];
debugOut.analog[5] = yawRate;
}
 
debugOut.analog[13] = term[PITCH];
debugOut.analog[14] = term[ROLL];
debugOut.analog[15] = yawTerm;
debugOut.analog[16] = throttleTerm;
debugOut.analog[8] = yawTerm;
debugOut.analog[9] = throttleTerm;
 
debugOut.analog[16] = gyroActivity;
 
for (i = 0; i < MAX_MOTORS; i++) {
int32_t tmp;
uint8_t throttle;
 
tmp = (int32_t)throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];
tmp += (int32_t)term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];
tmp += (int32_t)term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];
tmp += (int32_t)yawTerm * mixerMatrix.motor[i][MIX_YAW];
tmp = (int32_t) throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];
tmp += (int32_t) term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];
tmp += (int32_t) term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];
tmp += (int32_t) yawTerm * mixerMatrix.motor[i][MIX_YAW];
tmp = tmp >> 6;
motorFilters[i] = motorFilter(tmp, motorFilters[i]);
// Now we scale back down to a 0..255 range.
387,7 → 271,8
CHECK_MIN_MAX(tmp, 1, 255);
throttle = tmp;
 
// if (i < 4) debugOut.analog[22 + i] = throttle;
if (i < 4)
debugOut.analog[10 + i] = throttle;
 
if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {
motor[i].throttle = throttle;
399,14 → 284,4
}
 
I2C_Start(TWI_STATE_MOTOR_TX);
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugging
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (!(--debugDataTimer)) {
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
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,8 → 12,12
#define PITCH 0
#define ROLL 1
 
void flight_setParameters(uint8_t _invKi, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor);
void flight_setGround(void);
void flight_takeOff(void);
 
void flight_control(void);
void transmitMotorThrottleData(void);
void flight_setNeutral(void);
// void flight_setNeutral(void);
 
#endif //_FLIGHT_H
/branches/dongfang_FC_rewrite/heightControl.c
90,8 → 90,10
int32_t height = analog_getHeight();
int32_t heightError = rampedTargetHeight - height;
int16_t dHeight = analog_getDHeight();
//lastHeight = height;
debugOut.analog[14] = height/100L;
debugOut.analog[15] = dHeight;
 
if (heightError > 0) {
debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
} else {
122,9 → 124,9
int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10;
int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7;
 
debugOut.analog[24] = dThrottleP;
debugOut.analog[25] = dThrottleI;
debugOut.analog[26] = dThrottleD;
//debugOut.analog[24] = dThrottleP;
//debugOut.analog[25] = dThrottleI;
//debugOut.analog[26] = dThrottleD;
 
//debugOut.analog[27] = dynamicParams.heightP;
//debugOut.analog[28] = dynamicParams.heightI;
137,11 → 139,13
else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
dThrottle = -staticParams.heightControlMaxThrottleChange;
 
/*
debugOut.analog[19] = throttle;
debugOut.analog[20] = dThrottle;
debugOut.analog[21] = height;
debugOut.analog[22] = rampedTargetHeight;
debugOut.analog[23] = heightError;
*/
 
if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)
/branches/dongfang_FC_rewrite/main.c
1,55 → 1,3
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt und genannt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// + from this software without specific prior written permission.
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
// + for non-commercial use (directly or indirectly)
// + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// + with our written permission
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// + clearly linked as origin
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#include <avr/boot.h>
#include <avr/io.h>
#include <avr/interrupt.h>
62,6 → 10,7
#include "output.h"
#include "menu.h"
#include "attitude.h"
#include "commands.h"
#include "flight.h"
#include "rc.h"
#include "analog.h"
177,7 → 126,7
attitude_setNeutral();
 
// Init flight parameters
flight_setNeutral();
// flight_setNeutral();
 
beep(2000);
 
208,6 → 157,7
// Flight control uses results from both.
calculateFlightAttitude();
controlMixer_periodicTask();
commands_handleCommands();
flight_control();
J4LOW;
 
/branches/dongfang_FC_rewrite/makefile
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 = main.c uart0.c printf_P.c timer0.c timer2.c analog.c menu.c output.c controlMixer.c failsafeControl.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
 
/branches/dongfang_FC_rewrite/rc.c
207,7 → 207,7
commandTimer = 0;
}
} // if RCQuality is no good, we just do nothing.
debugOut.analog[18] = RCQuality;
// debugOut.analog[18] = RCQuality;
}
 
/*
/branches/dongfang_FC_rewrite/uart0.c
25,7 → 25,9
 
#define FALSE 0
#define TRUE 1
//int8_t test __attribute__ ((section (".noinit")));
 
uint8_t requestedDebugLabel = 255;
 
uint8_t request_verInfo = FALSE;
uint8_t request_externalControl = FALSE;
uint8_t request_display = FALSE;
32,11 → 34,26
uint8_t request_display1 = FALSE;
uint8_t request_debugData = FALSE;
uint8_t request_data3D = FALSE;
uint8_t request_debugLabel = 255;
uint8_t request_PPMChannels = FALSE;
uint8_t request_motorTest = FALSE;
uint8_t request_variables = FALSE;
uint8_t request_OSD = FALSE;
 
/*
#define request_verInfo (1<<0)
#define request_externalControl (1<<1)
#define request_display (1<<3)
#define request_display1 (1<<4)
#define request_debugData (1<<5)
#define request_data3D (1<<6)
#define request_PPMChannels (1<<7)
#define request_motorTest (1<<8)
#define request_variables (1<<9)
#define request_OSD (1<<10)
*/
 
//uint16_t request = 0;
 
uint8_t displayLine = 0;
 
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
59,8 → 76,10
 
uint16_t debugData_timer;
uint16_t data3D_timer;
uint16_t OSD_timer;
uint16_t debugData_interval = 0; // in 1ms
uint16_t data3D_interval = 0; // in 1ms
uint16_t OSD_interval = 0;
 
#ifdef USE_DIRECT_GPS
int16_t toMk3MagTimer;
75,17 → 94,17
"GyroPitch ",
"GyroRoll ",
"GyroYaw ", //5
"AccPitch ",
"AccRoll ",
"AccZ ",
"AccPitch (angle)",
"AccRoll (angle) ", //10
"GPS sat ",
"mag hdng ",
"Pitch Term ",
"Roll Term ",
"Yaw Term ", //15
"Throttle Term ",
"PitchTerm ",
"RollTerm ",
"ThrottleTerm ",
"YawTerm ",
"M1 ", //10
"M2 ",
"M3 ",
"M4 ",
"pdpart ",
"control ", //15
"ipart ",
"ControlAct/10 ",
"RC Qual ",
"heightTrottleIn ",
96,9 → 115,9
"HeightPdThrottle",
"HeightIdThrottle", //25
"HeightDdThrottle",
"HeadingInDegrees",
"0 ",
"1 ",
"Yaw Limit / 100 ",
"HeadingError/100",
"PD Yaw ",
"2 ", //30
"3 "
};
181,6 → 200,10
versionInfo.protoMajor = VERSION_SERIAL_MAJOR;
versionInfo.protoMinor = VERSION_SERIAL_MINOR;
 
for (uint8_t i=0; i<32; i++) {
debugOut.analog[i] = i;
}
 
// restore global interrupt flags
SREG = sreg;
}
267,7 → 290,7
}
 
// --------------------------------------------------------------------------
void Addchecksum(uint16_t datalen) {
void addChecksum(uint16_t datalen) {
uint16_t tmpchecksum = 0, i;
for (i = 0; i < datalen; i++) {
tmpchecksum += txd_buffer[i];
393,11 → 416,11
txd_buffer[pt++] = '=' + (c & 0x3f);
}
va_end(ap);
Addchecksum(pt); // add checksum after data block and initates the transmission
addChecksum(pt); // add checksum after data block and initates the transmission
}
 
// --------------------------------------------------------------------------
void Decode64(void) {
void decode64(void) {
uint8_t a, b, c, d;
uint8_t x, y, z;
uint8_t ptrIn = 3;
441,7 → 464,7
if (!rxd_buffer_locked)
return;
uint8_t tempchar[3];
Decode64(); // decode data block in rxd_buffer
decode64(); // decode data block in rxd_buffer
 
switch (rxd_buffer[1] - 'a') {
 
532,9 → 555,9
default: // any Slave Address
switch (rxd_buffer[2]) {
case 'a':// request for labels of the analog debug outputs
request_debugLabel = pRxData[0];
if (request_debugLabel > 31)
request_debugLabel = 31;
requestedDebugLabel = pRxData[0];
if (requestedDebugLabel > 31)
requestedDebugLabel = 31;
break;
 
case 'b': // submit extern control
555,6 → 578,12
request_display1 = TRUE;
break;
 
case 'o':// request for OSD data (FC style)
OSD_interval = (uint16_t) pRxData[0] * 10;
if (OSD_interval > 0)
request_OSD = TRUE;
break;
case 'v': // request for version and board release
request_verInfo = TRUE;
break;
631,12 → 660,12
request_display1 = FALSE;
}
 
if (request_debugLabel != 0xFF) { // Texte f�r die Analogdaten
if (requestedDebugLabel != 0xFF && txd_complete) { // Texte f�r die Analogdaten
uint8_t label[16]; // local sram buffer
memcpy_P(label, ANALOG_LABEL[request_debugLabel], 16); // read lable from flash to sram buffer
sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_debugLabel,
sizeof(request_debugLabel), label, 16);
request_debugLabel = 0xFF;
memcpy_P(label, ANALOG_LABEL[requestedDebugLabel], 16); // read lable from flash to sram buffer
sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &requestedDebugLabel,
sizeof(requestedDebugLabel), label, 16);
requestedDebugLabel = 0xFF;
}
 
if (confirmFrame && txd_complete) { // Datensatz ohne checksum best�tigen
651,12 → 680,11
request_debugData = FALSE;
}
 
if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D)
&& txd_complete) {
if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) && txd_complete) {
sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D));
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.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
data3D_timer = setDelay(data3D_interval);
request_data3D = FALSE;
}
695,4 → 723,14
sendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables));
request_variables = FALSE;
}
 
if (((OSD_interval && checkDelay(OSD_timer)) || request_OSD) && txd_complete) {
int32_t height = analog_getHeight();
data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
sendOutData('O', FC_ADDRESS, 4, (uint8_t*)&data3D, sizeof(data3D), (uint8_t*)&GPSInfo, sizeof(GPSInfo), (uint8_t*)&height, sizeof(height), (uint8_t*)UBat, sizeof(UBat));
OSD_timer = setDelay(OSD_interval);
request_OSD = FALSE;
}
}
/branches/dongfang_FC_rewrite/uart0.h
5,6 → 5,7
#define RXD_BUFFER_LEN 180
 
#include <inttypes.h>
#include "ubx.h"
 
//Baud rate of the USART
#define USART0_BAUD 57600
20,10 → 21,17
extern uint8_t motorTest[16];
 
typedef struct {
int16_t anglePitch; // in 0.1 deg
int16_t angleRoll; // in 0.1 deg
int16_t heading; // in 0.1 deg
uint8_t reserved[8];
int16_t anglePitch; // in 0.1 deg
int16_t angleRoll; // in 0.1 deg
int16_t heading; // in 0.1 deg
uint8_t reserved[8];
}__attribute__((packed)) Data3D_t;
 
typedef struct {
Data3D_t attitude;
GPS_INFO_t GPSInfo;
int32_t airpressureHeight;
int16_t batteryVoltage;
}__attribute__((packed)) OSDData_t;
 
#endif //_UART0_H