// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 excample: 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"
#include "flight.h"
// Only for debug. Remove.
//#include "analog.h"
//#include "rc.h"
// Necessary for external control and motor test
#include "uart0.h"
#include "twimaster.h"
#include "attitude.h"
#include "controlMixer.h"
#ifdef USE_MK3MAG
#include "gps.h"
#endif
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
/*
* These are no longer maintained, just left at 0. The original implementation just summed the acc.
* 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;
// MK flags
uint16_t isFlying
= 0;
volatile uint8_t MKFlags
= 0;
uint8_t gyroPFactor
, gyroIFactor
; // the PD factors for the attitude control
uint8_t yawPFactor
, yawIFactor
; // the PD factors for the yaw control
// Some integral weight constant...
uint16_t Ki
= 10300 / 33;
uint8_t RequiredMotors
= 0;
// No support for altitude control right now.
// int16_t SetPointHeight = 0;
/************************************************************************/
/* Filter for motor value smoothing (necessary???) */
/************************************************************************/
int16_t motorFilter
(int16_t newvalue
, int16_t oldvalue
) {
switch(dynamicParams.
UserParams[5]) {
case 0:
return newvalue
;
case 1:
return (oldvalue
+ newvalue
) / 2;
case 2:
if(newvalue
> oldvalue
)
return (1 * (int16_t)oldvalue
+ newvalue
) / 2; //mean of old and new
else
return newvalue
- (oldvalue
- newvalue
) * 1; // 2 * new - old
case 3:
if(newvalue
< oldvalue
)
return (1 * (int16_t)oldvalue
+ newvalue
) / 2; //mean of old and new
else
return newvalue
- (oldvalue
- newvalue
) * 1; // 2 * new - old
default: return newvalue
;
}
}
/************************************************************************/
/* Neutral Readings */
/************************************************************************/
void flight_setNeutral
() {
// GPSStickPitch = 0;
// GPSStickRoll = 0;
MKFlags
|= MKFLAG_CALIBRATE
;
// not really used here any more.
dynamicParams.
KalmanK = -1;
dynamicParams.
KalmanMaxDrift = 0;
dynamicParams.
KalmanMaxFusion = 32;
controlMixer_initVariables
();
}
/************************************************************************/
/* Transmit Motor Data via I2C */
/************************************************************************/
void sendMotorData
(void) {
uint8_t i
;
if(!(MKFlags
& MKFLAG_MOTOR_RUN
)) {
// If pilot has not started the engines....
MKFlags
&= ~
(MKFLAG_FLY
| MKFLAG_START
); // clear flag FLY and START if motors are off
for(i
= 0; i
< MAX_MOTORS
; i
++) {
// and if we are not in motor test mode, cut throttle on all motors.
if(!motorTestActive
) Motor
[i
].
SetPoint = 0;
else Motor
[i
].
SetPoint = motorTest
[i
];
}
if(motorTestActive
) motorTestActive
--;
}
/*
DebugOut.Analog[] = Motor[0].SetPoint; // Front
DebugOut.Analog[] = Motor[1].SetPoint; // Rear
DebugOut.Analog[] = Motor[3].SetPoint; // Left
DebugOut.Analog[] = Motor[2].SetPoint; // Right
*/
// Start I2C Interrupt Mode
I2C_Start
(TWI_STATE_MOTOR_TX
);
}
void setFlightParameters
(uint8_t _Ki
, uint8_t _gyroPFactor
, uint8_t _gyroIFactor
, uint8_t _yawPFactor
, uint8_t _yawIFactor
) {
Ki
= 10300 / _Ki
;
gyroPFactor
= _gyroPFactor
;
gyroIFactor
= _gyroIFactor
;
yawPFactor
= _yawPFactor
;
yawIFactor
= _yawIFactor
;
}
void setNormalFlightParameters
(void) {
setFlightParameters
(dynamicParams.
IFactor + 1,
dynamicParams.
GyroP + 10,
staticParams.
GlobalConfig & CFG_HEADING_HOLD
? 0 : dynamicParams.
GyroI,
dynamicParams.
GyroP + 10,
dynamicParams.
UserParams[6]
);
}
void setStableFlightParameters
(void) {
setFlightParameters
(33, 90, 120, 90, 120);
}
void handleCommands
(uint8_t command
, uint8_t argument
, uint8_t isCommandRepeated
) {
if(!(MKFlags
& MKFLAG_MOTOR_RUN
)) {
if (command
== COMMAND_GYROCAL
&& !isCommandRepeated
) {
// Run gyro calibration but do not repeat it.
GRN_OFF
;
// TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough?
// isFlying = 0;
// check roll/pitch stick position
// if pitch stick is top or roll stick is left or right --> change parameter setting
// according to roll/pitch stick position
if (argument
< 6) {
// Gyro calinbration, with or without selecting a new parameter-set.
if(argument
> 0 && argument
< 6) {
// A valid parameter-set (1..5) was chosen - use it.
setActiveParamSet
(argument
);
}
ParamSet_ReadFromEEProm
(getActiveParamSet
());
attitude_setNeutral
();
flight_setNeutral
();
// Right stick is centered; calibrate it to zero (hmm strictly does not belong here).
// If heading hold is active, do not do it. TODO: We also want to re-set old calibration.
controlMixer_setNeutral
(!argument
);
beepNumber
(getActiveParamSet
());
} else if(staticParams.
GlobalConfig & (CFG_COMPASS_ACTIVE
| CFG_GPS_ACTIVE
) && argument
== 7) {
// If right stick is centered and down
compassCalState
= 1;
beep
(1000);
}
}
// save the ACC neutral setting to eeprom
else {
if(command
== COMMAND_ACCCAL
&& !isCommandRepeated
) {
// Run gyro and acc. meter calibration but do not repeat it.
GRN_OFF
;
analog_calibrateAcc
();
attitude_setNeutral
();
flight_setNeutral
();
controlMixer_setNeutral
(1); // Calibrate right stick neutral position.
beepNumber
(getActiveParamSet
());
}
}
} // end !MOTOR_RUN condition.
if (command
== COMMAND_START
) {
isFlying
= 1; // TODO: Really????
// if (!controlMixer_isCommandRepeated()) {
// attitude_startDynamicCalibration(); // Try sense the effect of the motors on sensors.
MKFlags
|= (MKFLAG_MOTOR_RUN
| MKFLAG_START
); // set flag RUN and START. TODO: Is that START flag used at all???
// } else { // Pilot is holding stick, ever after motor start. Continue to sense the effect of the motors on sensors.
// attitude_continueDynamicCalibration();
// setPointYaw = 0;
// IPartPitch = 0;
// IPartRoll = 0;
// }
} else if (command
== COMMAND_STOP
) {
isFlying
= 0;
MKFlags
&= ~
(MKFLAG_MOTOR_RUN
);
}
}
/************************************************************************/
/* Main Flight Control */
/************************************************************************/
void flight_control
(void) {
int16_t tmp_int
;
// Mixer Fractions that are combined for Motor Control
int16_t yawTerm
, throttleTerm
, term
[2];
// PID controller variables
int16_t PDPart
[2], PDPartYaw
, PPart
[2];
static int32_t IPart
[2] = {0,0};
static int32_t setPointYaw
= 0;
// Removed. Too complicated, and apparently not necessary with MEMS gyros anyway.
// static int32_t IntegralGyroPitchError = 0, IntegralGyroRollError = 0;
// static int32_t CorrectionPitch, CorrectionRoll;
static uint16_t emergencyFlightTime
;
static int8_t debugDataTimer
= 1;
// High resolution motor values for smoothing of PID motor outputs
static int16_t motorFilters
[MAX_MOTORS
];
uint8_t i
, axis
;
// Fire the main flight attitude calculation, including integration of angles.
calculateFlightAttitude
();
GRN_ON
;
/*
* TODO: update should: Set the stick variables if good signal, set them to zero if bad.
* Set variables also.
*/
controlMixer_update
();
throttleTerm
= controlThrottle
;
if(throttleTerm
< staticParams.
MinThrottle + 10) throttleTerm
= staticParams.
MinThrottle + 10;
/************************************************************************/
/* RC-signal is bad */
/* This part could be abstracted, as having yet another control input */
/* to the control mixer: An emergency autopilot control. */
/************************************************************************/
if(controlMixer_getSignalQuality
() <= SIGNAL_BAD
) { // the rc-frame signal is not reveived or noisy
RED_ON
;
beepRCAlarm
();
if(emergencyFlightTime
) {
// continue emergency flight
emergencyFlightTime
--;
if(isFlying
> 1000) {
// We're probably still flying. Descend slowly.
throttleTerm
= staticParams.
EmergencyGas; // Set emergency throttle
MKFlags
|= (MKFLAG_EMERGENCY_LANDING
); // 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_LANDING
);
}
} else {
// signal is acceptable
if(controlMixer_getSignalQuality
() > SIGNAL_BAD
) {
// Reset emergency landing control variables.
MKFlags
&= ~
(MKFLAG_EMERGENCY_LANDING
); // clear flag for emergency landing
// The time is in whole seconds.
emergencyFlightTime
= staticParams.
EmergencyGasDuration * 488;
}
// 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;
// TODO: Don't stomp on other modules' variables!!!
controlYaw
= 0;
if(isFlying
== 250) {
updateCompassCourse
= 1;
yawAngle
= 0;
setPointYaw
= 0;
}
} else {
// DebugOut.Digital[1] = 0;
// 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
);
}
/*
* Get the current command (start/stop motors, calibrate), if any.
*/
uint8_t command
= controlMixer_getCommand
();
uint8_t repeated
= controlMixer_isCommandRepeated
();
uint8_t argument
= controlMixer_getArgument
();
handleCommands
(command
, argument
, repeated
);
// if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
setNormalFlightParameters
();
// }
} // end else (not bad signal case)
/*
* Looping the H&I way basically is just a matter of turning off attitude angle measurement
* by integration (because 300 deg/s gyros are too slow) and turning down the throttle.
* This is the throttle part.
*/
if(looping
) {
if(throttleTerm
> staticParams.
LoopGasLimit) throttleTerm
= staticParams.
LoopGasLimit;
}
/************************************************************************/
/* Yawing */
/************************************************************************/
if(abs(controlYaw
) > 4 * staticParams.
StickYawP) { // yaw stick is activated
badCompassHeading
= 1000;
if(!(staticParams.
GlobalConfig & CFG_COMPASS_FIX
)) {
updateCompassCourse
= 1;
}
}
setPointYaw
= controlYaw
;
// Trim drift of yawAngle with controlYaw.
// TODO: We want NO feedback of control related stuff to the attitude related stuff.
yawAngle
-= controlYaw
;
// limit the effect
CHECK_MIN_MAX
(yawAngle
, -50000, 50000)
/************************************************************************/
/* Compass is currently not supported. */
/************************************************************************/
/*
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) {
updateCompass();
}
*/
#if defined (USE_MK3MAG)
/************************************************************************/
/* GPS is currently not supported. */
/************************************************************************/
/*
if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) {
GPS_Main();
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
}
else {
// GPSStickPitch = 0;
// GPSStickRoll = 0;
}
*/
#endif
#define SENSOR_LIMIT (4096 * 4)
/************************************************************************/
/* Calculate control feedback from angle (gyro integral) */
/* and angular velocity (gyro signal) */
/************************************************************************/
// The P-part is the P of the PID controller. That's the angle integrals (not rates).
for (axis
=PITCH
; axis
<=ROLL
; axis
++) {
if(looping
& (1<<(4+axis
))) {
PPart
[axis
] = 0;
} else { // TODO: Where do the 44000 come from???
PPart
[axis
] = angle
[axis
] * gyroIFactor
/ (44000 / CONTROL_SCALING
); // P-Part - Proportional to Integral
}
/*
* Now blend in the D-part - proportional to the Differential of the integral = the rate.
* Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING
* where pfactor is in [0..1].
*/
PDPart
[axis
] = PPart
[axis
] + (int32_t)((int32_t)rate_PID
[axis
] * gyroPFactor
/ (256L / CONTROL_SCALING
))
+ (differential
[axis
] * (int16_t)dynamicParams.
GyroD) / 16;
CHECK_MIN_MAX
(PDPart
[axis
], -SENSOR_LIMIT
, SENSOR_LIMIT
);
}
PDPartYaw
= (int32_t)(yawRate
* 2 * (int32_t)yawPFactor
) / (256L / CONTROL_SCALING
)
+ (int32_t)(yawAngle
* yawIFactor
) / (2 * (44000 / CONTROL_SCALING
));
// limit control feedback
CHECK_MIN_MAX
(PDPartYaw
, -SENSOR_LIMIT
, SENSOR_LIMIT
);
/*
* 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
}
/*
* Height control was here.
*/
if(throttleTerm
> staticParams.
MaxThrottle - 20) throttleTerm
= (staticParams.
MaxThrottle - 20);
throttleTerm
*= CONTROL_SCALING
;
/*
* Compose yaw term.
* The yaw term is limited: Absolute value is max. = the throttle term / 2.
* However, at low throttle the yaw term is limited to a fixed value,
* and at high throttle it is limited by the throttle reserve (the difference
* between current throttle and maximum throttle).
*/
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value
yawTerm
= PDPartYaw
- setPointYaw
* CONTROL_SCALING
;
// limit yawTerm
if(throttleTerm
> MIN_YAWGAS
) {
CHECK_MIN_MAX
(yawTerm
, - (throttleTerm
/ 2), (throttleTerm
/ 2));
} else {
CHECK_MIN_MAX
(yawTerm
, - (MIN_YAWGAS
/ 2), (MIN_YAWGAS
/ 2));
}
tmp_int
= staticParams.
MaxThrottle * CONTROL_SCALING
;
CHECK_MIN_MAX
(yawTerm
, -(tmp_int
- throttleTerm
), (tmp_int
- throttleTerm
));
tmp_int
= (int32_t)((int32_t)dynamicParams.
DynamicStability * (int32_t)(throttleTerm
+ abs(yawTerm
) / 2)) / 64;
for (axis
=PITCH
; axis
<=ROLL
; axis
++) {
/*
* Compose pitch and roll terms. This is finally where the sticks come into play.
*/
if(gyroIFactor
) {
// Integration mode: Integrate (angle - stick) = the difference between angle and stick pos.
// That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time.
// TODO: Find out why this seems to be proportional to stick position - not integrating it at all.
IPart
[axis
] += PPart
[axis
] - control
[axis
]; // Integrate difference between P part (the angle) and the stick pos.
} else {
// "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.
// To keep up with a full stick PDPart should be about 156...
IPart
[axis
] += PDPart
[axis
] - control
[axis
]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
}
// TODO: From which planet comes the 16000?
CHECK_MIN_MAX
(IPart
[axis
], -(CONTROL_SCALING
* 16000L), (CONTROL_SCALING
* 16000L));
// Add (P, D) parts minus stick pos. to the scaled-down I part.
term
[axis
] = PDPart
[axis
] - control
[axis
] + IPart
[axis
] / Ki
; // PID-controller for pitch
/*
* 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).
* TODO: Why a growing function of yaw?
*/
CHECK_MIN_MAX
(term
[axis
], -tmp_int
, tmp_int
);
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Universal Mixer
// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
for(i
= 0; i
< MAX_MOTORS
; i
++) {
int16_t tmp
;
if(Mixer.
Motor[i
][MIX_THROTTLE
] > 0) { // If a motor has a zero throttle mix, it is not considered.
tmp
= ((int32_t)throttleTerm
* Mixer.
Motor[i
][MIX_THROTTLE
]) / 64L;
tmp
+= ((int32_t)term
[PITCH
] * Mixer.
Motor[i
][MIX_PITCH
]) / 64L;
tmp
+= ((int32_t)term
[ROLL
] * Mixer.
Motor[i
][MIX_ROLL
]) / 64L;
tmp
+= ((int32_t)yawTerm
* Mixer.
Motor[i
][MIX_YAW
]) / 64L;
motorFilters
[i
] = motorFilter
(tmp
, motorFilters
[i
]);
tmp
= motorFilters
[i
] / CONTROL_SCALING
;
CHECK_MIN_MAX
(tmp
, staticParams.
MinThrottle, staticParams.
MaxThrottle);
Motor
[i
].
SetPoint = tmp
;
}
else Motor
[i
].
SetPoint = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugging
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!(--debugDataTimer
)) {
debugDataTimer
= 24; // update debug outputs at 488 / 24 = 20.3 Hz.
DebugOut.
Analog[0] = (10 * angle
[PITCH
]) / GYRO_DEG_FACTOR_PITCHROLL
; // in 0.1 deg
DebugOut.
Analog[1] = (10 * angle
[ROLL
]) / GYRO_DEG_FACTOR_PITCHROLL
; // in 0.1 deg
DebugOut.
Analog[2] = yawGyroHeading
/ GYRO_DEG_FACTOR_YAW
;
// DebugOut.Analog[9] = setPointYaw;
// DebugOut.Analog[10] = yawIFactor;
// DebugOut.Analog[11] = gyroIFactor;
// DebugOut.Analog[12] = RC_getVariable(0);
// DebugOut.Analog[13] = dynamicParams.UserParams[0];
// DebugOut.Analog[14] = RC_getVariable(4);
// DebugOut.Analog[15] = dynamicParams.UserParams[4];
/* DebugOut.Analog[11] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; */
// 12..15 are the controls.
// DebugOut.Analog[16] = pitchAxisAcc;
// DebugOut.Analog[17] = rollAxisAcc;
DebugOut.
Analog[18] = HIRES_GYRO_INTEGRATION_FACTOR
;
DebugOut.
Analog[19] = throttleTerm
;
DebugOut.
Analog[20] = term
[PITCH
];
DebugOut.
Analog[21] = term
[ROLL
];
DebugOut.
Analog[22] = yawTerm
;
DebugOut.
Analog[23] = PPart
[PITCH
]; //
DebugOut.
Analog[24] = IPart
[PITCH
] /Ki
; // meget meget lille.
DebugOut.
Analog[25] = PDPart
[PITCH
]; // omtrent lig ppart.
DebugOut.
Analog[26] = accNoisePeak
[PITCH
];
DebugOut.
Analog[27] = accNoisePeak
[ROLL
];
DebugOut.
Analog[30] = gyroNoisePeak
[PITCH
];
DebugOut.
Analog[31] = gyroNoisePeak
[ROLL
];
}
}