/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 |