/*#######################################################################################
Flight Control
#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 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 "main.h"
#include "eeprom.h"
#include "timer0.h"
#include "_Settings.h"
#include "analog.h"
#include "fc.h"
#include "gps.h"
#include "uart.h"
#include "rc.h"
#include "twimaster.h"
#ifdef USE_MM3
#include "mm3.h"
#endif
#ifdef USE_CMPS03
#include "cmps03.h"
#endif
#include "led.h"
volatile uint16_t I2CTimeout
= 100;
// gyro readings
volatile int16_t Reading_GyroPitch
, Reading_GyroRoll
, Reading_GyroYaw
;
// gyro neutral readings
volatile int16_t AdNeutralPitch
= 0, AdNeutralRoll
= 0, AdNeutralYaw
= 0;
volatile int16_t StartNeutralRoll
= 0, StartNeutralPitch
= 0;
// mean accelerations
volatile int16_t Mean_AccPitch
, Mean_AccRoll
, Mean_AccTop
;
// neutral acceleration readings
volatile int16_t NeutralAccX
=0, NeutralAccY
=0;
volatile float NeutralAccZ
= 0;
// attitude gyro integrals
volatile int32_t IntegralPitch
= 0,IntegralPitch2
= 0;
volatile int32_t IntegralRoll
= 0,IntegralRoll2
= 0;
volatile int32_t IntegralYaw
= 0;
volatile int32_t Reading_IntegralGyroPitch
= 0, Reading_IntegralGyroPitch2
= 0;
volatile int32_t Reading_IntegralGyroRoll
= 0, Reading_IntegralGyroRoll2
= 0;
volatile int32_t Reading_IntegralGyroYaw
= 0;
volatile int32_t MeanIntegralPitch
;
volatile int32_t MeanIntegralRoll
;
// attitude acceleration integrals
volatile int32_t IntegralAccPitch
= 0, IntegralAccRoll
= 0;
volatile int32_t Reading_Integral_Top
= 0;
// compass course
volatile int16_t CompassHeading
= -1; // negative angle indicates invalid data.
volatile int16_t CompassCourse
= -1;
volatile int16_t CompassOffCourse
= 0;
// flags
uint8_t MotorsOn
= 0;
uint8_t EmergencyLanding
= 0;
int32_t TurnOver180Pitch
= 250000L, TurnOver180Roll
= 250000L;
float Gyro_P_Factor
;
float Gyro_I_Factor
;
volatile int16_t DiffPitch
, DiffRoll
;
int16_t Poti1
= 0, Poti2
= 0, Poti3
= 0, Poti4
= 0, Poti5
= 0, Poti6
= 0, Poti7
= 0, Poti8
= 0;
// setpoints for motors
volatile uint8_t Motor_Front
, Motor_Rear
, Motor_Right
, Motor_Left
;
// stick values derived by rc channels readings
int16_t StickPitch
= 0, StickRoll
= 0, StickYaw
= 0, StickThrust
= 0;
int16_t MaxStickPitch
= 0, MaxStickRoll
= 0;
// stick values derived by uart inputs
int16_t ExternStickPitch
= 0, ExternStickRoll
= 0, ExternStickYaw
= 0, ExternHeightValue
= -20;
int16_t ReadingHeight
= 0;
int16_t SetPointHeight
= 0;
int16_t AttitudeCorrectionRoll
= 0, AttitudeCorrectionPitch
= 0;
float Ki
= FACTOR_I
;
uint8_t Looping_Pitch
= 0, Looping_Roll
= 0;
uint8_t Looping_Left
= 0, Looping_Right
= 0, Looping_Down
= 0, Looping_Top
= 0;
fc_param_t FCParam
= {48,251,16,58,64,150,150,2,10,0,0,0,0,0,0,0,0,100,70,0,0,100};
/************************************************************************/
/* Creates numbeeps beeps at the speaker */
/************************************************************************/
void Beep
(uint8_t numbeeps
)
{
while(numbeeps
--)
{
if(MotorsOn
) return; //auf keinen Fall im Flug!
BeepTime
= 100; // 0.1 second
Delay_ms
(250); // blocks 250 ms as pause to next beep,
// this will block the flight control loop,
// therefore do not use this funktion if motors are running
}
}
/************************************************************************/
/* Neutral Readings */
/************************************************************************/
void SetNeutral
(void)
{
NeutralAccX
= 0;
NeutralAccY
= 0;
NeutralAccZ
= 0;
AdNeutralPitch
= 0;
AdNeutralRoll
= 0;
AdNeutralYaw
= 0;
FCParam.
Yaw_PosFeedback = 0;
FCParam.
Yaw_NegFeedback = 0;
CalibMean
();
Delay_ms_Mess
(100);
CalibMean
();
if((ParamSet.
GlobalConfig & CFG_HEIGHT_CONTROL
)) // Height Control activated?
{
if((ReadingAirPressure
> 950) || (ReadingAirPressure
< 750)) SearchAirPressureOffset
();
}
AdNeutralPitch
= AdValueGyrPitch
;
AdNeutralRoll
= AdValueGyrRoll
;
AdNeutralYaw
= AdValueGyrYaw
;
StartNeutralRoll
= AdNeutralRoll
;
StartNeutralPitch
= AdNeutralPitch
;
if(GetParamWord
(PID_ACC_PITCH
) > 1023)
{
NeutralAccY
= abs(Mean_AccRoll
) / ACC_AMPLIFY
;
NeutralAccX
= abs(Mean_AccPitch
) / ACC_AMPLIFY
;
NeutralAccZ
= Current_AccZ
;
}
else
{
NeutralAccX
= (int16_t)GetParamWord
(PID_ACC_PITCH
);
NeutralAccY
= (int16_t)GetParamWord
(PID_ACC_ROLL
);
NeutralAccZ
= (int16_t)GetParamWord
(PID_ACC_Z
);
}
Reading_IntegralGyroPitch
= 0;
Reading_IntegralGyroPitch2
= 0;
Reading_IntegralGyroRoll
= 0;
Reading_IntegralGyroRoll2
= 0;
Reading_IntegralGyroYaw
= 0;
Reading_GyroPitch
= 0;
Reading_GyroRoll
= 0;
Reading_GyroYaw
= 0;
StartAirPressure
= AirPressure
;
HeightD
= 0;
Reading_Integral_Top
= 0;
CompassCourse
= CompassHeading
;
BeepTime
= 50;
TurnOver180Pitch
= (int32_t) ParamSet.
AngleTurnOverPitch * 2500L;
TurnOver180Roll
= (int32_t) ParamSet.
AngleTurnOverRoll * 2500L;
ExternHeightValue
= 0;
GPS_Neutral
();
}
/************************************************************************/
/* Averaging Measurement Readings */
/************************************************************************/
void Mean
(void)
{
static int32_t tmpl
,tmpl2
;
// Get offset corrected gyro readings (~ to angular velocity)
Reading_GyroYaw
= AdNeutralYaw
- AdValueGyrYaw
;
Reading_GyroRoll
= AdValueGyrRoll
- AdNeutralRoll
;
Reading_GyroPitch
= AdValueGyrPitch
- AdNeutralPitch
;
DebugOut.
Analog[26] = Reading_GyroPitch
;
DebugOut.
Analog[28] = Reading_GyroRoll
;
// Acceleration Sensor
// sliding average sensor readings
Mean_AccPitch
= ((int32_t)Mean_AccPitch
* 1 + ((ACC_AMPLIFY
* (int32_t)AdValueAccPitch
))) / 2L;
Mean_AccRoll
= ((int32_t)Mean_AccRoll
* 1 + ((ACC_AMPLIFY
* (int32_t)AdValueAccRoll
))) / 2L;
Mean_AccTop
= ((int32_t)Mean_AccTop
* 1 + ((int32_t)AdValueAccTop
)) / 2L;
// sum sensor readings for later averaging
IntegralAccPitch
+= ACC_AMPLIFY
* AdValueAccPitch
;
IntegralAccRoll
+= ACC_AMPLIFY
* AdValueAccRoll
;
// Yaw
// calculate yaw gyro intergral (~ to rotation angle)
Reading_IntegralGyroYaw
+= Reading_GyroYaw
;
// Coupling fraction
if(!Looping_Pitch
&& !Looping_Roll
&& (ParamSet.
GlobalConfig & CFG_AXIS_COUPLING_ACTIVE
))
{
tmpl
= Reading_IntegralGyroPitch
/ 4096L;
tmpl
*= Reading_GyroYaw
;
tmpl
*= FCParam.
Yaw_PosFeedback; //125
tmpl
/= 2048L;
tmpl2
= Reading_IntegralGyroRoll
/ 4096L;
tmpl2
*= Reading_GyroYaw
;
tmpl2
*= FCParam.
Yaw_PosFeedback;
tmpl2
/= 2048L;
}
else tmpl
= tmpl2
= 0;
// Roll
Reading_GyroRoll
+= tmpl
;
Reading_GyroRoll
+= (tmpl2
* FCParam.
Yaw_NegFeedback) / 512L; //109
Reading_IntegralGyroRoll2
+= Reading_GyroRoll
;
Reading_IntegralGyroRoll
+= Reading_GyroRoll
- AttitudeCorrectionRoll
;
if(Reading_IntegralGyroRoll
> TurnOver180Roll
)
{
Reading_IntegralGyroRoll
= -(TurnOver180Roll
- 10000L);
Reading_IntegralGyroRoll2
= Reading_IntegralGyroRoll
;
}
if(Reading_IntegralGyroRoll
< -TurnOver180Roll
)
{
Reading_IntegralGyroRoll
= (TurnOver180Roll
- 10000L);
Reading_IntegralGyroRoll2
= Reading_IntegralGyroRoll
;
}
if(AdValueGyrRoll
< 15) Reading_GyroRoll
= -1000;
if(AdValueGyrRoll
< 7) Reading_GyroRoll
= -2000;
if(BoardRelease
== 10)
{
if(AdValueGyrRoll
> 1010) Reading_GyroRoll
= +1000;
if(AdValueGyrRoll
> 1017) Reading_GyroRoll
= +2000;
}
else
{
if(AdValueGyrRoll
> 2020) Reading_GyroRoll
= +1000;
if(AdValueGyrRoll
> 2034) Reading_GyroRoll
= +2000;
}
// Pitch
Reading_GyroPitch
-= tmpl2
;
Reading_GyroPitch
-= (tmpl
*FCParam.
Yaw_NegFeedback) / 512L;
Reading_IntegralGyroPitch2
+= Reading_GyroPitch
;
Reading_IntegralGyroPitch
+= Reading_GyroPitch
- AttitudeCorrectionPitch
;
if(Reading_IntegralGyroPitch
> TurnOver180Pitch
)
{
Reading_IntegralGyroPitch
= -(TurnOver180Pitch
- 10000L);
Reading_IntegralGyroPitch2
= Reading_IntegralGyroPitch
;
}
if(Reading_IntegralGyroPitch
< -TurnOver180Pitch
)
{
Reading_IntegralGyroPitch
= (TurnOver180Pitch
- 10000L);
Reading_IntegralGyroPitch2
= Reading_IntegralGyroPitch
;
}
if(AdValueGyrPitch
< 15) Reading_GyroPitch
= -1000;
if(AdValueGyrPitch
< 7) Reading_GyroPitch
= -2000;
if(BoardRelease
== 10)
{
if(AdValueGyrPitch
> 1010) Reading_GyroPitch
= +1000;
if(AdValueGyrPitch
> 1017) Reading_GyroPitch
= +2000;
}
else
{
if(AdValueGyrPitch
> 2020) Reading_GyroPitch
= +1000;
if(AdValueGyrPitch
> 2034) Reading_GyroPitch
= +2000;
}
// start ADC
ADC_Enable
();
IntegralYaw
= Reading_IntegralGyroYaw
;
IntegralPitch
= Reading_IntegralGyroPitch
;
IntegralRoll
= Reading_IntegralGyroRoll
;
IntegralPitch2
= Reading_IntegralGyroPitch2
;
IntegralRoll2
= Reading_IntegralGyroRoll2
;
if((ParamSet.
GlobalConfig & CFG_ROTARY_RATE_LIMITER
) && !Looping_Pitch
&& !Looping_Roll
)
{
if(Reading_GyroPitch
> 200) Reading_GyroPitch
+= 4 * (Reading_GyroPitch
- 200);
else if(Reading_GyroPitch
< -200) Reading_GyroPitch
+= 4 * (Reading_GyroPitch
+ 200);
if(Reading_GyroRoll
> 200) Reading_GyroRoll
+= 4 * (Reading_GyroRoll
- 200);
else if(Reading_GyroRoll
< -200) Reading_GyroRoll
+= 4 * (Reading_GyroRoll
+ 200);
}
}
/************************************************************************/
/* Averaging Measurement Readings for Calibration */
/************************************************************************/
void CalibMean
(void)
{
// stop ADC to avoid changing values during calculation
ADC_Disable
();
Reading_GyroPitch
= AdValueGyrPitch
;
Reading_GyroRoll
= AdValueGyrRoll
;
Reading_GyroYaw
= AdValueGyrYaw
;
Mean_AccPitch
= ACC_AMPLIFY
* (int32_t)AdValueAccPitch
;
Mean_AccRoll
= ACC_AMPLIFY
* (int32_t)AdValueAccRoll
;
Mean_AccTop
= (int32_t)AdValueAccTop
;
// start ADC (enables internal trigger so that the ISR in analog.c
// updates the readings once)
ADC_Enable
();
TurnOver180Pitch
= (int32_t) ParamSet.
AngleTurnOverPitch * 2500L;
TurnOver180Roll
= (int32_t) ParamSet.
AngleTurnOverRoll * 2500L;
}
/************************************************************************/
/* Transmit Motor Data via I2C */
/************************************************************************/
void SendMotorData
(void)
{
if(MOTOR_OFF
|| !MotorsOn
)
{
Motor_Rear
= 0;
Motor_Front
= 0;
Motor_Right
= 0;
Motor_Left
= 0;
if(MotorTest
[0]) Motor_Front
= MotorTest
[0];
if(MotorTest
[1]) Motor_Rear
= MotorTest
[1];
if(MotorTest
[2]) Motor_Left
= MotorTest
[2];
if(MotorTest
[3]) Motor_Right
= MotorTest
[3];
}
//DebugOut.Analog[12] = Motor_Front;
//DebugOut.Analog[13] = Motor_Rear;
//DebugOut.Analog[14] = Motor_Left;
//DebugOut.Analog[15] = Motor_Right;
//Start I2C Interrupt Mode
twi_state
= 0;
motor
= 0;
I2C_Start
();
}
/************************************************************************/
/* Maps the parameter to poti values */
/************************************************************************/
void ParameterMapping
(void)
{
if(RC_Quality
> 160) // do the mapping of RC-Potis only if the rc-signal is ok
// else the last updated values are used
{
//update poti values by rc-signals
#define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
CHK_POTI
(FCParam.
MaxHeight,ParamSet.
MaxHeight,0,255);
CHK_POTI
(FCParam.
Height_D,ParamSet.
Height_D,0,100);
CHK_POTI
(FCParam.
Height_P,ParamSet.
Height_P,0,100);
CHK_POTI
(FCParam.
Height_ACC_Effect,ParamSet.
Height_ACC_Effect,0,255);
CHK_POTI
(FCParam.
CompassYawEffect,ParamSet.
CompassYawEffect,0,255);
CHK_POTI
(FCParam.
Gyro_P,ParamSet.
Gyro_P,10,255);
CHK_POTI
(FCParam.
Gyro_I,ParamSet.
Gyro_I,0,255);
CHK_POTI
(FCParam.
I_Factor,ParamSet.
I_Factor,0,255);
CHK_POTI
(FCParam.
UserParam1,ParamSet.
UserParam1,0,255);
CHK_POTI
(FCParam.
UserParam2,ParamSet.
UserParam2,0,255);
CHK_POTI
(FCParam.
UserParam3,ParamSet.
UserParam3,0,255);
CHK_POTI
(FCParam.
UserParam4,ParamSet.
UserParam4,0,255);
CHK_POTI
(FCParam.
UserParam5,ParamSet.
UserParam5,0,255);
CHK_POTI
(FCParam.
UserParam6,ParamSet.
UserParam6,0,255);
CHK_POTI
(FCParam.
UserParam7,ParamSet.
UserParam7,0,255);
CHK_POTI
(FCParam.
UserParam8,ParamSet.
UserParam8,0,255);
CHK_POTI
(FCParam.
ServoPitchControl,ParamSet.
ServoPitchControl,0,255);
CHK_POTI
(FCParam.
LoopThrustLimit,ParamSet.
LoopThrustLimit,0,255);
CHK_POTI
(FCParam.
Yaw_PosFeedback,ParamSet.
Yaw_PosFeedback,0,255);
CHK_POTI
(FCParam.
Yaw_NegFeedback,ParamSet.
Yaw_NegFeedback,0,255);
CHK_POTI
(FCParam.
DynamicStability,ParamSet.
DynamicStability,0,255);
Ki
= (float) FCParam.
I_Factor * FACTOR_I
;
}
}
/************************************************************************/
/* MotorControl */
/************************************************************************/
void MotorControl
(void)
{
int16_t MotorValue
, pd_result
, h
, tmp_int
;
int16_t YawMixFraction
, ThrustMixFraction
;
static int32_t SumPitch
= 0, SumRoll
= 0;
static int32_t SetPointYaw
= 0;
static int32_t IntegralErrorPitch
= 0;
static int32_t IntegralErrorRoll
= 0;
static uint16_t RcLostTimer
;
static uint8_t delay_neutral
= 0, delay_startmotors
= 0, delay_stopmotors
= 0;
static uint16_t Model_Is_Flying
= 0;
static uint8_t HeightControlActive
= 0;
static int16_t HeightControlThrust
= 0;
static int8_t TimerDebugOut
= 0;
static uint16_t UpdateCompassCourse
= 0;
static int32_t CorrectionPitch
, CorrectionRoll
;
Mean
();
GRN_ON
;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// determine thrust value
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
ThrustMixFraction
= StickThrust
;
if(ThrustMixFraction
< 0) ThrustMixFraction
= 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// RC-signal is bad
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(RC_Quality
< 120) // the rc-frame signal is not reveived or noisy
{
if(!PcAccess
) // if also no PC-Access via UART
{
if(BeepModulation
== 0xFFFF)
{
BeepTime
= 15000; // 1.5 seconds
BeepModulation
= 0x0C00;
}
}
if(RcLostTimer
) RcLostTimer
--; // decremtent timer after rc sigal lost
else // rc lost countdown finished
{
MotorsOn
= 0; // stop all motors
EmergencyLanding
= 0; // emergency landing is over
}
ROT_ON
; // set red led
if(Model_Is_Flying
> 2000) // wahrscheinlich in der Luft --> langsam absenken
{
ThrustMixFraction
= ParamSet.
EmergencyThrust; // set emergency thrust
EmergencyLanding
= 1; // enable emergency landing
// set neutral rc inputs
PPM_diff
[ParamSet.
ChannelAssignment[CH_PITCH
]] = 0;
PPM_diff
[ParamSet.
ChannelAssignment[CH_ROLL
]] = 0;
PPM_diff
[ParamSet.
ChannelAssignment[CH_YAW
]] = 0;
PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]] = 0;
PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]] = 0;
PPM_in
[ParamSet.
ChannelAssignment[CH_YAW
]] = 0;
}
else MotorsOn
= 0; // switch of all motors
} // eof RC_Quality < 120
else
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// RC-signal is good
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(RC_Quality
> 150)
{
EmergencyLanding
= 0; // switch off emergency landing if RC-signal is okay
// reset emergency timer
RcLostTimer
= ParamSet.
EmergencyThrustDuration * 50;
if(ThrustMixFraction
> 40)
{
if(Model_Is_Flying
< 0xFFFF) Model_Is_Flying
++;
}
if((Model_Is_Flying
< 200) || (ThrustMixFraction
< 40))
{
SumPitch
= 0;
SumRoll
= 0;
Reading_IntegralGyroYaw
= 0;
}
if(Poti1
< PPM_in
[ParamSet.
ChannelAssignment[CH_POTI1
]] + 110) Poti1
++; else if(Poti1
> PPM_in
[ParamSet.
ChannelAssignment[CH_POTI1
]] + 110 && Poti1
) Poti1
--;
if(Poti2
< PPM_in
[ParamSet.
ChannelAssignment[CH_POTI2
]] + 110) Poti2
++; else if(Poti2
> PPM_in
[ParamSet.
ChannelAssignment[CH_POTI2
]] + 110 && Poti2
) Poti2
--;
if(Poti3
< PPM_in
[ParamSet.
ChannelAssignment[CH_POTI3
]] + 110) Poti3
++; else if(Poti3
> PPM_in
[ParamSet.
ChannelAssignment[CH_POTI3
]] + 110 && Poti3
) Poti3
--;
if(Poti4
< PPM_in
[ParamSet.
ChannelAssignment[CH_POTI4
]] + 110) Poti4
++; else if(Poti4
> PPM_in
[ParamSet.
ChannelAssignment[CH_POTI4
]] + 110 && Poti4
) Poti4
--;
//PPM24-Extension
if(Poti5
< PPM_in
[9] + 110) Poti5
++; else if(Poti5
> PPM_in
[9] + 110 && Poti5
) Poti5
--;
if(Poti6
< PPM_in
[10] + 110) Poti6
++; else if(Poti6
> PPM_in
[10] + 110 && Poti6
) Poti6
--;
if(Poti7
< PPM_in
[11] + 110) Poti7
++; else if(Poti7
> PPM_in
[11] + 110 && Poti7
) Poti7
--;
if(Poti8
< PPM_in
[12] + 110) Poti8
++; else if(Poti8
> PPM_in
[12] + 110 && Poti8
) Poti8
--;
//limit poti values
if(Poti1
< 0) Poti1
= 0; else if(Poti1
> 255) Poti1
= 255;
if(Poti2
< 0) Poti2
= 0; else if(Poti2
> 255) Poti2
= 255;
if(Poti3
< 0) Poti3
= 0; else if(Poti3
> 255) Poti3
= 255;
if(Poti4
< 0) Poti4
= 0; else if(Poti4
> 255) Poti4
= 255;
//PPM24-Extension
if(Poti5
< 0) Poti5
= 0; else if(Poti5
> 255) Poti5
= 255;
if(Poti6
< 0) Poti6
= 0; else if(Poti6
> 255) Poti6
= 255;
if(Poti7
< 0) Poti7
= 0; else if(Poti7
> 255) Poti7
= 255;
if(Poti8
< 0) Poti8
= 0; else if(Poti8
> 255) Poti8
= 255;
// if motors are off and the thrust stick is in the upper position
if((PPM_in
[ParamSet.
ChannelAssignment[CH_THRUST
]] > 80) && MotorsOn
== 0)
{
// and if the yaw stick is in the leftmost position
if(PPM_in
[ParamSet.
ChannelAssignment[CH_YAW
]] > 75)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// calibrate the neutral readings of all attitude sensors
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
{
if(++delay_neutral
> 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
{
delay_neutral
= 0;
GRN_OFF
;
Model_Is_Flying
= 0;
// check roll/pitch stick position
// if pitch stick is topmost or roll stick is leftmost --> change parameter setting
// according to roll/pitch stick position
if(PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]] > 70 || abs(PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]]) > 70)
{
uint8_t setting
= 1; // default
// _________
// |2 3 4|
// | |
// |1 5|
// | |
// |_________|
//
// roll stick leftmost and pitch stick centered --> setting 1
if(PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]] > 70 && PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]] < 70) setting
= 1;
// roll stick leftmost and pitch stick topmost --> setting 2
if(PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]] > 70 && PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]] > 70) setting
= 2;
// roll stick centered an pitch stick topmost --> setting 3
if(PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]] < 70 && PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]] > 70) setting
= 3;
// roll stick rightmost and pitch stick topmost --> setting 4
if(PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]] <-70 && PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]] > 70) setting
= 4;
// roll stick rightmost and pitch stick centered --> setting 5
if(PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]] <-70 && PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]] < 70) setting
= 5;
// update active parameter set in eeprom
SetActiveParamSet
(setting
);
}
ParamSet_ReadFromEEProm
(GetActiveParamSet
());
SetNeutral
();
Beep
(GetActiveParamSet
());
}
}
// and if the yaw stick is in the rightmost position
// save the ACC neutral setting to eeprom
else if(PPM_in
[ParamSet.
ChannelAssignment[CH_YAW
]] < -75)
{
if(++delay_neutral
> 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
{
delay_neutral
= 0;
GRN_OFF
;
SetParamWord
(PID_ACC_PITCH
, 0xFFFF); // make value invalid
Model_Is_Flying
= 0;
SetNeutral
();
// Save ACC neutral settings to eeprom
SetParamWord
(PID_ACC_PITCH
, (uint16_t)NeutralAccX
);
SetParamWord
(PID_ACC_ROLL
, (uint16_t)NeutralAccY
);
SetParamWord
(PID_ACC_Z
, (uint16_t)NeutralAccZ
);
Beep
(GetActiveParamSet
());
}
}
else delay_neutral
= 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// thrust stick is down
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in
[ParamSet.
ChannelAssignment[CH_THRUST
]] < -85)
{
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// and yaw stick is rightmost --> start motors
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in
[ParamSet.
ChannelAssignment[CH_YAW
]] < -75)
{
if(++delay_startmotors
> 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
{
delay_startmotors
= 200; // do not repeat if once executed
Model_Is_Flying
= 1;
MotorsOn
= 1;
SetPointYaw
= 0;
Reading_IntegralGyroYaw
= 0;
Reading_IntegralGyroPitch
= 0;
Reading_IntegralGyroRoll
= 0;
Reading_IntegralGyroPitch2
= IntegralPitch
;
Reading_IntegralGyroRoll2
= IntegralRoll
;
SumPitch
= 0;
SumRoll
= 0;
if(ParamSet.
GlobalConfig & CFG_GPS_ACTIVE
)
{
GPS_SetHomePosition
();
}
}
}
else delay_startmotors
= 0; // reset delay timer if sticks are not in this position
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// and yaw stick is leftmost --> stop motors
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in
[ParamSet.
ChannelAssignment[CH_YAW
]] > 75)
{
if(++delay_stopmotors
> 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
{
delay_stopmotors
= 200; // do not repeat if once executed
Model_Is_Flying
= 0;
MotorsOn
= 0;
if(ParamSet.
GlobalConfig & CFG_GPS_ACTIVE
)
{
GPS_ClearHomePosition
();
}
}
}
else delay_stopmotors
= 0; // reset delay timer if sticks are not in this position
}
// remapping of paameters only if the signal rc-sigbnal conditions are good
} // eof RC_Quality > 150
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// new values from RC
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!NewPpmData
-- || EmergencyLanding
) // NewData = 0 means new data from RC
{
int tmp_int
;
ParameterMapping
(); // remapping params (online poti replacement)
// calculate Stick inputs by rc channels (P) and changing of rc channels (D)
StickPitch
= (StickPitch
* 3 + PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]] * ParamSet.
Stick_P) / 4;
StickPitch
+= PPM_diff
[ParamSet.
ChannelAssignment[CH_PITCH
]] * ParamSet.
Stick_D;
StickRoll
= (StickRoll
* 3 + PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]] * ParamSet.
Stick_P) / 4;
StickRoll
+= PPM_diff
[ParamSet.
ChannelAssignment[CH_ROLL
]] * ParamSet.
Stick_D;
// direct mapping of yaw and thrust
StickYaw
= -PPM_in
[ParamSet.
ChannelAssignment[CH_YAW
]];
StickThrust
= PPM_in
[ParamSet.
ChannelAssignment[CH_THRUST
]] + 120;// shift to positive numbers
// update max stick positions for pitch and roll
if(abs(PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]]) > MaxStickPitch
)
MaxStickPitch
= abs(PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]]);
else MaxStickPitch
--;
if(abs(PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]]) > MaxStickRoll
)
MaxStickRoll
= abs(PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]]);
else MaxStickRoll
--;
// update gyro control loop factors
Gyro_P_Factor
= ((float) FCParam.
Gyro_P + 10.0) / 256.0;
Gyro_I_Factor
= ((float) FCParam.
Gyro_I) / 44000;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Digital Control via DubWise
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define KEY_VALUE (FCParam.UserParam1 * 4) // step width
if(DubWiseKeys
[1]) BeepTime
= 10;
if(DubWiseKeys
[1] & DUB_KEY_UP
) tmp_int
= KEY_VALUE
;
else if(DubWiseKeys
[1] & DUB_KEY_DOWN
) tmp_int
= -KEY_VALUE
;
else tmp_int
= 0;
ExternStickPitch
= (ExternStickPitch
* 7 + tmp_int
) / 8;
if(DubWiseKeys
[1] & DUB_KEY_LEFT
) tmp_int
= KEY_VALUE
;
else if(DubWiseKeys
[1] & DUB_KEY_RIGHT
) tmp_int
= -KEY_VALUE
;
else tmp_int
= 0;
ExternStickRoll
= (ExternStickRoll
* 7 + tmp_int
) / 8;
if(DubWiseKeys
[0] & 8) ExternStickYaw
= 50;else
if(DubWiseKeys
[0] & 4) ExternStickYaw
=-50;else ExternStickYaw
= 0;
if(DubWiseKeys
[0] & 2) ExternHeightValue
++;
if(DubWiseKeys
[0] & 16) ExternHeightValue
--;
StickPitch
+= ExternStickPitch
/ 8;
StickRoll
+= ExternStickRoll
/ 8;
StickYaw
+= ExternStickYaw
;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analog control via serial communication
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ExternControl.
Config & 0x01 && FCParam.
UserParam1 > 128)
{
StickPitch
+= (int16_t) ExternControl.
Pitch * (int16_t) ParamSet.
Stick_P;
StickRoll
+= (int16_t) ExternControl.
Roll * (int16_t) ParamSet.
Stick_P;
StickYaw
+= ExternControl.
Yaw;
ExternHeightValue
= (int16_t) ExternControl.
Height * (int16_t)ParamSet.
Height_Gain;
if(ExternControl.
Thrust < StickThrust
) StickThrust
= ExternControl.
Thrust;
}
// disable I part of gyro control feedback
if(ParamSet.
GlobalConfig & CFG_HEADING_HOLD
) Gyro_I_Factor
= 0;
// avoid negative scaling factors
if(Gyro_P_Factor
< 0) Gyro_P_Factor
= 0;
if(Gyro_I_Factor
< 0) Gyro_I_Factor
= 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]] > ParamSet.
LoopThreshold) && ParamSet.
LoopConfig & CFG_LOOP_LEFT
) Looping_Left
= 1;
else
{
if(Looping_Left
) // Hysteresis
{
if((PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]] < (ParamSet.
LoopThreshold - ParamSet.
LoopHysteresis))) Looping_Left
= 0;
}
}
if((PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]] < -ParamSet.
LoopThreshold) && ParamSet.
LoopConfig & CFG_LOOP_RIGHT
) Looping_Right
= 1;
else
{
if(Looping_Right
) // Hysteresis
{
if(PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]] > -(ParamSet.
LoopThreshold - ParamSet.
LoopHysteresis)) Looping_Right
= 0;
}
}
if((PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]] > ParamSet.
LoopThreshold) && ParamSet.
LoopConfig & CFG_LOOP_UP
) Looping_Top
= 1;
else
{
if(Looping_Top
) // Hysteresis
{
if((PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]] < (ParamSet.
LoopThreshold - ParamSet.
LoopHysteresis))) Looping_Top
= 0;
}
}
if((PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]] < -ParamSet.
LoopThreshold) && ParamSet.
LoopConfig & CFG_LOOP_DOWN
) Looping_Down
= 1;
else
{
if(Looping_Down
) // Hysteresis
{
if(PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]] > -(ParamSet.
LoopThreshold - ParamSet.
LoopHysteresis)) Looping_Down
= 0;
}
}
if(Looping_Left
|| Looping_Right
) Looping_Roll
= 1; else Looping_Roll
= 0;
if(Looping_Top
|| Looping_Down
) {Looping_Pitch
= 1; Looping_Roll
= 0; Looping_Left
= 0; Looping_Right
= 0;} else Looping_Pitch
= 0;
} // End of new RC-Values or Emergency Landing
if(Looping_Roll
) BeepTime
= 100;
if(Looping_Roll
|| Looping_Pitch
)
{
if(ThrustMixFraction
> ParamSet.
LoopThrustLimit) ThrustMixFraction
= ParamSet.
LoopThrustLimit;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ LED Control on J16/J17
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
LED1_Time
= FCParam.
UserParam7;
LED2_Time
= FCParam.
UserParam8;
LED_Update
();
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// in case of emergency landing
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// set all inputs to save values
if(EmergencyLanding
)
{
StickYaw
= 0;
StickPitch
= 0;
StickRoll
= 0;
Gyro_P_Factor
= 0.5;
Gyro_I_Factor
= 0.003;
Looping_Roll
= 0;
Looping_Pitch
= 0;
MaxStickPitch
= 0;
MaxStickRoll
= 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Trim Gyro-Integrals to ACC-Signals
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define BALANCE_NUMBER 256L
// sum for averaging
MeanIntegralPitch
+= IntegralPitch
;
MeanIntegralRoll
+= IntegralRoll
;
if(Looping_Pitch
|| Looping_Roll
) // if looping in any direction
{
// reset averaging for acc and gyro integral as well as gyro integral acc correction
MeasurementCounter
= 0;
IntegralAccPitch
= 0;
IntegralAccRoll
= 0;
MeanIntegralPitch
= 0;
MeanIntegralRoll
= 0;
Reading_IntegralGyroPitch2
= Reading_IntegralGyroPitch
;
Reading_IntegralGyroRoll2
= Reading_IntegralGyroRoll
;
AttitudeCorrectionPitch
= 0;
AttitudeCorrectionRoll
= 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!Looping_Pitch
&& !Looping_Roll
) // if not lopping in any direction
{
int32_t tmp_long
, tmp_long2
;
// determine the deviation of gyro integral from averaged acceleration sensor
tmp_long
= (int32_t)(IntegralPitch
/ ParamSet.
GyroAccFaktor - (int32_t)Mean_AccPitch
);
tmp_long
/= 16;
tmp_long2
= (int32_t)(IntegralRoll
/ ParamSet.
GyroAccFaktor - (int32_t)Mean_AccRoll
);
tmp_long2
/= 16;
if((MaxStickPitch
> 15) || (MaxStickRoll
> 15)) // reduce effect during stick commands
{
tmp_long
/= 3;
tmp_long2
/= 3;
}
if(abs(PPM_in
[ParamSet.
ChannelAssignment[CH_YAW
]]) > 25) // reduce further is yaw stick is active
{
tmp_long
/= 3;
tmp_long2
/= 3;
}
#define BALANCE 32
// limit correction effect
if(tmp_long
> BALANCE
) tmp_long
= BALANCE
;
if(tmp_long
< -BALANCE
) tmp_long
=-BALANCE
;
if(tmp_long2
> BALANCE
) tmp_long2
= BALANCE
;
if(tmp_long2
<-BALANCE
) tmp_long2
=-BALANCE
;
// correct current readings
Reading_IntegralGyroPitch
-= tmp_long
;
Reading_IntegralGyroRoll
-= tmp_long2
;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// MeasurementCounter is incremented in the isr of analog.c
if(MeasurementCounter
>= BALANCE_NUMBER
) // averaging number has reached
{
static int16_t cnt
= 0;
static int8_t last_n_p
, last_n_n
, last_r_p
, last_r_n
;
static int32_t MeanIntegralPitch_old
, MeanIntegralRoll_old
;
// if not lopping in any direction (this should be alwais the case,
// because the Measurement counter is reset to 0 if looping in any direction is active.)
if(!Looping_Pitch
&& !Looping_Roll
)
{
// Calculate mean value of the gyro integrals
MeanIntegralPitch
/= BALANCE_NUMBER
;
MeanIntegralRoll
/= BALANCE_NUMBER
;
// Calculate mean of the acceleration values
IntegralAccPitch
= (ParamSet.
GyroAccFaktor * IntegralAccPitch
) / BALANCE_NUMBER
;
IntegralAccRoll
= (ParamSet.
GyroAccFaktor * IntegralAccRoll
) / BALANCE_NUMBER
;
// Pitch ++++++++++++++++++++++++++++++++++++++++++++++++
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral
IntegralErrorPitch
= (int32_t)(MeanIntegralPitch
- (int32_t)IntegralAccPitch
);
CorrectionPitch
= IntegralErrorPitch
/ ParamSet.
GyroAccTrim;
AttitudeCorrectionPitch
= CorrectionPitch
/ BALANCE_NUMBER
;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral
IntegralErrorRoll
= (int32_t)(MeanIntegralRoll
- (int32_t)IntegralAccRoll
);
CorrectionRoll
= IntegralErrorRoll
/ ParamSet.
GyroAccTrim;
AttitudeCorrectionRoll
= CorrectionRoll
/ BALANCE_NUMBER
;
if((MaxStickPitch
> 15) || (MaxStickRoll
> 15) || (abs(PPM_in
[ParamSet.
ChannelAssignment[CH_YAW
]]) > 25))
{
AttitudeCorrectionPitch
/= 2;
AttitudeCorrectionRoll
/= 2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gyro-Drift ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor)
IntegralErrorPitch
= IntegralPitch2
- IntegralPitch
;
Reading_IntegralGyroPitch2
-= IntegralErrorPitch
;
// deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor)
IntegralErrorRoll
= IntegralRoll2
- IntegralRoll
;
Reading_IntegralGyroRoll2
-= IntegralErrorRoll
;
DebugOut.
Analog[17] = IntegralAccPitch
/ 26;
DebugOut.
Analog[18] = IntegralAccRoll
/ 26;
DebugOut.
Analog[19] = IntegralErrorPitch
;// / 26;
DebugOut.
Analog[20] = IntegralErrorRoll
;// / 26;
DebugOut.
Analog[21] = MeanIntegralPitch
/ 26;
DebugOut.
Analog[22] = MeanIntegralRoll
/ 26;
//DebugOut.Analog[28] = CorrectionPitch;
DebugOut.
Analog[29] = CorrectionRoll
;
DebugOut.
Analog[30] = AttitudeCorrectionRoll
* 10;
#define ERROR_LIMIT (BALANCE_NUMBER * 4)
#define ERROR_LIMIT2 (BALANCE_NUMBER * 16)
#define MOVEMENT_LIMIT 20000
// Pitch +++++++++++++++++++++++++++++++++++++++++++++++++
cnt
= 1;// + labs(IntegralErrorPitch) / 4096;
CorrectionPitch
= 0;
if(labs(MeanIntegralPitch_old
- MeanIntegralPitch
) < MOVEMENT_LIMIT
)
{
if(IntegralErrorPitch
> ERROR_LIMIT2
)
{
if(last_n_p
)
{
cnt
+= labs(IntegralErrorPitch
) / ERROR_LIMIT2
;
CorrectionPitch
= IntegralErrorPitch
/ 8;
if(CorrectionPitch
> 5000) CorrectionPitch
= 5000;
AttitudeCorrectionPitch
+= CorrectionPitch
/ BALANCE_NUMBER
;
}
else last_n_p
= 1;
}
else last_n_p
= 0;
if(IntegralErrorPitch
< -ERROR_LIMIT2
)
{
if(last_n_n
)
{
cnt
+= labs(IntegralErrorPitch
) / ERROR_LIMIT2
;
CorrectionPitch
= IntegralErrorPitch
/ 8;
if(CorrectionPitch
< -5000) CorrectionPitch
= -5000;
AttitudeCorrectionPitch
+= CorrectionPitch
/ BALANCE_NUMBER
;
}
else last_n_n
= 1;
}
else last_n_n
= 0;
}
else cnt
= 0;
if(cnt
> ParamSet.
DriftComp) cnt
= ParamSet.
DriftComp;
// correct Gyro Offsets
if(IntegralErrorPitch
> ERROR_LIMIT
) AdNeutralPitch
+= cnt
;
if(IntegralErrorPitch
< -ERROR_LIMIT
) AdNeutralPitch
-= cnt
;
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
cnt
= 1;// + labs(IntegralErrorPitch) / 4096;
CorrectionRoll
= 0;
if(labs(MeanIntegralRoll_old
- MeanIntegralRoll
) < MOVEMENT_LIMIT
)
{
if(IntegralErrorRoll
> ERROR_LIMIT2
)
{
if(last_r_p
)
{
cnt
+= labs(IntegralErrorRoll
) / ERROR_LIMIT2
;
CorrectionRoll
= IntegralErrorRoll
/ 8;
if(CorrectionRoll
> 5000) CorrectionRoll
= 5000;
AttitudeCorrectionRoll
+= CorrectionRoll
/ BALANCE_NUMBER
;
}
else last_r_p
= 1;
}
else last_r_p
= 0;
if(IntegralErrorRoll
< -ERROR_LIMIT2
)
{
if(last_r_n
)
{
cnt
+= labs(IntegralErrorRoll
) / ERROR_LIMIT2
;
CorrectionRoll
= IntegralErrorRoll
/ 8;
if(CorrectionRoll
< -5000) CorrectionRoll
= -5000;
AttitudeCorrectionRoll
+= CorrectionRoll
/ BALANCE_NUMBER
;
}
else last_r_n
= 1;
}
else last_r_n
= 0;
}
else cnt
= 0;
// correct Gyro Offsets
if(cnt
> ParamSet.
DriftComp) cnt
= ParamSet.
DriftComp;
if(IntegralErrorRoll
> ERROR_LIMIT
) AdNeutralRoll
+= cnt
;
if(IntegralErrorRoll
< -ERROR_LIMIT
) AdNeutralRoll
-= cnt
;
DebugOut.
Analog[27] = CorrectionRoll
;
DebugOut.
Analog[23] = AdNeutralPitch
;//10*(AdNeutralPitch - StartNeutralPitch);
DebugOut.
Analog[24] = 10*(AdNeutralRoll
- StartNeutralRoll
);
}
else // looping is active
{
AttitudeCorrectionRoll
= 0;
AttitudeCorrectionPitch
= 0;
}
// if Gyro_I_Faktor == 0 , for example at Heading Hold, ignore attitude correction
if(!Gyro_I_Factor
)
{
AttitudeCorrectionRoll
= 0;
AttitudeCorrectionPitch
= 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
MeanIntegralPitch_old
= MeanIntegralPitch
;
MeanIntegralRoll_old
= MeanIntegralRoll
;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
// reset variables used for averaging
IntegralAccPitch
= 0;
IntegralAccRoll
= 0;
MeanIntegralPitch
= 0;
MeanIntegralRoll
= 0;
MeasurementCounter
= 0;
} // end of averaging
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Yawing
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(abs(StickYaw
) > 20 ) // yaw stick is activated
{
if(!(ParamSet.
GlobalConfig & CFG_COMPASS_FIX
)) UpdateCompassCourse
= 1;
}
// exponential stick sensitivity in yawring rate
tmp_int
= (int32_t) ParamSet.
Yaw_P * ((int32_t)StickYaw
* abs(StickYaw
)) / 512L; // expo y = ax + bx²
tmp_int
+= (ParamSet.
Yaw_P * StickYaw
) / 4;
SetPointYaw
= tmp_int
;
// trimm drift of Reading_IntegralGyroYaw with SetPointYaw(StickYaw)
Reading_IntegralGyroYaw
-= tmp_int
;
// limit the effect
if(Reading_IntegralGyroYaw
> 50000) Reading_IntegralGyroYaw
= 50000;
if(Reading_IntegralGyroYaw
<-50000) Reading_IntegralGyroYaw
=-50000;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Compass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// compass code is used if Compass option or GPS option is selected
if((ParamSet.
GlobalConfig & CFG_COMPASS_ACTIVE
) || (ParamSet.
GlobalConfig & CFG_GPS_ACTIVE
))
{
static uint8_t updCompass
= 0;
if (!updCompass
--)
{
updCompass
= 49; // update only at 2ms*50 = 100ms (10Hz)
// get current compass heading (angule between MK head and magnetic north)
#ifdef USE_MM3
CompassHeading
= MM3_Heading
();
#endif
#ifdef USE_CMPS03
CompassHeading
= CMPS03_Heading
();
#endif
if (CompassHeading
< 0) CompassOffCourse
= 0; // disable gyro compass correction on bad compass data
else CompassOffCourse
= ((540 + CompassHeading
- CompassCourse
) % 360) - 180; // calc course deviation
}
if ((MaxStickPitch
< 75) && (MaxStickRoll
< 75))
{
if(UpdateCompassCourse
)
{
UpdateCompassCourse
= 0;
CompassCourse
= CompassHeading
;
CompassOffCourse
= 0;
}
Reading_IntegralGyroYaw
+= (CompassOffCourse
* FCParam.
CompassYawEffect) / 16;
}
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// GPS
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ParamSet.
GlobalConfig & CFG_GPS_ACTIVE
)
{
GPS_P_Factor
= FCParam.
UserParam5;
GPS_D_Factor
= FCParam.
UserParam6;
if(EmergencyLanding
) GPS_Main
(230); // enables Comming Home
else GPS_Main
(Poti3
);
}
else
{
GPS_Neutral
();
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!TimerDebugOut
--)
{
TimerDebugOut
= 24; // update debug outputs every 25*2ms = 50 ms (20Hz)
DebugOut.
Analog[0] = IntegralPitch
/ ParamSet.
GyroAccFaktor;
DebugOut.
Analog[1] = IntegralRoll
/ ParamSet.
GyroAccFaktor;
DebugOut.
Analog[2] = Mean_AccPitch
;
DebugOut.
Analog[3] = Mean_AccRoll
;
DebugOut.
Analog[4] = Reading_GyroYaw
;
DebugOut.
Analog[5] = ReadingHeight
;
DebugOut.
Analog[6] = (Reading_Integral_Top
/ 512);
DebugOut.
Analog[8] = CompassHeading
;
DebugOut.
Analog[9] = UBat
;
DebugOut.
Analog[10] = RC_Quality
;
//DebugOut.Analog[11] = RC_Quality;
DebugOut.
Analog[16] = Mean_AccTop
;
/* DebugOut.Analog[16] = motor_rx[0];
DebugOut.Analog[17] = motor_rx[1];
DebugOut.Analog[18] = motor_rx[2];
DebugOut.Analog[19] = motor_rx[3];
DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
DebugOut.Analog[20] /= 14;
DebugOut.Analog[21] = motor_rx[4];
DebugOut.Analog[22] = motor_rx[5];
DebugOut.Analog[23] = motor_rx[6];
DebugOut.Analog[24] = motor_rx[7];
DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
DebugOut.Analog[9] = Reading_GyroPitch;
DebugOut.Analog[9] = SetPointHeight;
DebugOut.Analog[10] = Reading_IntegralGyroYaw / 128;
DebugOut.Analog[10] = FCParam.Gyro_I;
DebugOut.Analog[10] = ParamSet.Gyro_I;
DebugOut.Analog[9] = CompassOffCourse;
DebugOut.Analog[10] = ThrustMixFraction;
DebugOut.Analog[3] = HeightD * 32;
DebugOut.Analog[4] = HeightControlThrust;
*/
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// calculate control feedback from angle (gyro integral) and agular velocity (gyro signal)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(Looping_Pitch
) Reading_GyroPitch
= Reading_GyroPitch
* Gyro_P_Factor
;
else Reading_GyroPitch
= IntegralPitch
* Gyro_I_Factor
+ Reading_GyroPitch
* Gyro_P_Factor
;
if(Looping_Roll
) Reading_GyroRoll
= Reading_GyroRoll
* Gyro_P_Factor
;
else Reading_GyroRoll
= IntegralRoll
* Gyro_I_Factor
+ Reading_GyroRoll
* Gyro_P_Factor
;
Reading_GyroYaw
= Reading_GyroYaw
* (2 * Gyro_P_Factor
) + IntegralYaw
* Gyro_I_Factor
/ 2;
DebugOut.
Analog[25] = IntegralRoll
* Gyro_I_Factor
;
DebugOut.
Analog[31] = StickRoll
;// / (26*Gyro_I_Factor);
DebugOut.
Analog[28] = Reading_GyroRoll
;
// limit control feedback
#define MAX_SENSOR 2048
if(Reading_GyroPitch
> MAX_SENSOR
) Reading_GyroPitch
= MAX_SENSOR
;
if(Reading_GyroPitch
< -MAX_SENSOR
) Reading_GyroPitch
= -MAX_SENSOR
;
if(Reading_GyroRoll
> MAX_SENSOR
) Reading_GyroRoll
= MAX_SENSOR
;
if(Reading_GyroRoll
< -MAX_SENSOR
) Reading_GyroRoll
= -MAX_SENSOR
;
if(Reading_GyroYaw
> MAX_SENSOR
) Reading_GyroYaw
= MAX_SENSOR
;
if(Reading_GyroYaw
< -MAX_SENSOR
) Reading_GyroYaw
= -MAX_SENSOR
;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Height Control
// The height control algorithm reduces the thrust but does not increase the thrust.
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// If height control is activated and no emergency landing is active
if((ParamSet.
GlobalConfig & CFG_HEIGHT_CONTROL
) && (!EmergencyLanding
) )
{
int tmp_int
;
// if height control is activated by an rc channel
if(ParamSet.
GlobalConfig & CFG_HEIGHT_SWITCH
)
{ // check if parameter is less than activation threshold
if(FCParam.
MaxHeight < 50)
{
SetPointHeight
= ReadingHeight
- 20; // update SetPoint with current reading
HeightControlActive
= 0; // disable height control
}
else HeightControlActive
= 1; // enable height control
}
else // no switchable height control
{
SetPointHeight
= ((int16_t) ExternHeightValue
+ (int16_t) FCParam.
MaxHeight) * (int16_t)ParamSet.
Height_Gain - 20;
HeightControlActive
= 1;
}
// get current height
h
= ReadingHeight
;
// if current height is above the setpoint reduce thrust
if((h
> SetPointHeight
) && HeightControlActive
)
{
// ThrustMixFraction - HightDeviation * P - HeightChange * D - ACCTop * DACC
// height difference -> P control part
h
= ((h
- SetPointHeight
) * (int16_t) FCParam.
Height_P) / 16;
h
= ThrustMixFraction
- h
; // reduce gas
// height gradient --> D control part
h
-= (HeightD
* FCParam.
Height_D) / 8; // D control part
// acceleration sensor effect
tmp_int
= ((Reading_Integral_Top
/ 512) * (int32_t) FCParam.
Height_ACC_Effect) / 32;
if(tmp_int
> 50) tmp_int
= 50;
if(tmp_int
< -50) tmp_int
= -50;
h
-= tmp_int
;
// update height control thrust
HeightControlThrust
= (HeightControlThrust
*15 + h
) / 16;
// limit thrust reduction
if(HeightControlThrust
< ParamSet.
Height_MinThrust)
{
if(ThrustMixFraction
>= ParamSet.
Height_MinThrust) HeightControlThrust
= ParamSet.
Height_MinThrust;
// allows landing also if thrust stick is reduced below min thrust on height control
if(ThrustMixFraction
< ParamSet.
Height_MinThrust) HeightControlThrust
= ThrustMixFraction
;
}
// limit thrust to stick setting
if(HeightControlThrust
> ThrustMixFraction
) HeightControlThrust
= ThrustMixFraction
;
ThrustMixFraction
= HeightControlThrust
;
}
}
// limit thrust to parameter setting
if(ThrustMixFraction
> ParamSet.
Trust_Max - 20) ThrustMixFraction
= ParamSet.
Trust_Max - 20;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Mixer and PI-Controller
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.
Analog[7] = ThrustMixFraction
;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Yaw-Fraction
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
YawMixFraction
= Reading_GyroYaw
- SetPointYaw
; // yaw controller
// limit YawMixFraction
if(YawMixFraction
> (ThrustMixFraction
/ 2)) YawMixFraction
= ThrustMixFraction
/ 2;
if(YawMixFraction
< -(ThrustMixFraction
/ 2)) YawMixFraction
= -(ThrustMixFraction
/ 2);
if(YawMixFraction
> ((ParamSet.
Trust_Max - ThrustMixFraction
))) YawMixFraction
= ((ParamSet.
Trust_Max - ThrustMixFraction
));
if(YawMixFraction
< -((ParamSet.
Trust_Max - ThrustMixFraction
))) YawMixFraction
= -((ParamSet.
Trust_Max - ThrustMixFraction
));
if(ThrustMixFraction
< 20) YawMixFraction
= 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Pitch-Axis
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffPitch
= Reading_GyroPitch
- (StickPitch
- GPS_Pitch
); // get difference
if(Gyro_I_Factor
) SumPitch
+= IntegralPitch
* Gyro_I_Factor
- (StickPitch
- GPS_Pitch
); // I-part for attitude control
else SumPitch
+= DiffPitch
; // I-part for head holding
if(SumPitch
> 16000) SumPitch
= 16000;
if(SumPitch
< -16000) SumPitch
= -16000;
pd_result
= DiffPitch
+ Ki
* SumPitch
; // PI-controller for pitch
tmp_int
= (int32_t)((int32_t)FCParam.
DynamicStability * (int32_t)(ThrustMixFraction
+ abs(YawMixFraction
)/2)) / 64;
if(pd_result
> tmp_int
) pd_result
= tmp_int
;
if(pd_result
< -tmp_int
) pd_result
= -tmp_int
;
// Motor Front
MotorValue
= ThrustMixFraction
+ pd_result
+ YawMixFraction
; // Mixer
if ((MotorValue
< 0)) MotorValue
= 0;
else if(MotorValue
> ParamSet.
Trust_Max) MotorValue
= ParamSet.
Trust_Max;
if (MotorValue
< ParamSet.
Trust_Min) MotorValue
= ParamSet.
Trust_Min;
Motor_Front
= MotorValue
;
// Motor Rear
MotorValue
= ThrustMixFraction
- pd_result
+ YawMixFraction
; // Mixer
if ((MotorValue
< 0)) MotorValue
= 0;
else if(MotorValue
> ParamSet.
Trust_Max) MotorValue
= ParamSet.
Trust_Max;
if (MotorValue
< ParamSet.
Trust_Min) MotorValue
= ParamSet.
Trust_Min;
Motor_Rear
= MotorValue
;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Roll-Axis
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffRoll
= Reading_GyroRoll
- (StickRoll
- GPS_Roll
); // get difference
if(Gyro_I_Factor
) SumRoll
+= IntegralRoll
* Gyro_I_Factor
- (StickRoll
- GPS_Roll
); // I-part for attitude control
else SumRoll
+= DiffRoll
; // I-part for head holding
if(SumRoll
> 16000) SumRoll
= 16000;
if(SumRoll
< -16000) SumRoll
= -16000;
pd_result
= DiffRoll
+ Ki
* SumRoll
; // PI-controller for roll
tmp_int
= (int32_t)((int32_t)FCParam.
DynamicStability * (int32_t)(ThrustMixFraction
+ abs(YawMixFraction
)/2)) / 64;
if(pd_result
> tmp_int
) pd_result
= tmp_int
;
if(pd_result
< -tmp_int
) pd_result
= -tmp_int
;
// Motor Left
MotorValue
= ThrustMixFraction
+ pd_result
- YawMixFraction
; // Mixer
if ((MotorValue
< 0)) MotorValue
= 0;
else if(MotorValue
> ParamSet.
Trust_Max) MotorValue
= ParamSet.
Trust_Max;
if (MotorValue
< ParamSet.
Trust_Min) MotorValue
= ParamSet.
Trust_Min;
Motor_Left
= MotorValue
;
// Motor Right
MotorValue
= ThrustMixFraction
- pd_result
- YawMixFraction
; // Mixer
if ((MotorValue
< 0)) MotorValue
= 0;
else if(MotorValue
> ParamSet.
Trust_Max) MotorValue
= ParamSet.
Trust_Max;
if (MotorValue
< ParamSet.
Trust_Min) MotorValue
= ParamSet.
Trust_Min;
Motor_Right
= MotorValue
;
}