Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1611 → Rev 1612

/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.c
0,0 → 1,15
#include "ADXRS610_FC2.0.h"
#include "configuration.h"
 
void gyro_calibrate(void) {}
 
void gyro_setDefaults(void) {
staticParams.GyroD = 5;
staticParams.DriftComp = 1;
staticParams.GyroAccFactor = 1;
 
// Not used.
staticParams.AngleTurnOverPitch = 78;
staticParams.AngleTurnOverRoll = 78;
}
 
/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.h
0,0 → 1,30
#ifndef _ADXRS610_H
#define _ADXRS610_H
 
#include "sensors.h"
/*
* Configuration for the ADXR610 gyros, as they are oriented and wired on the FC 2.0 ME board.
*/
 
#define GYRO_HW_NAME "ADXR"
 
#define GYRO_HW_FACTOR 1.2288f
 
/*
* Correction factor - determined experimentally: Hold the copter in the hand, and turn it 90 degrees.
* If AnglePitch or AngleRoll in debug in MK-Tool changes by x degrees, multiply the value here by x/90.
* If the hardware related contants are set correctly, flight should be OK without bothering to
* make any adjustments here. It is only for luxury.
*/
#define GYRO_PITCHROLL_CORRECTION 1.0f
 
/*
* Same for yaw.
*/
#define GYRO_YAW_CORRECTION 1.0f
 
/*
* Yaw axis is reverse on the ME.
*/
#define GYRO_REVERSE_YAW yes
#endif
/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c
0,0 → 1,82
//#include "ENC-03_FC1.3.h"
#include "printf_P.h"
#include "analog.h"
#include "twimaster.h"
#include "configuration.h"
#include "timer0.h"
 
#define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510
#define PITCHROLL_MAXLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 515
 
#define DAC_PITCH 0
#define DAC_ROLL 1
#define DAC_YAW 2
 
// void gyro_init(void) {}
void gyro_calibrate(void) {
uint8_t i, numberOfAxesInRange = 0;
uint16_t timeout;
// GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
timeout = SetDelay(2000);
 
for(i = 140; i != 0; i--) {
// If all 3 axis are in range, shorten the remaining number of iterations.
if(numberOfAxesInRange == 3 && i > 10) i = 9;
numberOfAxesInRange = 0;
if(rawPitchGyroSum < PITCHROLL_MINLIMIT) DACValues[DAC_PITCH]--;
else if(rawPitchGyroSum > PITCHROLL_MAXLIMIT) DACValues[DAC_PITCH]++;
else numberOfAxesInRange++;
if(rawRollGyroSum < PITCHROLL_MINLIMIT) DACValues[DAC_ROLL]--;
else if(rawRollGyroSum > PITCHROLL_MAXLIMIT) DACValues[DAC_ROLL]++;
else numberOfAxesInRange++;
if(rawYawGyroSum < GYRO_SUMMATION_FACTOR_YAW * 510) DACValues[DAC_YAW]--;
else if(rawYawGyroSum > GYRO_SUMMATION_FACTOR_YAW * 515) DACValues[DAC_YAW]++ ;
else numberOfAxesInRange++;
if(DACValues[DAC_PITCH] < 10) {
/* GyroDefectNick = 1; */ DACValues[DAC_PITCH] = 10;
} else if(DACValues[DAC_PITCH] > 245) {
/* GyroDefectNick = 1; */ DACValues[DAC_PITCH] = 245;
}
if(DACValues[DAC_ROLL] < 10) {
/* GyroDefectRoll = 1; */ DACValues[DAC_ROLL] = 10;
} else if(DACValues[DAC_ROLL] > 245) {
/* GyroDefectRoll = 1; */ DACValues[DAC_ROLL] = 245;
}
if(DACValues[DAC_YAW] < 10) {
/* GyroDefectYaw = 1; */ DACValues[DAC_YAW] = 10;
} else if(DACValues[DAC_YAW] > 245) {
/* GyroDefectYaw = 1; */ DACValues[DAC_YAW] = 245;
}
 
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission
// Wait for I2C to finish transmission.
while(twi_state) {
// Did it take too long?
if(CheckDelay(timeout)) {
printf("\r\n DAC or I2C Error1 check I2C, 3Vref, DAC, and BL-Ctrl");
break;
}
}
 
analog_start();
 
Delay_ms_Mess(i<10 ? 10 : 2);
}
Delay_ms_Mess(70);
}
 
void gyro_setDefaults(void) {
staticParams.GyroD = 3;
staticParams.DriftComp = 32;
staticParams.GyroAccFactor = 5;
 
// Not used.
staticParams.AngleTurnOverPitch = 85;
staticParams.AngleTurnOverRoll = 85;
}
/branches/dongfang_FC_rewrite/ENC-03_FC1.3.h
0,0 → 1,25
#ifndef _ENC03_FC13_H
#define _ENC03_FC13_H
 
#include "sensors.h"
/*
* Configuration for the ENC-03 gyros and oriented and wired on FC 1.3 (with DAC).
*/
 
#define GYRO_HW_NAME "ENC"
 
#define GYRO_HW_FACTOR 1.304f
 
/*
* Correction factor - determined experimentally: Hold the copter in the hand, and turn it 90 degrees.
* If AnglePitch or AngleRoll in debug in MK-Tool changes by x degrees, multiply the value here by x/90.
* If the hardware related contants are set correctly, flight should be OK without bothering to
* make any adjustments here. It is only for luxury.
*/
#define GYRO_PITCHROLL_CORRECTION 1.11f
 
/*
* Same for yaw.
*/
#define GYRO_YAW_CORRECTION 1.28f
#endif
/branches/dongfang_FC_rewrite/GPSControl.c
0,0 → 1,8
#include "GPSControl.h"
#include<inttypes.h>
 
int16_t GPSStickPitch, GPSStickRoll;
 
void GPS_setNeutral(void) {
GPSStickPitch = GPSStickRoll = 0;
}
/branches/dongfang_FC_rewrite/GPSControl.h
0,0 → 1,10
#ifndef _GPSCONTROL_H
#define _GPSCONTROL_H
 
#include<inttypes.h>
 
extern int16_t GPSStickPitch, GPSStickRoll;
 
void GPS_setNeutral(void);
 
#endif
/branches/dongfang_FC_rewrite/H_and_I_AxisCoupling.txt
0,0 → 1,76
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++
if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
{
tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
tmpl3 *= Parameter_AchsKopplung2; //65
tmpl3 /= 4096L;
tmpl4 = (MesswertNick * winkel_roll) / 2048L;
tmpl4 *= Parameter_AchsKopplung2; //65
tmpl4 /= 4096L;
KopplungsteilNickRoll = tmpl3;
KopplungsteilRollNick = tmpl4;
tmpl4 -= tmpl3;
ErsatzKompass += tmpl4;
if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen
 
tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
tmpl *= Parameter_AchsKopplung1; // 90
tmpl /= 4096L;
tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
tmpl2 *= Parameter_AchsKopplung1;
tmpl2 /= 4096L;
if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
//MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;
}
else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0;
 
TrimRoll = tmpl - tmpl2 / 100L;
TrimNick = -tmpl2 + tmpl / 100L;
 
 
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
Mess_IntegralRoll2 += MesswertRoll + TrimRoll;
Mess_IntegralRoll += MesswertRoll + TrimRoll - LageKorrekturRoll;
if(Mess_IntegralRoll > Umschlag180Roll)
{
Mess_IntegralRoll = -(Umschlag180Roll - 25000L);
Mess_IntegralRoll2 = Mess_IntegralRoll;
}
if(Mess_IntegralRoll <-Umschlag180Roll)
{
Mess_IntegralRoll = (Umschlag180Roll - 25000L);
Mess_IntegralRoll2 = Mess_IntegralRoll;
}
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
Mess_IntegralNick2 += MesswertNick + TrimNick;
Mess_IntegralNick += MesswertNick + TrimNick - LageKorrekturNick;
if(Mess_IntegralNick > Umschlag180Nick)
{
Mess_IntegralNick = -(Umschlag180Nick - 25000L);
Mess_IntegralNick2 = Mess_IntegralNick;
}
if(Mess_IntegralNick <-Umschlag180Nick)
{
Mess_IntegralNick = (Umschlag180Nick - 25000L);
Mess_IntegralNick2 = Mess_IntegralNick;
}
 
 
..
 
if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
 
 
#define TRIM_MAX 200
if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
 
MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
 
messwertNick er nu P-part....
 
/branches/dongfang_FC_rewrite/License.txt
0,0 → 1,52
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 nichtkommerziellen 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 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-profit 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, our webpage (http://www.MikroKopter.de) must be
// + clearly linked and named 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.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/branches/dongfang_FC_rewrite/analog.c
0,0 → 1,419
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include "analog.h"
 
#include "sensors.h"
 
// for Delay functions
#include "timer0.h"
 
// For DebugOut
#include "uart0.h"
 
// For reading and writing acc. meter offsets.
#include "eeprom.h"
 
/*
* Arrays could have been used arrays for the 2 * 3 axes, but despite some repetition,
* the code is easier to read without.
*
* For each A/D conversion cycle, each channel (eg. the yaw gyro, or the Z axis
* accelerometer) is sampled a number of times (see array channelsForStates), and
* the results for each channel are summed. Here are those for the gyros and the
* acc. meters. They are not zero-offset.
* They are exported in the analog.h file - but please do not use them! The only
* reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating
* the offsets with the DAC.
*/
volatile int16_t rawPitchGyroSum, rawRollGyroSum, rawYawGyroSum;
volatile int16_t pitchAxisAcc = 0, rollAxisAcc = 0, ZAxisAcc = 0;
volatile int16_t filteredPitchAxisAcc = 0, filteredRollAxisAcc = 0;
 
// that float one - "Top" - is missing.
 
/*
* These 4 exported variables are zero-offset. The "filtered" ones are
* (if configured to with the GYROS_SECONDORDERFILTER define) low pass
* filtered versions of the other 2.
* They are derived from the "raw" values above, by zero-offsetting.
*/
volatile int16_t hiResPitchGyro = 0, hiResRollGyro = 0;
volatile int16_t filteredHiResPitchGyro = 0, filteredHiResRollGyro = 0;
volatile int16_t pitchGyroD = 0, rollGyroD = 0;
volatile int16_t yawGyro = 0;
 
/*
* Offset values. These are the raw gyro and acc. meter sums when the copter is
* standing still. They are used for adjusting the gyro and acc. meter values
* to be zero when the copter stands still.
*/
volatile int16_t pitchOffset, rollOffset, yawOffset;
volatile int16_t pitchAxisAccOffset, rollAxisAccOffset, ZAxisAccOffset;
 
/*
* This allows some experimentation with the gyro filters.
* Should be replaced by #define's later on...
*/
volatile uint8_t GYROS_FIRSTORDERFILTER;
volatile uint8_t GYROS_SECONDORDERFILTER;
volatile uint8_t GYROS_DFILTER;
volatile uint8_t ACC_FILTER;
 
// Air pressure (no support right now).
// volatile int32_t AirPressure = 32000;
// volatile uint8_t average_pressure = 0;
// volatile int16_t StartAirPressure;
// volatile uint16_t ReadingAirPressure = 1023;
// volatile int16_t HeightD = 0;
 
/*
* Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt.
* That is divided by 3 below, for a final 10.34 per volt.
* So the initial value of 100 is for 9.7 volts.
*/
volatile int16_t UBat = 100;
 
/*
* Control and status.
*/
volatile uint16_t ADCycleCount = 0;
volatile uint8_t analogDataReady = 1;
 
/*
* Experiment: Measuring vibration-induced sensor noise.
*/
volatile uint16_t pitchGyroNoisePeak, rollGyroNoisePeak;
volatile uint16_t pitchAccNoisePeak, rollAccNoisePeak;
 
// ADC channels
#define AD_GYRO_YAW 0
#define AD_GYRO_ROLL 1
#define AD_GYRO_PITCH 2
#define AD_AIRPRESSURE 3
#define AD_UBAT 4
#define AD_ACC_Z 5
#define AD_ACC_ROLL 6
#define AD_ACC_PITCH 7
 
/*
* Table of AD converter inputs for each state.
* The number of samples summed for each channel is equal to
* the number of times the channel appears in the array.
* The max. number of samples that can be taken in 2 ms is:
* 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control
* loop needs a little time between reading AD values and
* re-enabling ADC, the real limit is (how much?) lower.
* The acc. sensor is sampled even if not used - or installed
* at all. The cost is not significant.
*/
 
const uint8_t channelsForStates[] PROGMEM = {
AD_GYRO_PITCH,
AD_GYRO_ROLL,
AD_GYRO_YAW,
 
AD_ACC_ROLL,
AD_ACC_PITCH,
AD_GYRO_PITCH,
AD_GYRO_ROLL,
 
AD_ACC_Z, // at 7, finish Z acc.
 
AD_GYRO_PITCH,
AD_GYRO_ROLL,
AD_GYRO_YAW, // at 10, finish yaw gyro
 
AD_ACC_PITCH, // at 11, finish pitch axis acc.
AD_ACC_ROLL, // at 12, finish roll axis acc.
 
AD_GYRO_PITCH, // at 13, finish pitch gyro
AD_GYRO_ROLL, // at 14, finish roll gyro
 
AD_UBAT // at 15, measure battery.
};
 
// Feature removed. Could be reintroduced later - but should work for all gyro types then.
// uint8_t GyroDefectPitch = 0, GyroDefectRoll = 0, GyroDefectYaw = 0;
 
void analog_init(void) {
uint8_t sreg = SREG;
// disable all interrupts before reconfiguration
cli();
 
//ADC0 ... ADC7 is connected to PortA pin 0 ... 7
DDRA = 0x00;
PORTA = 0x00;
// Digital Input Disable Register 0
// Disable digital input buffer for analog adc_channel pins
DIDR0 = 0xFF;
// external reference, adjust data to the right
ADMUX &= ~((1 << REFS1)|(1 << REFS0)|(1 << ADLAR));
// set muxer to ADC adc_channel 0 (0 to 7 is a valid choice)
ADMUX = (ADMUX & 0xE0) | AD_GYRO_PITCH;
//Set ADC Control and Status Register A
//Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz
ADCSRA = (0<<ADEN)|(0<<ADSC)|(0<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(0<<ADIE);
//Set ADC Control and Status Register B
//Trigger Source to Free Running Mode
ADCSRB &= ~((1 << ADTS2)|(1 << ADTS1)|(1 << ADTS0));
// Start AD conversion
analog_start();
// restore global interrupt flags
SREG = sreg;
}
 
void measureNoise(const int16_t sensor, volatile uint16_t* const noiseMeasurement, const uint8_t damping) {
if (sensor > (int16_t)(*noiseMeasurement)) {
*noiseMeasurement = sensor;
} else if (-sensor > (int16_t)(*noiseMeasurement)) {
*noiseMeasurement = -sensor;
} else if (*noiseMeasurement > damping) {
*noiseMeasurement -= damping;
} else {
*noiseMeasurement = 0;
}
}
 
/*****************************************************/
/* Interrupt Service Routine for ADC */
/*****************************************************/
// Runs at 312.5 kHz or 3.2 µs
// When all states are processed the interrupt is disabled
// and the update of further AD conversions is stopped.
 
ISR(ADC_vect) {
static uint8_t ad_channel = AD_GYRO_PITCH, state = 0;
static uint16_t sensorInputs[8] = {0,0,0,0,0,0,0,0};
 
uint8_t i;
 
// for various filters...
static int16_t pitchGyroFilter, rollGyroFilter, tempOffsetGyro;
sensorInputs[ad_channel] += ADC;
 
/*
* Actually we don't need this "switch". We could do all the sampling into the
* sensorInputs array first, and all the processing after the last sample.
*/
switch(state++) {
case 7: // Z acc
#ifdef ACC_REVERSE_ZAXIS
ZAxisAcc = -ZAxisAccOffset - sensorInputs[AD_ACC_Z];
#else
ZAxisAcc = sensorInputs[AD_ACC_Z] - ZAxisAccOffset;
#endif
break;
case 10: // yaw gyro
rawYawGyroSum = sensorInputs[AD_GYRO_YAW];
#ifdef GYRO_REVERSE_YAW
yawGyro = rawYawGyroSum - yawOffset;
#else
yawGyro = yawOffset - rawYawGyroSum; // negative is "default" (FC 1.0-1.3).
#endif
break;
case 11: // pitch axis acc.
#ifdef ACC_REVERSE_PITCHAXIS
pitchAxisAcc = -pitchAxisAccOffset - sensorInputs[AD_ACC_PITCH];
#else
pitchAxisAcc = sensorInputs[AD_ACC_PITCH] - pitchAxisAccOffset;
#endif
filteredPitchAxisAcc = (filteredPitchAxisAcc * (ACC_FILTER-1) + pitchAxisAcc) / ACC_FILTER;
 
measureNoise(pitchAxisAcc, &pitchAccNoisePeak, 1);
break;
case 12: // roll axis acc.
#ifdef ACC_REVERSE_ROLLAXIS
rollAxisAcc = sensorInputs[AD_ACC_ROLL] - rollAxisAccOffset;
#else
rollAxisAcc = -rollAxisAccOffset - sensorInputs[AD_ACC_ROLL];
#endif
filteredRollAxisAcc = (filteredRollAxisAcc * (ACC_FILTER-1) + rollAxisAcc) / ACC_FILTER;
measureNoise(rollAxisAcc, &rollAccNoisePeak, 1);
break;
case 13: // pitch gyro
rawPitchGyroSum = sensorInputs[AD_GYRO_PITCH];
// Filter already before offsetting. The offsetting resolution improvement obtained by divding by
// GYROS_FIRSTORDERFILTER _after_ offsetting is too small to be worth pursuing.
pitchGyroFilter = (pitchGyroFilter * (GYROS_FIRSTORDERFILTER-1) + rawPitchGyroSum * GYRO_FACTOR_PITCHROLL) / GYROS_FIRSTORDERFILTER;
// Offset to 0.
#ifdef GYROS_REVERSE_PITCH
tempOffsetGyro = pitchOffset - pitchGyroFilter;
#else
tempOffsetGyro = pitchGyroFilter - pitchOffset;
#endif
// Calculate the delta from last shot and filter it.
pitchGyroD = (pitchGyroD * (GYROS_DFILTER-1) + (tempOffsetGyro - hiResPitchGyro)) / GYROS_DFILTER;
// How we can overwrite the last value. This value is used for the D part of the PID controller.
hiResPitchGyro = tempOffsetGyro;
// Filter a little more. This value is used in integration to angles.
filteredHiResPitchGyro = (filteredHiResPitchGyro * (GYROS_SECONDORDERFILTER-1) + hiResPitchGyro) / GYROS_SECONDORDERFILTER;
measureNoise(hiResPitchGyro, &pitchGyroNoisePeak, GYRO_NOISE_MEASUREMENT_DAMPING);
break;
case 14: // Roll gyro. Works the same as pitch.
rawRollGyroSum = sensorInputs[AD_GYRO_ROLL];
rollGyroFilter = (rollGyroFilter * (GYROS_FIRSTORDERFILTER-1) + rawRollGyroSum * GYRO_FACTOR_PITCHROLL) / GYROS_FIRSTORDERFILTER;
#ifdef GYRO_REVERSE_ROLL
tempOffsetGyro = rollOffset - rollGyroFilter;
#else
tempOffsetGyro = rollGyroFilter - rollOffset;
#endif
rollGyroD = (rollGyroD * (GYROS_DFILTER-1) + (tempOffsetGyro - hiResRollGyro)) / GYROS_DFILTER;
hiResRollGyro = tempOffsetGyro;
filteredHiResRollGyro = (filteredHiResRollGyro * (GYROS_SECONDORDERFILTER-1) + hiResRollGyro) / GYROS_SECONDORDERFILTER;
measureNoise(hiResRollGyro, &rollGyroNoisePeak, GYRO_NOISE_MEASUREMENT_DAMPING);
break;
case 15:
// battery
UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4;
analogDataReady = 1; // mark
ADCycleCount++;
// Stop the sampling. Cycle is over.
state = 0;
for (i=0; i<8; i++) {
sensorInputs[i] = 0;
}
break;
default: {} // do nothing.
}
 
// set up for next state.
ad_channel = pgm_read_byte(&channelsForStates[state]);
// ad_channel = channelsForStates[state];
// set adc muxer to next ad_channel
ADMUX = (ADMUX & 0xE0) | ad_channel;
// after full cycle stop further interrupts
if(state) analog_start();
}
 
void analog_calibrate(void) {
#define GYRO_OFFSET_CYCLES 32
uint8_t i;
int32_t _pitchOffset = 0, _rollOffset = 0, _yawOffset = 0;
 
// Set the filters... to be removed again, once some good settings are found.
GYROS_FIRSTORDERFILTER = (dynamicParams.UserParams[4] & 0b00000011) + 1;
GYROS_SECONDORDERFILTER = ((dynamicParams.UserParams[4] & 0b00001100) >> 2) + 1;
GYROS_DFILTER = ((dynamicParams.UserParams[4] & 0b00110000) >> 4) + 1;
ACC_FILTER = ((dynamicParams.UserParams[4] & 0b11000000) >> 6) + 1;
 
pitchOffset = rollOffset = yawOffset = 0;
 
gyro_calibrate();
 
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
for(i=0; i < GYRO_OFFSET_CYCLES; i++) {
Delay_ms_Mess(10);
_pitchOffset += rawPitchGyroSum * GYRO_FACTOR_PITCHROLL;
_rollOffset += rawRollGyroSum * GYRO_FACTOR_PITCHROLL;
_yawOffset += rawYawGyroSum;
}
pitchOffset = (_pitchOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
rollOffset = (_rollOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
yawOffset = (_yawOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
filteredHiResPitchGyro = filteredHiResRollGyro = 0;
 
pitchAxisAccOffset = (int16_t)GetParamWord(PID_ACC_NICK);
rollAxisAccOffset = (int16_t)GetParamWord(PID_ACC_ROLL);
ZAxisAccOffset = (int16_t)GetParamWord(PID_ACC_TOP);
// Noise is relative to offset. So, reset noise measurements when
// changing offsets.
pitchGyroNoisePeak = rollGyroNoisePeak = 0;
 
// Setting offset values has an influence in the analog.c ISR
// Therefore run measurement for 100ms to achive stable readings
Delay_ms_Mess(100);
}
 
/*
* Find acc. offsets for a neutral reading, and write them to EEPROM.
* Does not (!} update the local variables. This must be done with a
* call to analog_calibrate() - this always (?) is done by the caller
* anyway. There would be nothing wrong with updating the variables
* directly from here, though.
*/
void analog_calibrateAcc(void) {
#define ACC_OFFSET_CYCLES 10
uint8_t i;
int32_t _pitchAxisOffset = 0, _rollAxisOffset = 0, _ZAxisOffset = 0;
pitchAxisAccOffset = rollAxisAccOffset = ZAxisAccOffset = 0;
 
for(i=0; i < ACC_OFFSET_CYCLES; i++) {
Delay_ms_Mess(10);
_pitchAxisOffset += pitchAxisAcc;
_rollAxisOffset += rollAxisAcc;
_ZAxisOffset += ZAxisAcc;
}
 
// Save ACC neutral settings to eeprom
SetParamWord(PID_ACC_NICK, (uint16_t)((_pitchAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES));
SetParamWord(PID_ACC_ROLL, (uint16_t)((_rollAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES));
SetParamWord(PID_ACC_TOP, (uint16_t)((_ZAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES));
 
// Noise is relative to offset. So, reset noise measurements when
// changing offsets.
pitchAccNoisePeak = rollAccNoisePeak = 0;
}
/branches/dongfang_FC_rewrite/analog.h
0,0 → 1,211
#ifndef _ANALOG_H
#define _ANALOG_H
#include <inttypes.h>
 
//#include "invenSense.h"
#include "ENC-03_FC1.3.h"
 
 
/*
* How much low pass filtering to apply for hiResPitchGyro and hiResRollGyro.
* 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
* Temporarily replaced by userparam-configurable variable.
*/
//#define GYROS_FIRSTORDERFILTER 2
 
/*
* How much low pass filtering to apply for filteredHiResPitchGyro and filteredHiResRollGyro.
* 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
* Temporarily replaced by userparam-configurable variable.
*/
//#define GYROS_SECONDORDERFILTER 2
 
// Temporarily replaced by userparam-configurable variable.
//#define ACC_FILTER 4
 
/*
About setting constants right for different gyros:
Main parameters are positive directions and voltage/angular speed gain.
The "Positive direction" is the rotation direction around an axis where
the corresponding gyro gives a voltage > the no-rotation voltage.
A gyro is considered, in this code, to be "forward" if its positive
direction is the same as in FC1.0/1.1/1.2/1.3, and reverse otherwise.
Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and
GYRO_REVERSE_PITCH #define's if the respective gyros are reverse.
Setting gyro gain correctly: All sensor measurements in analog.c take
place in a cycle, each cycle comprising all sensors. Some sensors are
sampled more than ones, and the results added. The pitch and roll gyros
are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74
code.
In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0)
or 4 (other versions), offset to zero, low pass filtered and then assigned
to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or
roll.
So:
HiResXXXX = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
where V is 2 for FC1.0 and 4 for all others.
Assuming constant ADCValue, in the H&I code:
HiResXXXX = I * ADCValue.
 
where I is 8 for FC1.0 and 16 for all others.
 
The relation between rotation rate and ADCValue:
ADCValue [units] =
rotational speed [deg/s] *
gyro sensitivity [V / deg/s] *
amplifier gain [units] *
1024 [units] /
3V full range [V]
 
or: H is the number of steps the ADC value changes with,
for a 1 deg/s change in rotational velocity:
H = ADCValue [units] / rotation rate [deg/s] =
gyro sensitivity [V / deg/s] *
amplifier gain [units] *
1024 [units] /
3V full range [V]
 
Examples:
FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
(only about half as sensitive as V1.3. But it will take about twice the
rotation rate!)
 
All together: HiResXXXX = I * H * rotation rate [units / (deg/s)].
*/
 
/*
* A factor that the raw gyro values are multiplied by,
* before being zero-offset, filtered and passed to the attitude module.
* A value of 1 would cause a little bit of loss of precision in the
* filtering (on the other hand the values are so noisy in flight that
* it will not really matter - but when testing on the desk it might be
* noticeable). 4 is fine for the default filtering.
*/
#define GYRO_FACTOR_PITCHROLL 4
 
/*
* How many samples are summed in one ADC loop, for pitch&roll and yaw,
* respectively. This is = the number of occurences of each channel in the
* channelsForStates array in analog.c.
*/
#define GYRO_SUMMATION_FACTOR_PITCHROLL 4
#define GYRO_SUMMATION_FACTOR_YAW 2
 
/*
Integration:
The HiResXXXX values are divided by 8 (in H&I firmware) before integration.
In the Killagreg rewrite of the H&I firmware, the factor 8 is called
HIRES_GYRO_AMPLIFY. In this code, it is called HIRES_GYRO_INTEGRATION_FACTOR,
and care has been taken that all other constants (gyro to degree factor, and
180 degree flip-over detection limits) are corrected to it. Because the
division by the constant takes place in the flight attitude code, the
constant is there.
 
The control loop executes every 2ms, and for each iteration
HiResXXXX is added to gyroIntegralXXXX.
Assuming a constant rotation rate v and an initial gyroIntegralXXXX (for this
explanation), we get:
gyroIntegralXXXX =
N * HiResXXXX / HIRES_GYRO_INTEGRATION_FACTOR =
N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR
where N is the number of summations; N = t/2ms.
 
For one degree of rotation: t*v = 1:
 
gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR.
 
This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor.
 
Examples:
FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304
FC2.0: I=2, H=2.048, HIRES_GYRO_INTEGRATION_FACTOR=13 --> integralDegreeFactor = 1260
My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365
*/
 
/*
* The value of hiResXXXX for one deg/s = The hardware factor H * the number of samples * multiplier factor.
* Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
*/
#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL)
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW)
 
/*
* This value is subtracted from the gyro noise measurement in each iteration,
* making it return towards zero.
*/
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
 
/*
* The values that this module outputs
*/
extern volatile int16_t hiResPitchGyro, hiResRollGyro;
extern volatile int16_t filteredHiResPitchGyro, filteredHiResRollGyro;
extern volatile int16_t pitchGyroD, rollGyroD;
extern volatile uint16_t ADCycleCount;
extern volatile int16_t UBat;
extern volatile int16_t yawGyro;
 
/*
* This is not really for external use - but the ENC-03 gyro modules needs it.
*/
extern volatile int16_t rawPitchGyroSum, rawRollGyroSum, rawYawGyroSum;
 
/*
* The acceleration values that this module outputs
*/
extern volatile int16_t pitchAxisAcc, rollAxisAcc, ZAxisAcc;
extern volatile int16_t filteredPitchAxisAcc, filteredRollAxisAcc;
 
// Only for debugging! Not to be exported! Remove when finished.
// extern volatile int16_t pitchAxisAccOffset, rollAxisAccOffset, ZAxisAccOffset;
 
// Air pressure measurement not supported right now.
// extern volatile int32_t AirPressure;
// extern volatile int16_t HeightD;
// extern volatile uint16_t ReadingAirPressure;
// extern volatile int16_t StartAirPressure;
// extern uint8_t PressureSensorOffset;
// extern int8_t ExpandBaro;
 
/*
* Flag: Interrupt handler has done all A/D conversion and processing.
*/
extern volatile uint8_t analogDataReady;
 
// Diagnostics: Gyro noise level because of motor vibrations. The variables
// only really reflect the noise level when the copter stands still but with
// its motors running.
extern volatile uint16_t pitchGyroNoisePeak, rollGyroNoisePeak;
extern volatile uint16_t pitchAccNoisePeak, rollAccNoisePeak;
 
// void SearchAirPressureOffset(void);
 
void analog_init(void);
 
// clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
#define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE)))
 
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
#define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
 
/*
* "Warm" calibration: Zero-offset gyros.
*/
void analog_calibrate(void);
 
/*
* "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM.
*/
void analog_calibrateAcc(void);
#endif //_ANALOG_H
/branches/dongfang_FC_rewrite/attitude.c
0,0 → 1,515
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
/************************************************************************/
/* Flight Attitude */
/************************************************************************/
 
#include <stdlib.h>
#include <avr/io.h>
 
#include "attitude.h"
#include "dongfangMath.h"
 
// where our main data flow comes from.
#include "analog.h"
 
#include "configuration.h"
 
// Some calculations are performed depending on some stick related things.
#include "controlMixer.h"
 
// For Servo_On / Off
// #include "timer2.h"
 
#ifdef USE_MK3MAG
#include "mk3mag.h"
#include "gps.h"
#endif
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
 
/*
* Gyro readings, as read from the analog module. It would have been nice to flow
* them around between the different calculations as a struct or array (doing
* things functionally without side effects) but this is shorter and probably
* faster too.
* The variables are overwritten at each attitude calculation invocation - the values
* are not preserved or reused.
*/
int16_t pitchRate, rollRate, yawRate;
 
// With different (less) filtering
int16_t pitchRate_PID, rollRate_PID;
int16_t pitchDifferential, rollDifferential;
 
/*
* Gyro readings, after performing "axis coupling" - that is, the transfomation
* of rotation rates from the airframe-local coordinate system to a ground-fixed
* coordinate system. If axis copling is disabled, the gyro readings will be
* copied into these directly.
* These are global for the same pragmatic reason as with the gyro readings.
* The variables are overwritten at each attitude calculation invocation - the values
* are not preserved or reused.
*/
int16_t ACPitchRate, ACRollRate, ACYawRate;
 
/*
* Gyro integrals. These are the rotation angles of the airframe compared to the
* horizontal plane, yaw relative to yaw at start.
*/
int32_t pitchAngle, rollAngle, yawAngle;
 
int readingHeight = 0;
 
// compass course
int16_t compassHeading = -1; // negative angle indicates invalid data.
int16_t compassCourse = -1;
int16_t compassOffCourse = 0;
uint16_t updateCompassCourse = 0;
uint8_t compassCalState = 0;
 
// uint8_t FunnelCourse = 0;
uint16_t badCompassHeading = 500;
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass
 
int32_t turnOver180 = GYRO_DEG_FACTOR_PITCHROLL * 180L;
int32_t turnOver360 = GYRO_DEG_FACTOR_PITCHROLL * 360L;
 
int32_t pitchCorrectionSum = 0, rollCorrectionSum = 0;
 
/*
* Experiment: Compensating for dynamic-induced gyro biasing.
*/
int16_t dynamicOffsetPitch = 0, dynamicOffsetRoll = 0, dynamicOffsetYaw = 0;
// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;
// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;
// int16_t dynamicCalCount;
 
/************************************************************************
* Set inclination angles from the acc. sensor data.
* If acc. sensors are not used, set to zero.
* TODO: One could use inverse sine to calculate the angles more
* accurately, but sinc: 1) the angles are rather at times when it
* makes sense to set the integrals (standing on ground, or flying at
* constant speed, and 2) at small angles a, sin(a) ~= constant * a,
* it is hardly worth the trouble.
************************************************************************/
 
int32_t getPitchAngleEstimateFromAcc(void) {
return GYRO_ACC_FACTOR * (int32_t)filteredPitchAxisAcc;
}
 
int32_t getRollAngleEstimateFromAcc(void) {
return GYRO_ACC_FACTOR * (int32_t)filteredRollAxisAcc;
}
 
void setStaticAttitudeAngles(void) {
#ifdef ATTITUDE_USE_ACC_SENSORS
pitchAngle = getPitchAngleEstimateFromAcc();
rollAngle = getRollAngleEstimateFromAcc();
#else
pitchAngle = 0;
rollAngle = 0;
#endif
}
 
/************************************************************************
* Neutral Readings
************************************************************************/
void attitude_setNeutral(void) {
// Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0;
 
dynamicOffsetPitch = dynamicOffsetRoll = 0;
// Calibrate hardware.
analog_calibrate();
 
// reset gyro readings
pitchRate = rollRate = yawRate = 0;
 
// reset gyro integrals to acc guessing
setStaticAttitudeAngles();
yawAngle = 0;
 
// update compass course to current heading
compassCourse = compassHeading;
// Inititialize YawGyroIntegral value with current compass heading
yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW;
 
// Servo_On(); //enable servo output
}
 
/************************************************************************
* Get sensor data from the analog module, and release the ADC
* TODO: Ultimately, the analog module could do this (instead of dumping
* the values into variables).
*************************************************************************/
void getAnalogData(void) {
// For the differential calculation. Diff. is not supported right now.
// int16_t d2Pitch, d2Roll;
pitchRate_PID = (hiResPitchGyro + dynamicOffsetPitch) / HIRES_GYRO_INTEGRATION_FACTOR;
pitchRate = (filteredHiResPitchGyro + dynamicOffsetPitch) / HIRES_GYRO_INTEGRATION_FACTOR;
pitchDifferential = pitchGyroD;
 
rollRate_PID = (hiResRollGyro + dynamicOffsetRoll) / HIRES_GYRO_INTEGRATION_FACTOR;
rollRate = (filteredHiResRollGyro + dynamicOffsetRoll) / HIRES_GYRO_INTEGRATION_FACTOR;
rollDifferential = rollGyroD;
 
yawRate = yawGyro + dynamicOffsetYaw;
 
// We are done reading variables from the analog module. Interrupt-driven sensor reading may restart.
// TODO: Is that not a little early to measure for next control invocation?
analogDataReady = 0;
analog_start();
}
 
/************************************************************************
* Axis coupling, H&I Style
* Currently not working (and there is a bug in it,
* which causes unstable flight in heading-hold mode).
************************************************************************/
void H_and_I_axisCoupling(void) {
int32_t tmpl = 0, tmpl2 = 0, tmp13 = 0, tmp14 = 0;
int16_t CouplingNickRoll = 0, CouplingRollNick = 0;
 
tmp13 = (rollRate * pitchAngle) / 2048L;
tmp13 *= dynamicParams.AxisCoupling2; // 65
tmp13 /= 4096L;
CouplingNickRoll = tmp13;
tmp14 = (pitchRate * rollAngle) / 2048L;
tmp14 *= dynamicParams.AxisCoupling2; // 65
tmp14 /= 4096L;
CouplingRollNick = tmp14;
tmp14 -= tmp13;
 
ACYawRate = yawRate + tmp14;
/*
if(!dynamicParams.AxisCouplingYawCorrection) ACYawRate = yawRate - tmp14 / 2; // force yaw
else ACYawRate
*/
tmpl = ((yawRate + tmp14) * pitchAngle) / 2048L;
tmpl *= dynamicParams.AxisCoupling1;
tmpl /= 4096L;
tmpl2 = ((yawRate + tmp14) * rollAngle) / 2048L;
tmpl2 *= dynamicParams.AxisCoupling1;
tmpl2 /= 4096L;
 
// if(abs(yawRate > 64)) {
// if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1;
// }
ACPitchRate = pitchRate - tmpl2 + tmpl / 100L;
ACRollRate = rollRate + tmpl - tmpl2 / 100L;
}
 
/*
* This is the standard flight-style coordinate system transformation
* (from airframe-local axes to a ground-based system). For some reason
* the MK uses a left-hand coordinate system. The tranformation has been
* changed accordingly.
*/
void trigAxisCoupling(void) {
int16_t cospitch = int_cos(pitchAngle);
int16_t cosroll = int_cos(rollAngle);
int16_t sinroll = int_sin(rollAngle);
int16_t tanpitch = int_tan(pitchAngle);
#define ANTIOVF 1024
ACPitchRate = ((int32_t)pitchRate * cosroll + (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR;
ACRollRate = rollRate + (((int32_t)pitchRate * sinroll / ANTIOVF * tanpitch - (int32_t)yawRate * int_cos(rollAngle) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR));
ACYawRate = (-(int32_t)pitchRate * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch;
}
 
void integrate(void) {
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
if(!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) {
// The rotary rate limiter bit is abused for selecting axis coupling algorithm instead.
if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER)
trigAxisCoupling();
else
H_and_I_axisCoupling();
} else {
ACPitchRate = pitchRate;
ACRollRate = rollRate;
ACYawRate = yawRate;
}
 
DebugOut.Analog[3] = pitchRate;
DebugOut.Analog[3 + 3] = ACPitchRate;
DebugOut.Analog[4] = rollRate;
DebugOut.Analog[4 + 3] = ACRollRate;
DebugOut.Analog[5] = yawRate;
DebugOut.Analog[5 + 3] = ACYawRate;
 
/*
DebugOut.Analog[9] = int_cos(pitchAngle);
DebugOut.Analog[10] = int_sin(pitchAngle);
DebugOut.Analog[11] = int_tan(pitchAngle);
*/
 
/*
* Yaw
* Calculate yaw gyro integral (~ to rotation angle)
* Limit yawGyroHeading proportional to 0 deg to 360 deg
*/
yawGyroHeading += ACYawRate;
yawAngle += ACYawRate;
if(yawGyroHeading >= (360L * GYRO_DEG_FACTOR_YAW)) yawGyroHeading -= 360L * GYRO_DEG_FACTOR_YAW; // 360 deg. wrap
if(yawGyroHeading < 0) yawGyroHeading += 360L * GYRO_DEG_FACTOR_YAW;
 
/*
* Pitch axis integration and range boundary wrap.
*/
pitchAngle += ACPitchRate;
if(pitchAngle > turnOver180) {
pitchAngle -= turnOver360;
} else if (pitchAngle <= -turnOver180) {
pitchAngle += turnOver360;
}
/*
* Pitch axis integration and range boundary wrap.
*/
rollAngle += ACRollRate;
if(rollAngle > turnOver180) {
rollAngle -= turnOver360;
} else if (rollAngle <= -turnOver180) {
rollAngle += turnOver360;
}
}
 
/************************************************************************
* A kind of 0'th order integral correction, that corrects the integrals
* directly. This is the "gyroAccFactor" stuff in the original code.
* There is (there) also what I would call a "minus 1st order correction"
* - it corrects the differential of the integral = the gyro offsets.
* That should only be necessary with drifty gyros like ENC-03.
************************************************************************/
void correctIntegralsByAcc0thOrder(void) {
// TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
// are less than ....., or reintroduce Kalman.
// Well actually the Z axis acc. check is not so silly.
if(!looping && //((ZAxisAcc >= -4) || (MKFlags & MKFLAG_MOTOR_RUN))) { // if not looping in any direction
ZAxisAcc >= -dynamicParams.UserParams[7] && ZAxisAcc <= dynamicParams.UserParams[7]) {
DebugOut.Digital[0] = 1;
uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
uint8_t debugFullWeight = 1;
int32_t accDerivedPitch = getPitchAngleEstimateFromAcc();
int32_t accDerivedRoll = getRollAngleEstimateFromAcc();
if((maxControlPitch > 64) || (maxControlRoll > 64)) { // reduce effect during stick commands
permilleAcc /= 2;
debugFullWeight = 0;
}
if(abs(controlYaw) > 25) { // reduce further if yaw stick is active
permilleAcc /= 2;
debugFullWeight = 0;
}
 
/*
* Add to each sum: The amount by which the angle is changed just below.
*/
pitchCorrectionSum += permilleAcc * (accDerivedPitch - pitchAngle);
rollCorrectionSum += permilleAcc * (accDerivedRoll - rollAngle);
// There should not be a risk of overflow here, since the integrals do not exceed a few 100000.
pitchAngle = ((int32_t)(1000 - permilleAcc) * pitchAngle + (int32_t)permilleAcc * accDerivedPitch) / 1000L;
rollAngle = ((int32_t)(1000 - permilleAcc) * rollAngle + (int32_t)permilleAcc * accDerivedRoll) / 1000L;
DebugOut.Digital[1] = debugFullWeight;
} else {
DebugOut.Digital[0] = 0;
}
}
 
/************************************************************************
* This is an attempt to correct not the error in the angle integrals
* (that happens in correctIntegralsByAcc0thOrder above) but rather the
* cause of it: Gyro drift, vibration and rounding errors.
* All the corrections made in correctIntegralsByAcc0thOrder over
* MINUSFIRSTORDERCORRECTION_TIME cycles are summed up. This number is
* then divided by MINUSFIRSTORDERCORRECTION_TIME to get the approx.
* correction that should have been applied to each iteration to fix
* the error. This is then added to the dynamic offsets.
************************************************************************/
// 2 times / sec.
#define DRIFTCORRECTION_TIME 488/2
void driftCompensation(void) {
static int16_t timer = DRIFTCORRECTION_TIME;
int16_t deltaCompensation;
if (! --timer) {
timer = DRIFTCORRECTION_TIME;
deltaCompensation = ((pitchCorrectionSum + 1000L * DRIFTCORRECTION_TIME / 2) / 1000 / DRIFTCORRECTION_TIME);
CHECK_MIN_MAX(deltaCompensation, -staticParams.DriftComp, staticParams.DriftComp);
dynamicOffsetPitch += deltaCompensation / staticParams.GyroAccTrim;
 
deltaCompensation = ((rollCorrectionSum + 1000L * DRIFTCORRECTION_TIME / 2) / 1000 / DRIFTCORRECTION_TIME);
CHECK_MIN_MAX(deltaCompensation, -staticParams.DriftComp, staticParams.DriftComp);
dynamicOffsetRoll += deltaCompensation / staticParams.GyroAccTrim;
 
pitchCorrectionSum = rollCorrectionSum = 0;
 
DebugOut.Analog[28] = dynamicOffsetPitch;
DebugOut.Analog[29] = dynamicOffsetRoll;
}
}
 
/************************************************************************
* Main procedure.
************************************************************************/
void calculateFlightAttitude(void) {
getAnalogData();
integrate();
#ifdef ATTITUDE_USE_ACC_SENSORS
correctIntegralsByAcc0thOrder();
driftCompensation();
#endif
}
 
/*
void updateCompass(void) {
int16_t w, v, r,correction, error;
if(compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) {
setCompassCalState();
} else {
// get maximum attitude angle
w = abs(pitchAngle / 512);
v = abs(rollAngle / 512);
if(v > w) w = v;
correction = w / 8 + 1;
// calculate the deviation of the yaw gyro heading and the compass heading
if (compassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined
else error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180;
if(abs(yawRate) > 128) { // spinning fast
error = 0;
}
if(!badCompassHeading && w < 25) {
if(updateCompassCourse) {
beep(200);
yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW;
compassCourse = (int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW);
updateCompassCourse = 0;
}
}
yawGyroHeading += (error * 8) / correction;
w = (w * dynamicParams.CompassYawEffect) / 32;
w = dynamicParams.CompassYawEffect - w;
if(w >= 0) {
if(!badCompassHeading) {
v = 64 + (maxControlPitch + maxControlRoll) / 8;
// calc course deviation
r = ((540 + (yawGyroHeading / GYRO_DEG_FACTOR_YAW) - compassCourse) % 360) - 180;
v = (r * w) / v; // align to compass course
// limit yaw rate
w = 3 * dynamicParams.CompassYawEffect;
if (v > w) v = w;
else if (v < -w) v = -w;
yawAngle += v;
}
else
{ // wait a while
badCompassHeading--;
}
}
else { // ignore compass at extreme attitudes for a while
badCompassHeading = 500;
}
}
}
*/
 
/*
* This is part of an experiment to measure average sensor offsets caused by motor vibration,
* and to compensate them away. It brings about some improvement, but no miracles.
* As long as the left stick is kept in the start-motors position, the dynamic compensation
* will measure the effect of vibration, to use for later compensation. So, one should keep
* the stick in the start-motors position for a few seconds, till all motors run (at the wrong
* speed unfortunately... must find a better way)
*/
/*
void attitude_startDynamicCalibration(void) {
dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0;
savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000;
}
 
void attitude_continueDynamicCalibration(void) {
// measure dynamic offset now...
dynamicCalPitch += hiResPitchGyro;
dynamicCalRoll += hiResRollGyro;
dynamicCalYaw += rawYawGyroSum;
dynamicCalCount++;
// Param6: Manual mode. The offsets are taken from Param7 and Param8.
if (dynamicParams.UserParam6 || 1) { // currently always enabled.
// manual mode
dynamicOffsetPitch = dynamicParams.UserParam7 - 128;
dynamicOffsetRoll = dynamicParams.UserParam8 - 128;
} else {
// use the sampled value (does not seem to work so well....)
dynamicOffsetPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount;
dynamicOffsetRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount;
dynamicOffsetYaw = -dynamicCalYaw / dynamicCalCount;
}
// keep resetting these meanwhile, to avoid accumulating errors.
setStaticAttitudeIntegrals();
yawAngle = 0;
}
*/
/branches/dongfang_FC_rewrite/attitude.h
0,0 → 1,149
/*#######################################################################################
Attitude sense system (processing of gyro, accelerometer and altimeter data)
#######################################################################################*/
 
#ifndef _ATTITUDE_H
#define _ATTITUDE_H
 
#include <inttypes.h>
 
#include "analog.h"
 
// For debugging only.
#include "uart0.h"
 
/*
* If you have no acc. sensor or do not want to use it, remove this define. This will cause the
* acc. sensor to be ignored at attitude calibration.
*/
#define ATTITUDE_USE_ACC_SENSORS yes
 
/*
* Default hysteresis to use for the -180 degree to 180 degree wrap.
*/
#define PITCHOVER_HYSTERESIS 0L
#define ROLLOVER_HYSTERESIS 0L
 
/*
* The frequency at which numerical integration takes place. 488 in original code.
*/
#define INTEGRATION_FREQUENCY 488
 
/*
* Gyro readings are divided by this before being used in attitude control. This will scale them
* to match the scale of the stick control etc. variables. This is just a rough non-precision
* scaling - the below definitions make precise conversion factors.
* Will be about 4 for InvenSense, 8 for FC1.3 and 13 for ADXRS610 (hmmm - originally is was only 8???)
* The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees.
*/
#define HIRES_GYRO_INTEGRATION_FACTOR (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)
 
/*
* Constant for deriving an attitude angle from acceleration measurement.
*
* The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef.
* 1g is (3V * 1024) / (5 * 3V) = 205 counts. The ADC ISR sums 2 acc. samples to each
* [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g.
* We approximate a small pitch/roll angle v by assuming that the copter does not accelerate:
* In this explanation it is assumed that the ADC values are 0 based, and gravity is included.
* The sine of v is the far side / the hypothenusis:
* sin v = acc / sqrt(acc^2 + acc_z^2)
* Using that v is a small angle, and the near side is about equal to the the hypothenusis:
* sin v ~= acc / acc_z
* Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410,
* and sin v ~= v (small angles, in radians):
* sin v ~= acc / 410
* v / 57.3 ~= acc / 410
* v ~= acc * 57.3 / 410
* acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree.
*
* Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3)
*/
#define DEG_ACC_FACTOR 7
 
/*
* Growth of the integral per degree:
* The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by
* HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration.
* The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable).
*/
#define GYRO_DEG_FACTOR_PITCHROLL (int)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR)
#define GYRO_DEG_FACTOR_YAW (int)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION)
 
/*
* This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value
* = the factor an acc. sensor should be multiplied by to get the gyro integral
* value for the same (small) angle.
* The value is about 200.
*/
#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR))
 
/*
* Rotation rates
*/
extern int16_t pitchRate_PID, rollRate_PID, yawRate;
extern int16_t pitchDifferential, rollDifferential;
 
/*
* Attitudes calcualted by numerical integration of gyro rates
*/
extern int32_t pitchAngle, rollAngle, yawAngle;
 
// extern volatile int32_t ReadingIntegralTop; // calculated in analog.c
 
/*
* Compass navigation
*/
extern int16_t compassHeading;
extern int16_t compassCourse;
extern int16_t compassOffCourse;
extern uint8_t compassCalState;
extern int32_t yawGyroHeading;
extern int16_t yawGyroHeadingInDeg;
extern uint16_t updateCompassCourse;
extern uint16_t badCompassHeading;
 
/*
* Height control
*/
extern int readingHeight;
extern int setPointHeight;
 
/*
* Interval wrap-over values for attitude integrals
*/
extern long turnOver180Pitch, turnOver180Roll;
 
// No longer used.
// extern uint8_t FunnelCourse;
 
/*
* Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements,
* to help cancelling out drift and vibration noise effects. The dynamic offsets themselves
* can be updated in flight by different ways, for example:
* - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool
* - Summing up how much acc. meter correction was done to the gyro integrals over the last n
* integration, and then adding the sum / n to the dynamic offset
* - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that
* - Invent your own...
*/
extern int16_t dynamicOffsetPitch, dynamicOffsetRoll, dynamicOffsetYaw;
// extern int16_t savedDynamicOffsetPitch, savedDynamicOffsetRoll;
 
/*
* Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor).
* To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right).
*/
void attitude_setNeutral(void);
 
/*
* Experiment.
*/
// void attitude_startDynamicCalibration(void);
// void attitude_continueDynamicCalibration(void);
 
/*
* Main routine, called from the flight loop.
*/
void calculateFlightAttitude(void);
#endif //_ATTITUDE_H
/branches/dongfang_FC_rewrite/configuration.c
0,0 → 1,210
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <util/delay.h>
#include <avr/eeprom.h>
#include "configuration.h"
#include "eeprom.h"
#include "timer0.h"
 
int16_t variables[8] = {0,0,0,0,0,0,0,0};
fc_param_t dynamicParams = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0};
uint8_t CPUType = ATMEGA644;
uint8_t BoardRelease = 13;
 
/************************************************************************
* Map the parameter to pot values
************************************************************************/
void configuration_applyVariablesToParams(void) {
uint8_t i;
#define SET_POT_MM(b,a,min,max) { if (a<255) {if (a>=251) b=variables[a-251]; else b=a;} if(b<=min) b=min; else if(b>=max) b=max;}
#define SET_POT(b,a) { if (a<255) {if (a>=251) b=variables[a-251]; else b=a;}}
SET_POT(dynamicParams.MaxHeight,staticParams.MaxHeight);
SET_POT_MM(dynamicParams.HeightD,staticParams.HeightD,0,100);
SET_POT_MM(dynamicParams.HeightP,staticParams.HeightP,0,100);
SET_POT(dynamicParams.Height_ACC_Effect,staticParams.Height_ACC_Effect);
SET_POT(dynamicParams.CompassYawEffect,staticParams.CompassYawEffect);
SET_POT_MM(dynamicParams.GyroP,staticParams.GyroP,10,255);
SET_POT(dynamicParams.GyroI,staticParams.GyroI);
SET_POT(dynamicParams.GyroD,staticParams.GyroD);
SET_POT(dynamicParams.IFactor,staticParams.IFactor);
for (i=0; i<sizeof(staticParams.UserParams1); i++) {
SET_POT(dynamicParams.UserParams[i],staticParams.UserParams1[i]);
}
for (i=0; i<sizeof(staticParams.UserParams2); i++) {
SET_POT(dynamicParams.UserParams[i + sizeof(staticParams.UserParams1)], staticParams.UserParams2[i]);
}
SET_POT(dynamicParams.ServoPitchControl,staticParams.ServoPitchControl);
SET_POT(dynamicParams.LoopGasLimit,staticParams.LoopGasLimit);
SET_POT(dynamicParams.AxisCoupling1,staticParams.AxisCoupling1);
SET_POT(dynamicParams.AxisCoupling2,staticParams.AxisCoupling2);
SET_POT(dynamicParams.AxisCouplingYawCorrection,staticParams.AxisCouplingYawCorrection);
SET_POT(dynamicParams.DynamicStability,staticParams.DynamicStability);
SET_POT_MM(dynamicParams.J16Timing,staticParams.J16Timing,1,255);
SET_POT_MM(dynamicParams.J17Timing,staticParams.J17Timing,1,255);
#if defined (USE_MK3MAG)
SET_POT(dynamicParams.NaviGpsModeControl,staticParams.NaviGpsModeControl);
SET_POT(dynamicParams.NaviGpsGain,staticParams.NaviGpsGain);
SET_POT(dynamicParams.NaviGpsP,staticParams.NaviGpsP);
SET_POT(dynamicParams.NaviGpsI,staticParams.NaviGpsI);
SET_POT(dynamicParams.NaviGpsD,staticParams.NaviGpsD);
SET_POT(dynamicParams.NaviGpsACC,staticParams.NaviGpsACC);
SET_POT_MM(dynamicParams.NaviOperatingRadius,staticParams.NaviOperatingRadius,10, 255);
SET_POT(dynamicParams.NaviWindCorrection,staticParams.NaviWindCorrection);
SET_POT(dynamicParams.NaviSpeedCompensation,staticParams.NaviSpeedCompensation);
#endif
SET_POT(dynamicParams.ExternalControl,staticParams.ExternalControl);
}
 
uint8_t getCPUType(void) { // works only after reset or power on when the registers have default values
uint8_t CPUType = ATMEGA644;
if( (UCSR1A == 0x20) && (UCSR1C == 0x06) ) CPUType = ATMEGA644P; // initial Values for 644P after reset
return CPUType;
}
 
/*
* Automatic detection of hardware components is not supported in this development-oriented
* FC firmware. It would go against the point of it: To enable alternative hardware
* configurations with otherwise unsupported components. Instead, one should write
* custom code + adjust constants for the new hardware, and include the relevant code
* from the makefile.
* However - we still do detect the board release. Reason: Otherwise it would be too
* tedious to have to modify the code for how to turn on and off LEDs when deploying
* on different HW version....
*/
 
uint8_t getBoardRelease(void) {
uint8_t BoardRelease = 13;
// the board release is coded via the pull up or down the 2 status LED
 
PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate
DDRB &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input
 
_delay_loop_2(1000); // make some delay
 
switch( PINB & ((1<<PINB1)|(1<<PINB0))) {
case 0x00:
BoardRelease = 10; // 1.0
break;
case 0x01:
BoardRelease = 11; // 1.1 or 1.2
break;
case 0x02:
BoardRelease = 20; // 2.0
break;
case 0x03:
BoardRelease = 13; // 1.3
break;
default:
break;
}
// set LED ports as output
DDRB |= (1<<DDB1)|(1<<DDB0);
RED_ON;
GRN_OFF;
return BoardRelease;
}
 
void beep(uint16_t millis) {
BeepTime = millis;
}
 
/*
* Make [numbeeps] beeps.
*/
void beepNumber(uint8_t numbeeps) {
while(numbeeps--) {
if(MKFlags & MKFLAG_MOTOR_RUN) return; //auf keinen Fall bei laufenden Motoren!
beep(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 function if motors are running
}
}
 
/*
* Beep the R/C alarm signal
*/
void beepRCAlarm(void) {
if(BeepModulation == 0xFFFF) { // If not already beeping an alarm signal (?)
BeepTime = 15000; // 1.5 seconds
BeepModulation = 0x0C00;
}
}
 
/*
* Beep the I2C bus error signal
*/
void beepI2CAlarm(void) {
if((BeepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN)) {
BeepTime = 10000; // 1 second
BeepModulation = 0x0080;
}
}
 
/*
* Beep the battery low alarm signal
*/
void beepBatteryAlarm(void) {
BeepModulation = 0x0300;
if(!BeepTime) {
BeepTime = 6000; // 0.6 seconds
}
}
 
/*
* Beep the EEPROM checksum alarm
*/
void beepEEPROMAlarm(void) {
BeepModulation = 0x0007;
if(!BeepTime) {
BeepTime = 6000; // 0.6 seconds
}
}
/branches/dongfang_FC_rewrite/configuration.h
0,0 → 1,208
#ifndef _CONFIGURATION_H
#define _CONFIGURATION_H
 
#include <inttypes.h>
#include <avr/io.h>
 
typedef struct {
uint8_t HeightD;
uint8_t MaxHeight;
uint8_t HeightP;
uint8_t Height_ACC_Effect;
uint8_t CompassYawEffect;
uint8_t GyroD;
uint8_t GyroP;
uint8_t GyroI;
uint8_t StickYawP;
uint8_t IFactor;
uint8_t UserParams[8];
/*
uint8_t UserParam2;
uint8_t UserParam3;
uint8_t UserParam4;
uint8_t UserParam5;
uint8_t UserParam6;
uint8_t UserParam7;
uint8_t UserParam8;
*/
uint8_t ServoPitchControl;
uint8_t LoopGasLimit;
uint8_t AxisCoupling1;
uint8_t AxisCoupling2;
uint8_t AxisCouplingYawCorrection;
uint8_t DynamicStability;
uint8_t ExternalControl;
uint8_t J16Timing;
uint8_t J17Timing;
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
uint8_t NaviGpsModeControl;
uint8_t NaviGpsGain;
uint8_t NaviGpsP;
uint8_t NaviGpsI;
uint8_t NaviGpsD;
uint8_t NaviGpsACC;
uint8_t NaviOperatingRadius;
uint8_t NaviWindCorrection;
uint8_t NaviSpeedCompensation;
#endif
int8_t KalmanK;
int8_t KalmanMaxDrift;
int8_t KalmanMaxFusion;
} fc_param_t;
 
extern fc_param_t dynamicParams;
 
// values above 250 representing poti1 to poti4
typedef struct {
uint8_t ChannelAssignment[8]; // see upper defines for details
uint8_t GlobalConfig; // see upper defines for bitcoding
uint8_t HeightMinGas; // Value : 0-100
uint8_t HeightD; // Value : 0-250
uint8_t MaxHeight; // Value : 0-32
uint8_t HeightP; // Value : 0-32
uint8_t Height_Gain; // Value : 0-50
uint8_t Height_ACC_Effect; // Value : 0-250
uint8_t StickP; // Value : 1-6
uint8_t StickD; // Value : 0-64
uint8_t StickYawP; // Value : 1-20
uint8_t GasMin; // Value : 0-32
uint8_t GasMax; // Value : 33-250
uint8_t GyroAccFactor; // Value : 1-64
uint8_t CompassYawEffect; // Value : 0-32
uint8_t GyroP; // Value : 10-250
uint8_t GyroI; // Value : 0-250
uint8_t GyroD; // Value : 0-250
uint8_t LowVoltageWarning; // Value : 0-250
uint8_t EmergencyGas; // Value : 0-250 //Gaswert bei Empängsverlust
uint8_t EmergencyGasDuration; // Value : 0-250 // Zeitbis auf EmergencyGas geschaltet wird, wg. Rx-Problemen
uint8_t UfoArrangement; // x oder + Formation
uint8_t IFactor; // Value : 0-250
uint8_t UserParams1[4]; // Value : 0-250
/*
uint8_t UserParam2; // Value : 0-250
uint8_t UserParam3; // Value : 0-250
uint8_t UserParam4; // Value : 0-250
*/
uint8_t ServoPitchControl; // Value : 0-250 // Stellung des Servos
uint8_t ServoPitchComp; // Value : 0-250 // Einfluss Gyro/Servo
uint8_t ServoPitchMin; // Value : 0-250 // Anschlag
uint8_t ServoPitchMax; // Value : 0-250 // Anschlag
uint8_t ServoRefresh; // Value: 0-250 // Refreshrate of servo pwm output
uint8_t LoopGasLimit; // Value: 0-250 max. Gas während Looping
uint8_t LoopThreshold; // Value: 0-250 Schwelle für Stickausschlag
uint8_t LoopHysteresis; // Value: 0-250 Hysterese für Stickausschlag
uint8_t AxisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung)
uint8_t AxisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
uint8_t AxisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
uint8_t AngleTurnOverPitch; // Value: 0-250 180°-Punkt
uint8_t AngleTurnOverRoll; // Value: 0-250 180°-Punkt
uint8_t GyroAccTrim; // 1/k (Koppel_ACC_Wirkung)
uint8_t DriftComp; // limit for gyrodrift compensation
uint8_t DynamicStability; // PID limit for Attitude controller
uint8_t UserParams2[4]; // Value : 0-250
/*
uint8_t UserParam6; // Value : 0-250
uint8_t UserParam7; // Value : 0-250
uint8_t UserParam8; // Value : 0-250
*/
uint8_t J16Bitmask; // for the J16 Output
uint8_t J16Timing; // for the J16 Output
uint8_t J17Bitmask; // for the J17 Output
uint8_t J17Timing; // for the J17 Output
uint8_t NaviGpsModeControl; // Parameters for the Naviboard
uint8_t NaviGpsGain; // overall gain for GPS-PID controller
uint8_t NaviGpsP; // P gain for GPS-PID controller
uint8_t NaviGpsI; // I gain for GPS-PID controller
uint8_t NaviGpsD; // D gain for GPS-PID controller
uint8_t NaviGpsPLimit; // P limit for GPS-PID controller
uint8_t NaviGpsILimit; // I limit for GPS-PID controller
uint8_t NaviGpsDLimit; // D limit for GPS-PID controller
uint8_t NaviGpsACC; // ACC gain for GPS-PID controller
uint8_t NaviGpsMinSat; // number of sattelites neccesary for GPS functions
uint8_t NaviStickThreshold; // activation threshild for detection of manual stick movements
uint8_t NaviWindCorrection; // streng of wind course correction
uint8_t NaviSpeedCompensation; // D gain fefore position hold login
uint8_t NaviOperatingRadius; // Radius limit in m around start position for GPS flights
uint8_t NaviAngleLimitation; // limitation of attitude angle controlled by the gps algorithm
uint8_t NaviPHLoginTime; // position hold logintimeout
uint8_t ExternalControl; // for serial Control
uint8_t BitConfig; // see upper defines for bitcoding
uint8_t ServoPitchCompInvert; // Value : 0-250 0 oder 1 // WICHTIG!!! am Ende lassen
uint8_t Reserved[4];
int8_t Name[12];
} paramset_t;
 
#define PARAMSET_STRUCT_LEN sizeof(paramset_t)
 
extern paramset_t staticParams;
 
typedef struct {
uint8_t Revision;
int8_t Name[12];
int8_t Motor[16][4];
} __attribute__((packed)) MixerTable_t;
 
extern MixerTable_t Mixer;
 
// MKFlags
#define MKFLAG_MOTOR_RUN (1<<0)
#define MKFLAG_FLY (1<<1)
#define MKFLAG_CALIBRATE (1<<2)
#define MKFLAG_START (1<<3)
#define MKFLAG_EMERGENCY_LANDING (1<<4)
#define MKFLAG_RESERVE1 (1<<5)
#define MKFLAG_RESERVE2 (1<<6)
#define MKFLAG_RESERVE3 (1<<7)
 
// bit mask for staticParams.GlobalConfig
#define CFG_HEIGHT_CONTROL (1<<0)
#define CFG_HEIGHT_SWITCH (1<<1)
#define CFG_HEADING_HOLD (1<<2)
#define CFG_COMPASS_ACTIVE (1<<3)
#define CFG_COMPASS_FIX (1<<4)
#define CFG_GPS_ACTIVE (1<<5)
#define CFG_AXIS_COUPLING_ACTIVE (1<<6)
#define CFG_ROTARY_RATE_LIMITER (1<<7)
 
// bit mask for ParamSet.BitConfig
#define CFG_LOOP_UP (1<<0)
#define CFG_LOOP_DOWN (1<<1)
#define CFG_LOOP_LEFT (1<<2)
#define CFG_LOOP_RIGHT (1<<3)
#define CFG_HEIGHT_3SWITCH (1<<4)
 
#define ATMEGA644 0
#define ATMEGA644P 1
#define SYSCLK F_CPU
 
// Not really a part of configuration, but heck, LEDs and beepers now have a home.
#define RED_OFF {if((BoardRelease == 10) || (BoardRelease == 20)) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);}
#define RED_ON {if((BoardRelease == 10) || (BoardRelease == 20)) PORTB |= (1<<PORTB0); else PORTB &=~(1<<PORTB0);}
#define RED_FLASH PORTB ^= (1<<PORTB0)
#define GRN_OFF {if(BoardRelease < 12) PORTB &=~(1<<PORTB1); else PORTB |= (1<<PORTB1);}
#define GRN_ON {if(BoardRelease < 12) PORTB |= (1<<PORTB1); else PORTB &=~(1<<PORTB1);}
#define GRN_FLASH PORTB ^= (1<<PORTB1)
 
// Mixer table
#define MIX_THROTTLE 0
#define MIX_PITCH 1
#define MIX_ROLL 2
#define MIX_YAW 3
 
extern volatile uint8_t MKFlags;
extern int16_t variables[8];
extern uint8_t BoardRelease;
extern uint8_t CPUType;
 
void configuration_applyVariablesToParams(void);
uint8_t getCPUType(void);
uint8_t getBoardRelease(void);
 
// Not really a part of configuration, but heck, LEDs and beepers now have a home.
void beep(uint16_t millis);
void beepNumber(uint8_t numbeeps);
void beepRCAlarm(void);
void beepI2CAlarm(void);
void beepBatteryAlarm(void);
 
#endif // _CONFIGURATION_H
/branches/dongfang_FC_rewrite/control.c
0,0 → 1,371
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 "control.h"
 
#include "rc.h"
#include "configuration.h"
#include "attitude.h"
#include "eeprom.h"
#include "flight.h"
 
#define RCChannel(dimension) (PPM_in[staticParams.ChannelAssignment[dimension]])
 
uint16_t maxStickPitch = 0, maxStickRoll = 0;
int16_t stickPitch = 0, stickRoll = 0, stickYaw = 0, stickThrottle = 0;
int16_t GPSStickPitch = 0, GPSStickRoll = 0;
int16_t externalStickPitch = 0, externalStickRoll = 0, externalStickYaw = 0, externalHeightValue = -20;
 
// dongfang's own experiment: Cablibrated sticks.
int16_t stickOffsetPitch = 0, stickOffsetRoll = 0;
 
// Looping-or-not flags.
uint8_t loopingPitch = 0, loopingRoll = 0;
uint8_t loopingLeft = 0, loopingRight = 0, loopingDown = 0, loopingTop = 0;
 
// Internal variables for reading commands made with an R/S stick.
uint8_t lastStickCommand = STICK_COMMAND_UNDEF;
uint8_t stickCommandTimer = 0;
 
ExternalControl_t externalControl;
 
/*
* Stick diagram:
* 2--3--4
* | | +
* 1 9 5 ^ 0
* | | |
* 8--7--6
*
* + <--
* 0
*
* Not in any of these positions: 0
*/
 
/*
* The stick most be further from center than this to indicate a settings number (1-5).
*/
#define STICK_SETTINGSELECTION_THRESHOLD 70
 
uint8_t control_getLeftRCStickIndex(int16_t thresholdThrottle, int16_t thresholdYaw) {
if(RCChannel(CH_THROTTLE) > thresholdThrottle) {
// throttle is up
if(RCChannel(CH_YAW) > thresholdYaw)
return STICK_COMMAND_GYROCAL;
if(RCChannel(CH_YAW) < -thresholdYaw)
return STICK_COMMAND_ACCCAL;
return STICK_COMMAND_UNDEF;
} else if(RCChannel(CH_THROTTLE) < -thresholdThrottle) {
// pitch is down
if(RCChannel(CH_YAW) > thresholdYaw)
return STICK_COMMAND_STOP;
if(RCChannel(CH_YAW) < -thresholdYaw)
return STICK_COMMAND_START;
return STICK_COMMAND_UNDEF;
} else {
// pitch is around center
return STICK_COMMAND_UNDEF;
}
}
 
uint8_t control_getRightRCStickIndex(void) {
if(RCChannel(CH_PITCH) > STICK_SETTINGSELECTION_THRESHOLD) {
// pitch is up
if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD)
return 2;
if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD)
return 4;
return 3;
} else if(RCChannel(CH_PITCH) < -STICK_SETTINGSELECTION_THRESHOLD) {
// pitch is down
if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD)
return 8;
if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD)
return 6;
return 7;
} else {
// pitch is around center
if(RCChannel(CH_ROLL) > STICK_SETTINGSELECTION_THRESHOLD)
return 1;
if(RCChannel(CH_ROLL) < -STICK_SETTINGSELECTION_THRESHOLD)
return 5;
return 9;
}
}
 
/*
* This could be expanded to take calibrate / start / stop commands from ohter sources
* than the R/C (read: Custom MK R/C project)
*/
void control_senseStickCommands(void) {
uint8_t stickCommandNow = control_getLeftRCStickIndex(85, 85);
if (stickCommandNow != lastStickCommand) {
lastStickCommand = stickCommandNow;
stickCommandTimer = 0;
} else {
if (stickCommandTimer < 201)
stickCommandTimer++;
}
}
 
/*
* This could be expanded to take calibrate / start / stop commands from ohter sources
* than the R/C (read: Custom MK R/C project)
*/
uint8_t control_getStickCommand(void) {
// If the same command was made 200 times, it's stable.
if (stickCommandTimer >= 200) {
return lastStickCommand;
}
return STICK_COMMAND_UNDEF;
}
 
uint8_t control_isStickCommandRepeated(void) {
return stickCommandTimer > 200 ? 1 : 0;
}
 
/*
* To be fired only when the right stick is in the center position.
* This will cause the value of pitch and roll stick to be adjusted
* to zero (not just to near zero, as per the assumption in rc.c
* about the rc signal. I had values about 50..70 with a Futaba
* R617 receiver.) This calibration is not strictly necessary, but
* for control logic that depends on the exact (non)center position
* of a stick, it may be useful.
*/
void control_setNeutral(void) {
stickOffsetPitch += stickPitch;
stickOffsetRoll += stickRoll;
}
 
/*
* Set the potientiometer values to the values of the respective R/C channel
* right now. No slew rate limit.
*/
void control_initPots(void) {
uint8_t i;
for (i=0; i<4; i++) {
pots[i] = RCChannel(CH_POTS + i) + POT_OFFSET;
}
for (i=4; i<8; i++) {
pots[i] = PPM_in[9 + (i-4)] + POT_OFFSET;
}
}
 
/*
* Update potentiometer values with slow slew rate. Could be made faster if desired.
*/
void control_updatePots(void) {
uint8_t i;
uint16_t targetvalue;
for (i=0; i<8; i++) {
if (i<4) // configured pots
targetvalue = RCChannel(CH_POTS + i) + POT_OFFSET;
else // PPM24-Extension
targetvalue = PPM_in[9 + i] + POT_OFFSET;
if (targetvalue < 0) targetvalue = 0;
if (pots[i] < targetvalue && pots[i] < 255) pots[i]++; else if(pots[i] > 0 && pots[i] > targetvalue) pots[i]--;
}
}
 
/*
* Update the variables indicating stick position from the sum of R/C, GPS and external control.
*/
void control_update(void) {
// calculate Stick inputs by rc channels (P) and changing of rc channels (D)
stickPitch = RCChannel(CH_PITCH) * staticParams.StickP;
// (stick_pitch * 3 + RCChannel(CH_PITCH) * staticParams.StickP) / 4;
stickPitch += PPM_diff[staticParams.ChannelAssignment[CH_PITCH]] * staticParams.StickD;
stickPitch = stickPitch - stickOffsetPitch - GPSStickPitch;
stickRoll = RCChannel(CH_ROLL) * staticParams.StickP;
// stick_roll = (stick_roll * 3 + RCChannel(CH_ROLL) * staticParams.StickP) / 4;
stickRoll += PPM_diff[staticParams.ChannelAssignment[CH_ROLL]] * staticParams.StickD;
stickRoll = stickRoll - stickOffsetRoll - GPSStickRoll;
// mapping of yaw
stickYaw = -RCChannel(CH_YAW);
// (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) {
if (stickYaw > 2) stickYaw-= 2;
else if (stickYaw< -2) stickYaw += 2;
else stickYaw = 0;
}
// mapping of gas
stickThrottle = RCChannel(CH_THROTTLE) + 120;// shift to positive numbers
if(externalControl.config & 0x01 && dynamicParams.ExternalControl > 128) {
stickPitch += (int16_t) externalControl.pitch * (int16_t) staticParams.StickP;
stickRoll += (int16_t) externalControl.roll * (int16_t) staticParams.StickP;
stickYaw += externalControl.yaw;
// ExternHeightValue = (int16_t) ExternControl.Height * (int16_t)staticParams.Height_Gain;
// Dubious: Lowest throttle setting has precedence.
if(externalControl.throttle < stickThrottle) stickThrottle = externalControl.throttle;
}
if(stickThrottle < 0) stickThrottle = 0;
 
if(abs(stickPitch / STICK_GAIN) > maxStickPitch) {
maxStickPitch = abs(stickPitch) / STICK_GAIN;
if(maxStickPitch > 100) maxStickPitch = 100;
}
else if (maxStickPitch) maxStickPitch--;
 
if(abs(stickRoll / STICK_GAIN) > maxStickRoll) {
maxStickRoll = abs(stickRoll) / STICK_GAIN;
if(maxStickRoll > 100) maxStickRoll = 100;
}
else if (maxStickRoll) maxStickRoll--;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping? Do not consider external or GPS input for this :)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((RCChannel(CH_ROLL) > staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_LEFT) loopingLeft = 1;
else {
if(loopingLeft) { // Hysteresis
if((RCChannel(CH_ROLL) < (staticParams.LoopThreshold - staticParams.LoopHysteresis))) loopingLeft = 0;
}
}
if((RCChannel(CH_ROLL) < -staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_RIGHT) loopingRight = 1;
else {
if(loopingRight) { // Hysteresis
if(RCChannel(CH_ROLL) > -(staticParams.LoopThreshold - staticParams.LoopHysteresis)) loopingRight = 0;
}
}
if((RCChannel(CH_PITCH) > staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_UP) loopingTop = 1;
else {
if(loopingTop) { // Hysteresis
if((RCChannel(CH_PITCH) < (staticParams.LoopThreshold - staticParams.LoopHysteresis))) loopingTop = 0;
}
}
if((RCChannel(CH_PITCH) < -staticParams.LoopThreshold) && staticParams.BitConfig & CFG_LOOP_DOWN) loopingDown = 1;
else {
if(loopingDown) { // Hysteresis
if(RCChannel(CH_PITCH) > -(staticParams.LoopThreshold - staticParams.LoopHysteresis)) loopingDown = 0;
}
}
if(loopingLeft || loopingRight) loopingRoll = 1; else loopingRoll = 0;
if(loopingTop || loopingDown) { loopingPitch = 1; loopingRoll = 0; loopingLeft = 0; loopingRight = 0;} else loopingPitch = 0;
}
 
void setCompassCalState(void) {
static uint8_t stick = 1;
// if pitch is centered or top set stick to zero
if(RCChannel(CH_PITCH) > -20) stick = 0;
// if pitch is down trigger to next cal state
if((RCChannel(CH_PITCH) < -70) && !stick) {
stick = 1;
compassCalState++;
if(compassCalState < 5) beepNumber(compassCalState);
else beep(1000);
}
}
 
/*
*
*/
uint8_t control_hasNewRCData(void) {
// return !NewPpmData--;
return (NewPpmData-- == 0) ? 1 : 0;
}
 
void control_performPilotCalibrationCommands(uint8_t stickCommand) {
if (stickCommand == STICK_COMMAND_GYROCAL && !control_isStickCommandRepeated()) {
// 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
uint8_t setting = control_getRightRCStickIndex();
if ((setting > 0 && setting < 6) || setting == 9) {
// Gyro calinbration, with or without selecting a new parameter-set.
if(setting > 0 && setting < 6) {
// A valid parameter-set (1..5) was chosen - use it.
setActiveParamSet(setting);
}
ParamSet_ReadFromEEProm(getActiveParamSet());
attitude_setNeutral();
flight_setNeutral();
if (setting == 9) { // Right stick is centered; calibrate it to zero (hmm strictly does not belong here).
control_setNeutral(); // Calibrate right stick neutral position.
}
beepNumber(getActiveParamSet());
} else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && setting == 7) {
// If right stick is centered and down
compassCalState = 1;
beep(1000);
}
}
// save the ACC neutral setting to eeprom
else {
if(stickCommand == STICK_COMMAND_ACCCAL && !control_isStickCommandRepeated()) {
// Run gyro and acc. meter calibration but do not repeat it.
GRN_OFF;
analog_calibrateAcc();
attitude_setNeutral();
flight_setNeutral();
control_setNeutral(); // Calibrate right stick neutral position.
beepNumber(getActiveParamSet());
}
}
} // end !MOTOR_RUN condition.
/branches/dongfang_FC_rewrite/control.h
0,0 → 1,74
/*#######################################################################################
Stick control interface
#######################################################################################*/
/*
 
#ifndef _CONTROL_H
#define _CONTROL_H
 
#include <inttypes.h>
#define STICK_GAIN 4
 
// defines for lookup staticParams.ChannelAssignment
#define CH_PITCH 0
#define CH_ROLL 1
#define CH_THROTTLE 2
#define CH_YAW 3
#define CH_POTS 4
#define POT_OFFSET 110
 
extern int16_t stickPitch, stickRoll, stickYaw, stickThrottle;
extern int16_t stickOffsetNick, stickOffsetRoll;
extern uint16_t maxStickPitch, maxStickRoll;
extern uint8_t loopingPitch, loopingRoll;
 
// external control
extern int16_t externalStickPitch, externalStickRoll, externalStickYaw;
 
// current GPS-stick values
extern int16_t GPSStickPitch, GPSStickRoll;
 
typedef struct {
uint8_t digital[2];
uint8_t eemoteButtons;
int8_t pitch;
int8_t roll;
int8_t yaw;
uint8_t throttle;
int8_t height;
uint8_t free;
uint8_t frame;
uint8_t config;
} __attribute__((packed)) ExternalControl_t;
 
extern ExternalControl_t externalControl;
 
//uint8_t control_getLeftStickCalibrateIndex(void);
//uint8_t control_getLeftStickMotorStartIndex(void);
//uint8_t control_getRightRCStickIndex(void);
void control_initPots(void);
void control_updatePots(void);
 
void setCompassCalState(void);
void updateCompass(void);
 
void control_setNeutral(void);
void control_update(void);
 
#define STICK_COMMAND_UNDEF 0
#define STICK_COMMAND_START 6
#define STICK_COMMAND_STOP 8
#define STICK_COMMAND_GYROCAL 2
#define STICK_COMMAND_ACCCAL 4
 
void control_senseStickCommands(void);
uint8_t control_getStickCommand(void);
uint8_t control_isStickCommandRepeated(void);
 
void control_performPilotCalibrationCommands(uint8_t stickCommand);
 
extern volatile int16_t RC_Quality; // rc signal quality indicator (0 to 200)
extern uint8_t control_hasNewRCData(void);
 
#endif //_CONTROL_H
*/
/branches/dongfang_FC_rewrite/controlMixer.c
0,0 → 1,210
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 "controlMixer.h"
#include "rc.h"
#include "externalControl.h"
#include "configuration.h"
#include "attitude.h"
#include "eeprom.h"
#include "flight.h"
 
/*
* Number of cycles a command must be repeated before commit. Maybe this really belongs in RC.
*/
#define COMMAND_TIMER 200
 
uint16_t maxControlPitch = 0, maxControlRoll = 0;
int16_t controlPitch = 0, controlRoll = 0, controlYaw = 0, controlThrottle = 0;
uint8_t looping = 0;
 
// Internal variables for reading commands made with an R/C stick.
uint8_t lastCommand = COMMAND_NONE;
uint8_t commandTimer = 0;
 
/*
* This could be expanded to take arguments from ohter sources than the RC
* (read: Custom MK RC project)
*/
uint8_t controlMixer_getArgument(void) {
return RC_getArgument();
}
 
/*
* This could be expanded to take calibrate / start / stop commands from ohter sources
* than the R/C (read: Custom MK R/C project)
*/
uint8_t controlMixer_getCommand(void) {
// If the same command was made COMMAND_TIMER times, it's stable.
if (commandTimer >= COMMAND_TIMER) {
return lastCommand;
}
return COMMAND_NONE;
}
 
uint8_t controlMixer_isCommandRepeated(void) {
return commandTimer > COMMAND_TIMER ? 1 : 0;
}
 
void controlMixer_setNeutral(uint8_t calibrate) {
if (calibrate)
RC_calibrate();
EC_setNeutral();
}
 
/*
* 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<8; i++) {
variables[i] = RC_getVariable(i);
}
}
 
/*
* Update potentiometer values with limited slew rate. Could be made faster if desired.
*/
void controlMixer_updateVariables(void) {
uint8_t i;
int16_t targetvalue;
for (i=0; i<8; i++) {
targetvalue = RC_getVariable(i);
if (targetvalue < 0) targetvalue = 0;
if (variables[i] < targetvalue && variables[i] < 255) variables[i]++; else if(variables[i] > 0 && variables[i] > targetvalue) variables[i]--;
}
}
 
uint8_t controlMixer_getSignalQuality(void) {
uint8_t rcQ = RC_getSignalQuality();
uint8_t ecQ = EC_getSignalQuality();
DebugOut.Analog[16] = rcQ;
DebugOut.Analog[17] = ecQ;
// This needs not be the only correct solution...
return rcQ > ecQ ? rcQ : ecQ;
}
 
/*
* Update the variables indicating stick position from the sum of R/C, GPS and external control.
*/
void controlMixer_update(void) {
// calculate Stick inputs by rc channels (P) and changing of rc channels (D)
// TODO: If no signal --> zero.
RC_update();
EC_update();
 
int16_t* RC_PRTY = RC_getPRTY();
int16_t* EC_PRTY = EC_getPRTY();
 
controlPitch = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];
controlRoll = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];
DebugOut.Analog[14] = controlThrottle = RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE];
DebugOut.Analog[15] = controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
if (RC_getSignalQuality() >= SIGNAL_GOOD) {
controlMixer_updateVariables();
configuration_applyVariablesToParams();
looping = RC_getLooping(looping);
} else { // Signal is not OK
// Could handle switch to emergency flight here.
// throttle is handled elsewhere.
looping = 0;
}
 
DebugOut.Analog[18] = (int16_t)looping;
 
// (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
// TODO: Correct, for changed range (stick gain + expo is now applied already)
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
if (controlYaw > 2) controlYaw-= 2;
else if (controlYaw< -2) controlYaw += 2;
else controlYaw = 0;
}
 
/*
* Record maxima
*/
if(abs(controlPitch / STICK_GAIN) > maxControlPitch) {
maxControlPitch = abs(controlPitch) / STICK_GAIN;
if(maxControlPitch > 100) maxControlPitch = 100;
} else if (maxControlPitch) maxControlPitch--;
if(abs(controlRoll / STICK_GAIN) > maxControlRoll) {
maxControlRoll = abs(controlRoll) / STICK_GAIN;
if(maxControlRoll > 100) maxControlRoll = 100;
}
else if (maxControlRoll) maxControlRoll--;
 
// Here we could blend in other sources.
uint8_t commandNow = RC_getCommand();
 
if (commandNow != lastCommand) {
lastCommand = commandNow;
commandTimer = 0;
} else if (commandTimer < COMMAND_TIMER + 1)
commandTimer++;
}
 
/*
void setCompassCalState(void) {
static uint8_t stick = 1;
// if pitch is centered or top set stick to zero
if(RCChannel(CH_PITCH) > -20) stick = 0;
// if pitch is down trigger to next cal state
if((RCChannel(CH_PITCH) < -70) && !stick) {
stick = 1;
compassCalState++;
if(compassCalState < 5) beepNumber(compassCalState);
else beep(1000);
}
}
*/
/branches/dongfang_FC_rewrite/controlMixer.h
0,0 → 1,141
#include <inttypes.h>
/*
* The throttle range is normalized to [0, THROTTLE_RANGE[
* This is the normal range. Exceeding it a little is allowed.
*/
#define THROTTLE_RANGE 256
 
/*
* The stick (pitch, roll, yaw) ranges are normalized to [-STICK_RANGE, STICK_RANGE]
* This is the normal range. Exceeding it a little is allowed.
*/
#define STICK_RANGE 256
 
/*
* An attempt to define a generic control. That could be an R/C receiver, an external control
* (serial over Bluetooth, Wi232, XBee, whatever) or the NaviCtrl.
* This resembles somewhat an object-oriented class definition (except that there are no virtuals).
* The idea is that the combination of different control inputs, of the way they superimpose upon
* each other, the proirities between them and the behavior in case that one fails is simplified,
* and all in one place.
*/
/*
* Signal qualities, used to determine the availability of a control.
* NO_SIGNAL means there was never a signal. SIGNAL_LOST that there was a signal, but it was lost.
* SIGNAL_BAD is too bad for flight. This is the hysteresis range for deciding whether to engage
* or disengage emergency landing.
* SIGNAL_OK means the signal is usable for flight.
* SIGNAL_GOOD means the signal can also be used for setting parameters.
*/
#define NO_SIGNAL 0
#define SIGNAL_LOST 1
#define SIGNAL_BAD 2
#define SIGNAL_OK 3
#define SIGNAL_GOOD 4
 
/*
* An enumeration over the start motors, stop motors, calibrate gyros
* and calibreate acc. meters commands.
*/
#define COMMAND_NONE 0
#define COMMAND_START 6
#define COMMAND_STOP 8
#define COMMAND_GYROCAL 2
#define COMMAND_ACCCAL 4
 
/*
* The PRTY arrays
*/
#define CONTROL_PITCH 0
#define CONTROL_ROLL 1
#define CONTROL_THROTTLE 2
#define CONTROL_YAW 3
 
/*
* Looping flags.
* LOOPING_UP || LOOPING_DOWN <=> LOOPING_PITCH_AXIS
* LOOPING_LEFT || LOOPING_RIGHT <=> LOOPING_ROLL_AXIS
*/
#define LOOPING_UP (1<<0)
#define LOOPING_DOWN (1<<1)
#define LOOPING_LEFT (1<<2)
#define LOOPING_RIGHT (1<<3)
#define LOOPING_PITCH_AXIS (1<<4)
#define LOOPING_ROLL_AXIS (1<<5)
 
/*
* This is only relevant for "abstract controls" ie. all control sources have the
* same interface. This struct of code pointers is used like an abstract class
* definition from object-oriented languages, and all control input implementations
* will declare an instance of the stuct (=implementation of the abstract class).
*/
typedef struct {
/* Get the pitch input in the nominal range [-STICK_RANGE, STICK_RANGE]. */
int16_t(*getPitch)(void);
 
/* Get the roll input in the nominal range [-STICK_RANGE, STICK_RANGE]. */
int16_t(*getRoll)(void);
/* Get the yaw input in the nominal range [-STICK_RANGE, STICK_RANGE]. */
int16_t(*getYaw)(void);
/* Get the throttle input in the nominal range [0, THROTTLE_RANGE]. */
uint16_t(*getThrottle)(void);
 
/* Signal quality, by the above SIGNAL_... definitions. */
uint8_t (*getSignalQuality)(void);
 
/* Calibrate sticks to their center positions (only relevant for R/C, really) */
void (*calibrate)(void);
} t_control;
 
/*
* Our output.
*/
extern int16_t controlPitch, controlRoll, controlYaw, controlThrottle;
// extern int16_t stickOffsetNick, stickOffsetRoll;
extern uint16_t maxControlPitch, maxControlRoll;
extern uint8_t looping;
 
void controlMixer_initVariables(void);
void controlMixer_updateVariables(void);
 
void controlMixer_setNeutral(uint8_t calibrate);
 
/*
* Update the exported variables. Called at every flight control cycle.
*/
void controlMixer_update(void);
 
/*
* Get the current command. See the COMMAND_.... define's
*/
uint8_t controlMixer_getCommand(void);
 
void controlMixer_performCalibrationCommands(uint8_t command);
 
uint8_t controlMixer_getSignalQuality(void);
 
#define STICK_GAIN 4
 
/*
* Gets the argument for the current command (a number).
*
* Stick position to argument values (for stick control):
* 2--3--4
* | | +
* 1 9 5 ^ 0
* | | |
* 8--7--6
*
* + <--
* 0
*
* Not in any of these positions: 0
*/
uint8_t controlMixer_getArgument(void);
uint8_t controlMixer_isCommandRepeated(void);
// TODO: Abstract away if possible.
// void controlMixer_setCompassCalState(void);
// void controlMixer_updateCompass(void);
/branches/dongfang_FC_rewrite/dongfangMath.c
0,0 → 1,231
#include "dongfangMath.h"
#include <inttypes.h>
#include <avr/pgmspace.h>
 
const int16_t SIN_TABLE[] PROGMEM = {
(int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.01745240643728351 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.03489949670250097 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.05233595624294383 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.0697564737441253 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.08715574274765817 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.10452846326765346 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.12186934340514748 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.13917310096006544 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.15643446504023087 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.17364817766693033 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.1908089953765448 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.20791169081775931 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.224951054343865 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.24192189559966773 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.25881904510252074 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.27563735581699916 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.29237170472273677 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.3090169943749474 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.32556815445715664 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.3420201433256687 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.35836794954530027 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.374606593415912 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.3907311284892737 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.40673664307580015 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.42261826174069944 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.4383711467890774 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.45399049973954675 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.4694715627858908 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.48480962024633706 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.49999999999999994 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5150380749100542 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5299192642332049 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5446390350150271 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5591929034707469 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.573576436351046 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5877852522924731 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6018150231520483 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6156614753256582 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6293203910498374 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6427876096865393 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6560590289905072 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6691306063588582 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6819983600624985 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6946583704589973 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7071067811865475 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7193398003386511 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7313537016191705 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7431448254773942 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.754709580222772 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.766044443118978 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7771459614569708 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.788010753606722 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7986355100472928 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8090169943749475 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8191520442889918 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8290375725550417 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8386705679454239 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8480480961564261 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8571673007021122 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8660254037844386 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8746197071393957 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8829475928589269 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8910065241883678 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.898794046299167 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9063077870366499 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9135454576426009 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9205048534524403 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9271838545667874 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9335804264972017 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9396926207859083 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9455185755993167 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9510565162951535 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9563047559630354 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9612616959383189 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9659258262890683 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9702957262759965 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9743700647852352 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9781476007338056 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.981627183447664 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.984807753012208 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9876883405951378 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9902680687415703 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.992546151641322 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9945218953682733 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9961946980917455 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9975640502598242 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9986295347545738 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9993908270190958 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.0 * MATH_UNIT_FACTOR) };
 
const int16_t TAN_TABLE[] PROGMEM = {
(int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.017455064928217585 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.03492076949174773 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.0524077792830412 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.06992681194351041 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.08748866352592401 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.10510423526567646 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.1227845609029046 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.14054083470239145 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.15838444032453627 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.17632698070846498 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.19438030913771848 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.2125565616700221 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.23086819112556312 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.24932800284318068 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.2679491924311227 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.2867453857588079 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.3057306814586604 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.3249196962329063 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.3443276132896652 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.36397023426620234 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.3838640350354158 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.4040262258351568 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.4244748162096047 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.4452286853085361 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.4663076581549986 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.48773258856586144 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5095254494944288 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5317094316614788 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.554309051452769 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5773502691896257 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6008606190275604 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6248693519093275 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6494075931975106 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6745085168424267 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7002075382097097 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7265425280053608 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7535540501027942 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7812856265067173 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.809784033195007 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8390996311772799 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8692867378162265 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9004040442978399 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9325150861376615 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9656887748070739 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9999999999999999 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.0355303137905694 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.0723687100246826 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.1106125148291928 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.1503684072210094 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.19175359259421 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.234897156535051 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.2799416321930788 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.3270448216204098 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.3763819204711734 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.4281480067421144 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.4825609685127403 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.5398649638145825 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.6003345290410504 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.6642794823505174 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.7320508075688767 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.8040477552714236 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.8807264653463318 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.9626105055051504 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.050303841579296 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.1445069205095586 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.2460367739042164 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.355852365823752 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.4750868534162964 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.6050890646938005 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.7474774194546216 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.904210877675822 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (3.0776835371752527 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (3.2708526184841404 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (3.4874144438409087 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (3.7320508075688776 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (4.010780933535842 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (4.331475874284157 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (4.704630109478451 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (5.144554015970307 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (5.671281819617707 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (6.313751514675041 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (7.115369722384195 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (8.144346427974593 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (9.514364454222587 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (11.430052302761348 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (14.300666256711896 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (19.08113668772816 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (28.636253282915515 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (32767)
};
 
int16_t int_sin(int32_t arg) {
int8_t sign;
int16_t result;
arg /= MATH_DRG_FACTOR;
arg %= 360;
if (arg < 0) {
arg = -arg;
sign = -1;
} else {
sign = 1;
}
if (arg >= 90) {
arg = 180 - arg;
}
result = pgm_read_word(&SIN_TABLE[(uint8_t) arg]);
return (sign == 1) ? result : -result;
}
 
int16_t int_cos(int32_t arg) {
if (arg > 90L * MATH_DRG_FACTOR)
return int_sin(arg + (90L - 360L) * MATH_DRG_FACTOR);
return int_sin(arg + 90L * MATH_DRG_FACTOR);
}
 
int16_t int_tan(int32_t arg) {
int8_t sign = 1;
int16_t result;
arg /= MATH_DRG_FACTOR;
if (arg >= 90) {
arg = 180 - arg;
sign = -1;
} else if (arg < -90) {
arg += 180;
} else if (arg < 0) {
arg =- arg;
sign = -1;
}
result = pgm_read_word(&TAN_TABLE[(uint8_t) arg]);
return (sign == 1) ? result : -result;
}
/branches/dongfang_FC_rewrite/dongfangMath.h
0,0 → 1,18
#include<inttypes.h>
#include "attitude.h"
 
/*
* Angular unit scaling: Number of units per degree
*/
#define MATH_DRG_FACTOR GYRO_DEG_FACTOR_PITCHROLL
 
/*
* Fix-point decimal scaling: Number of units for 1 (so if sin(something)
* returns UNIT_FACTOR * 0.8, the result is to be understood as 0.8)
* a * sin(b) = (a * int_sin(b * DRG_FACTOR)) / UNIT_FACTOR
*/
#define MATH_UNIT_FACTOR 8192
 
int16_t int_sin(int32_t arg);
int16_t int_cos(int32_t arg);
int16_t int_tan(int32_t arg);
/branches/dongfang_FC_rewrite/dsl.c
0,0 → 1,227
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// This code has been derived from the implementation of Stefan Engelke.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
Copyright (c) 2008 Stefan Engelke <stefan@tinkerer.eu>
 
Permission is hereby granted, free of charge, to any person
obtaining a copy of this software and associated documentation
files (the "Software"), to deal in the Software without
restriction, including without limitation the rights to use, copy,
modify, merge, publish, distribute, sublicense, and/or sell copies
of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
 
The above copyright notice and this permission notice shall be
included in all copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
 
$Id: rcdsl.c 60 2008-08-21 07:50:48Z taser $
 
RCDSL.H and RCDSL.C is an INOFFICIAL implementation of the
communication protocol used by DSL receivers of Act Europe.
The DSL receivers have a serial communication port to connect
two receivers in diversity mode. Each receiver is sending the
received servo signals periodically over this port. This fact
can be used to connect the receiver to the control unit of the
model via UART instead of evaluating the PPM signal.
 
If you have any questions, fell free to send me an e-mail.
 
*/
 
 
/*
Connection of DSL to SV1 of FC:
( DSL Pin1 is on side of channel 4 )
 
1. GND <--> pin 7 (GND)
2. TXD <--> pin 3 (RXD1 Atmega644p)
3. RXD <--> pin 4 (TXD1 Atmega644p) optional
4. 5V <--> pin 2 (5V)
 
Do not connect the receiver via PPM-Sumsignal output the same time.
 
Data are send at every 20 ms @ 38400 Baud 8-N-1
 
Data Frame: |0xFF|0xFF|0x1F|FREQALLOC|??|RSSI|VBAT|??|CRC|10|CH0D1|CH0D0|CH1D1|CH1D0|CRC| ...etc
 
FREQALLOC = 35, 40, 72
RSSI = 0.. 255 // Received signal strength indicator
VBAT = 0...255 // supply voltage (0.0V.. 7.8V)
 
Servo Pair: |0x1X|CHXD1|CHXD0|CHX+1D1|CHX+1D0|CRC|
X is channel index of 1 servo value
D1D0 is servo value as u16 in range of 7373 (1ms) to 14745 (2ms)
there are 8 channels submitted, i.e 4 servo pairs
 
 
Frame examples with signel received
 
FFFF 1F23F079A304AD 1036012B1E6F 122AFB2AECB2 142B4D2B4404 1636872B33CE
FFFF 1F23F079A304AD 1036002B1F6F 122AFE2AEBB0 142B4B2B4406 1636872B33CE
FFFF 1F23F079A304AD 1035FF2B226E 122AFC2AEAB3 142B4E2B4304 1636882B33CD
FFFF 1F23F079A304AD 1036022B1E6E 122AFB2AEEB0 142B4A2B4506 1636872B33CE
FFFF 1F23F079A304AD 1036022B1E6E 122AFE2AEBB0 142B4B2B4406 1636882B33CD
FFFF 1F23F079A304AD 1036012B1E6F 122AFD2AEAB2 142B4E2B4403 1636862B33CF
FFFF 1F23F079A304AD 1036032B1D6E 122AFD2AEBB1 142B4C2B4504 1636862B33CF
 
Frame examples with no signal received
 
FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
*/
 
#include <stdlib.h>
#include "dsl.h"
#include "rc.h"
#include "uart0.h"
 
uint8_t dsl_RSSI = 0;
uint8_t dsl_Battery = 0;
uint8_t dsl_Allocation = 0;
uint8_t PacketBuffer[6];
//uint8_t Jitter = 0; // same measurement as RC_Quality in rc.c
 
typedef union
{
int16_t Servo[2];
uint8_t byte[4];
} ChannelPair_t;
 
ChannelPair_t ChannelPair;
 
 
// This function is called, when a new servo signal is properly received.
// Parameters: servo - servo number (0-9)
// signal - servo signal between 7373 (1ms) and 14745 (2ms)
void dsl_new_signal(uint8_t channel, int16_t signal)
{
int16_t tmp;
uint8_t index = channel + 1; // mk channels start with 1
 
//RC_Quality = (212 * (uint16_t)dsl_RSSI) / 128; // have to be scaled approx. by a factor of 1.66 to get 200 at full level
//if(RC_Quality > 255) RC_Quality = 255;
 
// signal from DSL-receiver is between 7373 (1ms) und 14745 (2ms).
signal-= 11059; // shift to neutral
signal/= 24; // scale to mk rc resolution
 
if(abs(signal-PPM_in[index]) < 6)
{
if(RC_Quality < 200) RC_Quality +=10;
else RC_Quality = 200;
}
 
// calculate exponential history for signal
tmp = (3 * (PPM_in[index]) + signal) / 4;
if(tmp > signal+1) tmp--; else
if(tmp < signal-1) tmp++;
// calculate signal difference on good signal level
if(RC_Quality >= 195) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction
else PPM_diff[index] = 0;
PPM_in[index] = tmp; // update channel value
 
if(index == 4)
{
NewPpmData = 0;
}
}
 
// This function is called within dsl_parser(), when a complete
// data packet with valid checksum has been received.
void dsl_decode_packet(void)
{
uint8_t i;
 
// check for header condition
if((PacketBuffer[0] & 0xF0) == 0x10)
{
if(PacketBuffer[0] == 0x1F) // separate status frame
{
dsl_Allocation = PacketBuffer[1]; // Get frequency allocation
// ?? = PacketBuffer[2];
dsl_RSSI = PacketBuffer[3]; // Get signal quality
dsl_Battery = PacketBuffer[4]; // Get voltage of battery supply
// ?? = PacketBuffer[5];
if(dsl_RSSI == 0)
{
RC_Quality = 0;
for (i = 0; i<5; i++)
{
PPM_diff[i] = 0;
PPM_in[i] = 0;
}
}
}
else // probably a channel pair
{
i = PacketBuffer[0] & 0x0F; // last 4 bits of the header indicates the channel pair
if(i < 10)// maximum 12 channels
{
// big to little endian
ChannelPair.byte[1] = PacketBuffer[1];
ChannelPair.byte[0] = PacketBuffer[2];
ChannelPair.byte[3] = PacketBuffer[3];
ChannelPair.byte[2] = PacketBuffer[4];
dsl_new_signal(i, ChannelPair.Servo[0]);
dsl_new_signal(i+1,ChannelPair.Servo[1]);
}
}
} // EOF header condition
}
 
 
// this function should be called within the UART RX ISR
void dsl_parser(uint8_t c)
{
static uint8_t last_c = 0;
static uint8_t crc = 0;
static uint8_t cnt = 0;
static uint8_t packet_len = 0;
 
// check for sync condition
if ((c==0xFF) && (last_c==0xFF))
{
cnt = 0; // reset byte counter
crc = 0; // reset checksum
return;
}
 
if(cnt == 0) // begin of a packet
{
if(c == 0x1F) packet_len = 5; // a status packet has 5 bytes + crc
else packet_len = 4; // a channel pair packet has 4 bytes + crc
}
if(cnt > packet_len) // packet complete, crc byte received
{
// calculate checksum
crc = ~crc;
if (crc == 0xFF) crc = 0xFE;
// if crc matches decode the packet
if (c == crc) dsl_decode_packet();
// handle next packet
cnt = 0;
crc = 0;
}
else // collect channel data bytes
{
PacketBuffer[cnt++] = c;
crc += c;
}
// store last byte for sync check
last_c = c;
}
/branches/dongfang_FC_rewrite/dsl.h
0,0 → 1,15
#ifndef _DSL_H
#define _DSL_H
 
#include <inttypes.h>
 
extern uint8_t dsl_RSSI; // Received signal strength indicator
extern uint8_t dsl_Battery; // Battery voltage (0-255 [0V - 8.2V])
extern uint8_t dsl_Allocation; // Frequency allocation (35,40,72)
 
#define USART1_BAUD 38400
// this function should be called within the UART RX ISR
extern void dsl_parser(uint8_t c);
 
#endif //_DSL_H
 
/branches/dongfang_FC_rewrite/dynamic_calibration_scrap.txt
0,0 → 1,38
/*
* Here is a dynamic calibration experiment: Adjust integrals and gyro offsets if the pilot appears to be always
* pushing of pulling on the pitch or roll stick.
*/
/*
if(ADCycleCount >= dynamicParams.UserParam2 * 10) {
// This algo works OK on the desk but it is a little sluggish and it oscillates.
// It does not very effectively cancel drift because of dynamics.
minStickForAutoCal = dynamicParams.UserParam3 * 10;
maxStickForAutoCal = dynamicParams.UserParam4 * 10;
// If not already corrected to the limit, and dynamic calibration is enabled:
if (abs(dynamicOffsetPitch - savedDynamicOffsetPitch) < dynamicParams.UserParam1 && !dynamicParams.UserParam6) {
// The pilot pushes on the stick, the integral is > 0, and the gyro val is > 0. Looks like a value-too-high case, so increase the offset.
if (filteredHiResPitchGyro > dynamicOffsetPitch && pitchAngle > 0 && maxStickPitch >= minStickForAutoCal && maxStickPitch <= maxStickForAutoCal) {
dynamicOffsetPitch += (int8_t)(dynamicParams.UserParam7 - 128); // (adding something seems right...)
pitchAngle = (pitchAngle * (int32_t)dynamicParams.UserParam5) / 100L;
} else if (filteredHiResPitchGyro < dynamicOffsetPitch && pitchAngle < 0 && maxStickPitch <= -minStickForAutoCal && maxStickPitch >= -maxStickForAutoCal) {
dynamicOffsetPitch -= (int8_t)(dynamicParams.UserParam7 - 128); // (subtracting something seems right...)
pitchAngle = (pitchAngle * (int32_t)dynamicParams.UserParam5) / 100L;
}
}
// If not already corrected to the limit, and dynamic calibration is enabled:
if (abs(dynamicOffsetRoll - savedDynamicOffsetRoll) <= dynamicParams.UserParam1 && !dynamicParams.UserParam6) {
if (filteredHiResRollGyro > dynamicOffsetRoll && rollAngle > 0 && maxStickRoll >= minStickForAutoCal && maxStickRoll <= maxStickForAutoCal) {
dynamicOffsetRoll += (int8_t)(dynamicParams.UserParam8 - 128);
rollAngle = (rollAngle * (int32_t)dynamicParams.UserParam5) / 100L;
} else if (filteredHiResRollGyro < dynamicOffsetRoll && rollAngle < 0 && maxStickRoll <= -minStickForAutoCal && maxStickRoll >= -maxStickForAutoCal) {
dynamicOffsetRoll -= (int8_t)(dynamicParams.UserParam8 - 128);
rollAngle = (rollAngle * (int32_t)dynamicParams.UserParam5) / 100L;
}
}
ADCycleCount = 0;
}
*/
/branches/dongfang_FC_rewrite/eeprom.c
0,0 → 1,391
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Contant Values
// + 0-250 -> normale Values
// + 251 -> Poti1
// + 252 -> Poti2
// + 253 -> Poti3
// + 254 -> Poti4
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#ifndef EEMEM
#define EEMEM __attribute__ ((section (".eeprom")))
#endif
 
#include <avr/eeprom.h>
#include <string.h>
#include "eeprom.h"
#include "printf_P.h"
#include "output.h"
// TODO: Get rid of these. They have nothing to do with eeprom.
#include "flight.h"
#include "rc.h"
#include "sensors.h"
 
// byte array in eeprom
uint8_t EEPromArray[E2END+1] EEMEM;
 
paramset_t staticParams;
MixerTable_t Mixer;
 
/*
* Default for your own experiments here, so you don't have to reset them
* from MK-Tool all the time.
*/
void setDefaultUserParams(void) {
uint8_t i;
for (i=0; i<sizeof(staticParams.UserParams1); i++) {
staticParams.UserParams1[i] = 0;
}
for (i=0; i<sizeof(staticParams.UserParams2); i++) {
staticParams.UserParams2[i] = 0;
}
}
 
void setOtherDefaults(void) {
staticParams.ChannelAssignment[CH_PITCH] = 2;
staticParams.ChannelAssignment[CH_ROLL] = 1;
staticParams.ChannelAssignment[CH_THROTTLE] = 3;
staticParams.ChannelAssignment[CH_YAW] = 4;
staticParams.ChannelAssignment[CH_POTS+0] = 5;
staticParams.ChannelAssignment[CH_POTS+1] = 6;
staticParams.ChannelAssignment[CH_POTS+2] = 7;
staticParams.ChannelAssignment[CH_POTS+3] = 8;
staticParams.GlobalConfig = CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX;
staticParams.HeightMinGas = 30;
staticParams.MaxHeight = 251;
staticParams.HeightP = 10;
staticParams.HeightD = 30;
staticParams.Height_ACC_Effect = 30;
staticParams.Height_Gain = 4;
staticParams.StickP = 12;
staticParams.StickD = 16;
staticParams.StickYawP = 12;
staticParams.GasMin = 8;
staticParams.GasMax = 230;
staticParams.CompassYawEffect = 128;
staticParams.GyroP = 80;
staticParams.GyroI = 100;
staticParams.LowVoltageWarning = 94;
staticParams.EmergencyGas = 35;
staticParams.EmergencyGasDuration = 30;
staticParams.UfoArrangement = 0;
staticParams.IFactor = 32;
staticParams.ServoPitchControl = 100;
staticParams.ServoPitchComp = 40;
staticParams.ServoPitchCompInvert = 0;
staticParams.ServoPitchMin = 50;
staticParams.ServoPitchMax = 150;
staticParams.ServoRefresh = 5;
staticParams.LoopGasLimit = 50;
staticParams.LoopThreshold = 90;
staticParams.LoopHysteresis = 50;
staticParams.BitConfig = 0;
staticParams.AxisCoupling1 = 90;
staticParams.AxisCoupling2 = 67;
staticParams.AxisCouplingYawCorrection = 0;
staticParams.GyroAccTrim = 2;
staticParams.DynamicStability = 50;
staticParams.J16Bitmask = 95;
staticParams.J17Bitmask = 243;
staticParams.J16Timing = 15;
staticParams.J17Timing = 15;
staticParams.NaviGpsModeControl = 253;
staticParams.NaviGpsGain = 100;
staticParams.NaviGpsP = 90;
staticParams.NaviGpsI = 90;
staticParams.NaviGpsD = 90;
staticParams.NaviGpsPLimit = 75;
staticParams.NaviGpsILimit = 75;
staticParams.NaviGpsDLimit = 75;
staticParams.NaviGpsACC = 0;
staticParams.NaviGpsMinSat = 6;
staticParams.NaviStickThreshold = 8;
staticParams.NaviWindCorrection = 90;
staticParams.NaviSpeedCompensation = 30;
staticParams.NaviOperatingRadius = 100;
staticParams.NaviAngleLimitation = 100;
staticParams.NaviPHLoginTime = 4;
}
 
/***************************************************/
/* Default Values for parameter set 1 */
/***************************************************/
void ParamSet_DefaultSet1(void) { // sport
gyro_setDefaults();
setDefaultUserParams();
setOtherDefaults();
memcpy(staticParams.Name, "Sport\0",6);
}
 
/***************************************************/
/* Default Values for parameter set 2 */
/***************************************************/
void ParamSet_DefaultSet2(void) { // normal
gyro_setDefaults();
setDefaultUserParams();
setOtherDefaults();
staticParams.Height_Gain = 3;
staticParams.GyroAccTrim = 32;
staticParams.J16Timing = 20;
staticParams.J17Timing = 20;
memcpy(staticParams.Name, "Normal\0", 7);
}
 
/***************************************************/
/* Default Values for parameter set 3 */
/***************************************************/
void ParamSet_DefaultSet3(void) { // beginner
gyro_setDefaults();
setDefaultUserParams();
setOtherDefaults();
staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX;
staticParams.Height_Gain = 3;
staticParams.EmergencyGasDuration = 20;
staticParams.AxisCouplingYawCorrection = 70;
staticParams.GyroAccTrim = 32;
staticParams.J16Timing = 30;
staticParams.J17Timing = 30;
memcpy(staticParams.Name, "Beginner\0", 9);
}
 
/***************************************************/
/* Read Parameter from EEPROM as byte */
/***************************************************/
uint8_t GetParamByte(uint16_t param_id) {
return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);
}
 
/***************************************************/
/* Write Parameter to EEPROM as byte */
/***************************************************/
void SetParamByte(uint16_t param_id, uint8_t value) {
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
}
 
/***************************************************/
/* Read Parameter from EEPROM as word */
/***************************************************/
uint16_t GetParamWord(uint16_t param_id) {
return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]);
}
 
/***************************************************/
/* Write Parameter to EEPROM as word */
/***************************************************/
void SetParamWord(uint16_t param_id, uint16_t value) {
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value);
}
 
/***************************************************/
/* Read Parameter Set from EEPROM */
/***************************************************/
// number [1..5]
void ParamSet_ReadFromEEProm(uint8_t setnumber) {
if((1 > setnumber) || (setnumber > 5)) setnumber = 3;
eeprom_read_block((uint8_t *) &staticParams.ChannelAssignment[0], &EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber - 1)], PARAMSET_STRUCT_LEN);
output_init();
}
 
/***************************************************/
/* Write Parameter Set to EEPROM */
/***************************************************/
// number [1..5]
void ParamSet_WriteToEEProm(uint8_t setnumber) {
if(setnumber > 5) setnumber = 5;
if(setnumber < 1) return;
eeprom_write_block((uint8_t *) &staticParams.ChannelAssignment[0], &EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber - 1)], PARAMSET_STRUCT_LEN);
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAMSET_LENGTH], PARAMSET_STRUCT_LEN);
eeprom_write_block( &staticParams.ChannelAssignment[0], &EEPromArray[EEPROM_ADR_CHANNELS], 8); // backup the first 8 bytes that is the rc channel mapping
// set this parameter set to active set
setActiveParamSet(setnumber);
output_init();
}
 
 
/***************************************************/
/* Get active parameter set */
/***************************************************/
uint8_t getActiveParamSet(void) {
uint8_t setnumber;
setnumber = eeprom_read_byte(&EEPromArray[PID_ACTIVE_SET]);
if(setnumber > 5) {
setnumber = 3;
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);
}
return(setnumber);
}
 
/***************************************************/
/* Set active parameter set */
/***************************************************/
void setActiveParamSet(uint8_t setnumber) {
if(setnumber > 5) setnumber = 5;
if(setnumber < 1) setnumber = 1;
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);
}
 
/***************************************************/
/* Read MixerTable from EEPROM */
/***************************************************/
uint8_t MixerTable_ReadFromEEProm(void) {
if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE]) == EEMIXER_REVISION) {
eeprom_read_block((uint8_t *) &Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer));
return 1;
}
else return 0;
}
 
/***************************************************/
/* Write Mixer Table to EEPROM */
/***************************************************/
uint8_t MixerTable_WriteToEEProm(void) {
if(Mixer.Revision == EEMIXER_REVISION) {
eeprom_write_block((uint8_t *) &Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer));
return 1;
}
else return 0;
}
 
/***************************************************/
/* Default Values for Mixer Table */
/***************************************************/
void MixerTable_Default(void) { // Quadro
uint8_t i;
Mixer.Revision = EEMIXER_REVISION;
// clear mixer table (but preset throttle)
for(i = 0; i < 16; i++) {
Mixer.Motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0;
Mixer.Motor[i][MIX_PITCH] = 0;
Mixer.Motor[i][MIX_ROLL] = 0;
Mixer.Motor[i][MIX_YAW] = 0;
}
// default = Quadro
Mixer.Motor[0][MIX_PITCH] = +64; Mixer.Motor[0][MIX_YAW] = +64;
Mixer.Motor[1][MIX_PITCH] = -64; Mixer.Motor[1][MIX_YAW] = +64;
Mixer.Motor[2][MIX_ROLL] = -64; Mixer.Motor[2][MIX_YAW] = -64;
Mixer.Motor[3][MIX_ROLL] = +64; Mixer.Motor[3][MIX_YAW] = -64;
memcpy(Mixer.Name, "Quadro\0", 7);
}
 
/***************************************************/
/* Initialize EEPROM Parameter Sets */
/***************************************************/
void ParamSet_Init(void) {
uint8_t Channel_Backup = 0, i;
// parameter version check
if(eeprom_read_byte(&EEPromArray[PID_PARAM_REVISION]) != EEPARAM_REVISION) {
// if version check faild
printf("\n\rInit Parameter in EEPROM");
eeprom_write_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE], 0xFF); // reset also mixer table
// check if channel mapping backup is valid
if( (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+0]) < 12)
&& (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+1]) < 12)
&& (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+2]) < 12)
&& (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]) < 12)
)
{
Channel_Backup = 1;
}
// fill all 5 parameter settings
for (i=1; i<6; i++) {
switch(i) {
case 1:
ParamSet_DefaultSet1(); // Fill staticParams Structure to default parameter set 1 (Sport)
break;
case 2:
ParamSet_DefaultSet2(); // Kamera
break;
case 3:
ParamSet_DefaultSet3(); // Beginner
break;
default:
ParamSet_DefaultSet2(); // Kamera
break;
}
if(Channel_Backup) { // if we have a rc channel mapping backup in eeprom
// restore it
staticParams.ChannelAssignment[0] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+0]);
staticParams.ChannelAssignment[1] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+1]);
staticParams.ChannelAssignment[2] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+2]);
staticParams.ChannelAssignment[3] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]);
staticParams.ChannelAssignment[4] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+4]);
staticParams.ChannelAssignment[5] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+5]);
staticParams.ChannelAssignment[6] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+6]);
staticParams.ChannelAssignment[7] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+7]);
}
ParamSet_WriteToEEProm(i);
}
// default-Setting is parameter set 3
setActiveParamSet(1);
// update version info
SetParamByte(PID_PARAM_REVISION, EEPARAM_REVISION);
}
// read active parameter set to staticParams stucture
ParamSet_ReadFromEEProm(getActiveParamSet());
printf("\n\rUsing Parameter Set %d", getActiveParamSet());
// load mixer table
if(!MixerTable_ReadFromEEProm()) {
printf("\n\rGenerating default Mixer Table");
MixerTable_Default(); // Quadro
MixerTable_WriteToEEProm();
}
// determine motornumber
RequiredMotors = 0;
for(i = 0; i < 16; i++) {
if(Mixer.Motor[i][MIX_THROTTLE] > 0) RequiredMotors++;
}
printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.Name, RequiredMotors);
printf("\n\r==============================");
}
/branches/dongfang_FC_rewrite/eeprom.h
0,0 → 1,48
#ifndef _EEPROM_H
#define _EEPROM_H
 
#include <inttypes.h>
#include "configuration.h"
 
#define EEPROM_ADR_PARAM_BEGIN 0
#define PID_PARAM_REVISION 1 // byte
#define PID_ACTIVE_SET 2 // byte
#define PID_PRESSURE_OFFSET 3 // byte
#define PID_ACC_NICK 4 // word
#define PID_ACC_ROLL 6 // word
#define PID_ACC_TOP 8 // word
 
#ifdef USE_KILLAGREG
#define PID_MM3_X_OFF 11 // byte
#define PID_MM3_Y_OFF 12 // byte
#define PID_MM3_Z_OFF 13 // byte
#define PID_MM3_X_RANGE 14 // word
#define PID_MM3_Y_RANGE 16 // word
#define PID_MM3_Z_RANGE 18 // word
#endif
 
#define EEPROM_ADR_CHANNELS 80 // 8 bytes
 
#define EEPROM_ADR_PARAMSET_LENGTH 98 // word
#define EEPROM_ADR_PARAMSET_BEGIN 100
#define EEPROM_ADR_MIXER_TABLE 1000 // 1000 - 1076
 
#define EEPARAM_REVISION 75 // is count up, if paramater stucture has changed (compatibility)
#define EEMIXER_REVISION 1 // is count up, if Mixer stucture has changed (compatibility)
 
extern void ParamSet_Init(void);
extern void ParamSet_ReadFromEEProm(uint8_t setnumber);
extern void ParamSet_WriteToEEProm(uint8_t setnumber);
 
extern uint8_t MixerTable_ReadFromEEProm(void);
extern uint8_t MixerTable_WriteToEEProm(void);
 
extern uint8_t GetParamByte(uint16_t param_id);
extern void SetParamByte(uint16_t param_id, uint8_t value);
extern uint16_t GetParamWord(uint16_t param_id);
extern void SetParamWord(uint16_t param_id, uint16_t value);
 
extern uint8_t getActiveParamSet(void);
extern void setActiveParamSet(uint8_t setnumber);
 
#endif //_EEPROM_H
/branches/dongfang_FC_rewrite/externalControl.c
0,0 → 1,69
#include "externalcontrol.h"
#include "configuration.h"
#include "controlMixer.h"
 
ExternalControl_t externalControl;
uint8_t externalControlActive;
 
int16_t EC_PRTY[4];
 
// TODO: Who is going to call this
void EC_setNeutral(void) {
// if necessary. From main.c.
externalControl.config = 0;
externalControl.pitch = 0;
externalControl.roll = 0;
externalControl.yaw = 0;
externalControl.throttle = 0;
// From main.c. What does it do??
externalControl.digital[0] = 0x55;
}
 
int16_t* EC_getPRTY(void) {
return EC_PRTY;
}
 
// not implemented.
uint8_t EC_getArgument(void) {
return 0;
}
 
// not implemented.
uint8_t EC_getCommand(void) {
return COMMAND_NONE;
}
 
// not implemented.
int16_t EC_getVariable(uint8_t varNum) {
return 0;
}
 
void EC_update() {
if (externalControlActive) {
externalControlActive--;
EC_PRTY[CONTROL_PITCH] = (int16_t) externalControl.pitch * (int16_t) staticParams.StickP;
EC_PRTY[CONTROL_ROLL] = externalControl.roll * (int16_t) staticParams.StickP;
EC_PRTY[CONTROL_THROTTLE] = externalControl.throttle;
EC_PRTY[CONTROL_YAW] = externalControl.yaw; // No stickP or similar??????
} else {
EC_PRTY[CONTROL_PITCH] = EC_PRTY[CONTROL_ROLL] = EC_PRTY[CONTROL_THROTTLE] = EC_PRTY[CONTROL_YAW] = 0;
}
}
 
uint8_t EC_getSignalQuality(void) {
if (!(externalControl.config & 0x01 && dynamicParams.ExternalControl > 128))
// External control is not even configured.
return NO_SIGNAL;
 
if (externalControlActive > 100)
// Configured and heard from recently
return SIGNAL_GOOD;
 
if (externalControlActive)
// Configured and heard from
return SIGNAL_OK;
 
// Configured but expired.
return SIGNAL_LOST;
}
/branches/dongfang_FC_rewrite/externalControl.h
0,0 → 1,32
// Or does this simply belong in uart0.h??
#ifndef _EXTERNALCONTROL_H
#define _EXTERNALCONTROL_H
 
#include<inttypes.h>
 
typedef struct {
uint8_t digital[2];
uint8_t eemoteButtons;
int8_t pitch;
int8_t roll;
int8_t yaw;
uint8_t throttle;
int8_t height;
uint8_t free;
uint8_t frame;
uint8_t config;
} __attribute__((packed)) ExternalControl_t;
 
extern ExternalControl_t externalControl;
extern uint8_t externalControlActive;
 
void EC_update(void);
int16_t* EC_getPRTY(void);
uint8_t EC_getArgument(void);
uint8_t EC_getCommand(void);
int16_t EC_getVariable(uint8_t varNum);
void EC_calibrate(void);
uint8_t EC_getSignalQuality (void);
void EC_setNeutral(void);
 
#endif
/branches/dongfang_FC_rewrite/fc-0.74_bugs.txt
0,0 → 1,3
NaviAcc. variabler - overflow
Laesning af A/D values efter AD cycle restart
Den famoese axis coupling bug
/branches/dongfang_FC_rewrite/fc.lst
0,0 → 1,5803
1 .file "fc.c"
2 __SREG__ = 0x3f
3 __SP_H__ = 0x3e
4 __SP_L__ = 0x3d
5 __tmp_reg__ = 0
6 __zero_reg__ = 1
7 .global __do_copy_data
8 .global __do_clear_bss
9 .text
10 .global MotorSmoothing
12 MotorSmoothing:
13 /* prologue: function */
14 /* frame size = 0 */
15 0000 680F add r22,r24
16 0002 791F adc r23,r25
17 0004 77FD sbrc r23,7
18 0006 00C0 rjmp .L5
19 0008 9B01 movw r18,r22
20 000a 3595 asr r19
21 000c 2795 ror r18
22 000e C901 movw r24,r18
23 /* epilogue start */
24 0010 0895 ret
25 .L5:
26 0012 6F5F subi r22,lo8(-(1))
27 0014 7F4F sbci r23,hi8(-(1))
28 0016 9B01 movw r18,r22
29 0018 3595 asr r19
30 001a 2795 ror r18
31 001c C901 movw r24,r18
32 001e 0895 ret
34 .global Mean
36 Mean:
37 0020 2F92 push r2
38 0022 3F92 push r3
39 0024 4F92 push r4
40 0026 5F92 push r5
41 0028 6F92 push r6
42 002a 7F92 push r7
43 002c 8F92 push r8
44 002e 9F92 push r9
45 0030 AF92 push r10
46 0032 BF92 push r11
47 0034 CF92 push r12
48 0036 DF92 push r13
49 0038 EF92 push r14
50 003a FF92 push r15
51 003c 0F93 push r16
52 003e 1F93 push r17
53 0040 CF93 push r28
54 0042 DF93 push r29
55 /* prologue: function */
56 /* frame size = 0 */
57 0044 6091 0000 lds r22,hiResPitchGyro
58 0048 7091 0000 lds r23,(hiResPitchGyro)+1
59 004c E090 0000 lds r14,vibrationOffsetNick
60 0050 F090 0000 lds r15,(vibrationOffsetNick)+1
61 0054 0091 0000 lds r16,(vibrationOffsetNick)+2
62 0058 1091 0000 lds r17,(vibrationOffsetNick)+3
63 005c 8827 clr r24
64 005e 77FD sbrc r23,7
65 0060 8095 com r24
66 0062 982F mov r25,r24
67 0064 6E19 sub r22,r14
68 0066 7F09 sbc r23,r15
69 0068 800B sbc r24,r16
70 006a 910B sbc r25,r17
71 006c 24E0 ldi r18,lo8(4)
72 006e 30E0 ldi r19,hi8(4)
73 0070 40E0 ldi r20,hlo8(4)
74 0072 50E0 ldi r21,hhi8(4)
75 0074 0E94 0000 call __divmodsi4
76 0078 3093 0000 sts (GyroNick)+1,r19
77 007c 2093 0000 sts GyroNick,r18
78 0080 C090 0000 lds r12,filteredHiResPitchGyro
79 0084 D090 0000 lds r13,(filteredHiResPitchGyro)+1
80 0088 6091 0000 lds r22,hiResRollGyro
81 008c 7091 0000 lds r23,(hiResRollGyro)+1
82 0090 2090 0000 lds r2,vibrationOffsetRoll
83 0094 3090 0000 lds r3,(vibrationOffsetRoll)+1
84 0098 4090 0000 lds r4,(vibrationOffsetRoll)+2
85 009c 5090 0000 lds r5,(vibrationOffsetRoll)+3
86 00a0 8827 clr r24
87 00a2 77FD sbrc r23,7
88 00a4 8095 com r24
89 00a6 982F mov r25,r24
90 00a8 6219 sub r22,r2
91 00aa 7309 sbc r23,r3
92 00ac 8409 sbc r24,r4
93 00ae 9509 sbc r25,r5
94 00b0 24E0 ldi r18,lo8(4)
95 00b2 30E0 ldi r19,hi8(4)
96 00b4 40E0 ldi r20,hlo8(4)
97 00b6 50E0 ldi r21,hhi8(4)
98 00b8 0E94 0000 call __divmodsi4
99 00bc 3093 0000 sts (GyroRoll)+1,r19
100 00c0 2093 0000 sts GyroRoll,r18
101 00c4 C091 0000 lds r28,filteredHiResRollGyro
102 00c8 D091 0000 lds r29,(filteredHiResRollGyro)+1
103 00cc 8091 0000 lds r24,hiResPitchGyro
104 00d0 9091 0000 lds r25,(hiResPitchGyro)+1
105 00d4 2091 0000 lds r18,NickNoisePeak
106 00d8 3091 0000 lds r19,(NickNoisePeak)+1
107 00dc 2817 cp r18,r24
108 00de 3907 cpc r19,r25
109 00e0 04F0 brlt .+2
110 00e2 00C0 rjmp .L7
111 00e4 8091 0000 lds r24,hiResPitchGyro
112 00e8 9091 0000 lds r25,(hiResPitchGyro)+1
113 00ec 9093 0000 sts (NickNoisePeak)+1,r25
114 00f0 8093 0000 sts NickNoisePeak,r24
115 .L8:
116 00f4 8091 0000 lds r24,hiResRollGyro
117 00f8 9091 0000 lds r25,(hiResRollGyro)+1
118 00fc 2091 0000 lds r18,RollNoisePeak
119 0100 3091 0000 lds r19,(RollNoisePeak)+1
120 0104 2817 cp r18,r24
121 0106 3907 cpc r19,r25
122 0108 04F0 brlt .+2
123 010a 00C0 rjmp .L9
124 010c 8091 0000 lds r24,hiResRollGyro
125 0110 9091 0000 lds r25,(hiResRollGyro)+1
126 0114 9093 0000 sts (RollNoisePeak)+1,r25
127 0118 8093 0000 sts RollNoisePeak,r24
128 .L10:
129 011c 8091 0000 lds r24,rawYawGyroSum
130 0120 9091 0000 lds r25,(rawYawGyroSum)+1
131 0124 2091 0000 lds r18,vibrationOffsetYaw
132 0128 3091 0000 lds r19,(vibrationOffsetYaw)+1
133 012c 821B sub r24,r18
134 012e 930B sbc r25,r19
135 0130 9093 0000 sts (GyroYaw)+1,r25
136 0134 8093 0000 sts GyroYaw,r24
137 0138 2091 7A00 lds r18,122
138 013c 286C ori r18,lo8(-56)
139 013e 2093 7A00 sts 122,r18
140 0142 1092 0000 sts ADReady,__zero_reg__
141 0146 AA27 clr r26
142 0148 97FD sbrc r25,7
143 014a A095 com r26
144 014c BA2F mov r27,r26
145 014e 2091 0000 lds r18,YawGyroHeading
146 0152 3091 0000 lds r19,(YawGyroHeading)+1
147 0156 4091 0000 lds r20,(YawGyroHeading)+2
148 015a 5091 0000 lds r21,(YawGyroHeading)+3
149 015e 280F add r18,r24
150 0160 391F adc r19,r25
151 0162 4A1F adc r20,r26
152 0164 5B1F adc r21,r27
153 0166 2093 0000 sts YawGyroHeading,r18
154 016a 3093 0000 sts (YawGyroHeading)+1,r19
155 016e 4093 0000 sts (YawGyroHeading)+2,r20
156 0172 5093 0000 sts (YawGyroHeading)+3,r21
157 0176 6090 0000 lds r6,ReadingIntegralGyroYaw
158 017a 7090 0000 lds r7,(ReadingIntegralGyroYaw)+1
159 017e 8090 0000 lds r8,(ReadingIntegralGyroYaw)+2
160 0182 9090 0000 lds r9,(ReadingIntegralGyroYaw)+3
161 0186 680E add r6,r24
162 0188 791E adc r7,r25
163 018a 8A1E adc r8,r26
164 018c 9B1E adc r9,r27
165 018e 6092 0000 sts ReadingIntegralGyroYaw,r6
166 0192 7092 0000 sts (ReadingIntegralGyroYaw)+1,r7
167 0196 8092 0000 sts (ReadingIntegralGyroYaw)+2,r8
168 019a 9092 0000 sts (ReadingIntegralGyroYaw)+3,r9
169 019e 2030 cpi r18,lo8(184320)
170 01a0 80ED ldi r24,hi8(184320)
171 01a2 3807 cpc r19,r24
172 01a4 82E0 ldi r24,hlo8(184320)
173 01a6 4807 cpc r20,r24
174 01a8 80E0 ldi r24,hhi8(184320)
175 01aa 5807 cpc r21,r24
176 01ac 04F0 brlt .L11
177 01ae DA01 movw r26,r20
178 01b0 C901 movw r24,r18
179 01b2 8050 subi r24,lo8(-(-184320))
180 01b4 904D sbci r25,hi8(-(-184320))
181 01b6 A240 sbci r26,hlo8(-(-184320))
182 01b8 B040 sbci r27,hhi8(-(-184320))
183 01ba 8093 0000 sts YawGyroHeading,r24
184 01be 9093 0000 sts (YawGyroHeading)+1,r25
185 01c2 A093 0000 sts (YawGyroHeading)+2,r26
186 01c6 B093 0000 sts (YawGyroHeading)+3,r27
187 01ca 9C01 movw r18,r24
188 01cc AD01 movw r20,r26
189 .L11:
190 01ce 57FD sbrc r21,7
191 01d0 00C0 rjmp .L18
192 .L12:
193 01d2 B601 movw r22,r12
194 01d4 8827 clr r24
195 01d6 77FD sbrc r23,7
196 01d8 8095 com r24
197 01da 982F mov r25,r24
198 01dc 6E19 sub r22,r14
199 01de 7F09 sbc r23,r15
200 01e0 800B sbc r24,r16
201 01e2 910B sbc r25,r17
202 01e4 24E0 ldi r18,lo8(4)
203 01e6 30E0 ldi r19,hi8(4)
204 01e8 40E0 ldi r20,hlo8(4)
205 01ea 50E0 ldi r21,hhi8(4)
206 01ec 0E94 0000 call __divmodsi4
207 01f0 4427 clr r20
208 01f2 37FD sbrc r19,7
209 01f4 4095 com r20
210 01f6 542F mov r21,r20
211 01f8 A090 0000 lds r10,ReadingIntegralGyroNick
212 01fc B090 0000 lds r11,(ReadingIntegralGyroNick)+1
213 0200 C090 0000 lds r12,(ReadingIntegralGyroNick)+2
214 0204 D090 0000 lds r13,(ReadingIntegralGyroNick)+3
215 0208 A20E add r10,r18
216 020a B31E adc r11,r19
217 020c C41E adc r12,r20
218 020e D51E adc r13,r21
219 0210 A092 0000 sts ReadingIntegralGyroNick,r10
220 0214 B092 0000 sts (ReadingIntegralGyroNick)+1,r11
221 0218 C092 0000 sts (ReadingIntegralGyroNick)+2,r12
222 021c D092 0000 sts (ReadingIntegralGyroNick)+3,r13
223 0220 E090 0000 lds r14,TurnOver180Nick
224 0224 F090 0000 lds r15,(TurnOver180Nick)+1
225 0228 0091 0000 lds r16,(TurnOver180Nick)+2
226 022c 1091 0000 lds r17,(TurnOver180Nick)+3
227 0230 EA14 cp r14,r10
228 0232 FB04 cpc r15,r11
229 0234 0C05 cpc r16,r12
230 0236 1D05 cpc r17,r13
231 0238 04F0 brlt .+2
232 023a 00C0 rjmp .L13
233 023c 8827 clr r24
234 023e 9927 clr r25
235 0240 DC01 movw r26,r24
236 0242 8E19 sub r24,r14
237 0244 9F09 sbc r25,r15
238 0246 A00B sbc r26,r16
239 0248 B10B sbc r27,r17
240 024a 8093 0000 sts ReadingIntegralGyroNick,r24
241 024e 9093 0000 sts (ReadingIntegralGyroNick)+1,r25
242 0252 A093 0000 sts (ReadingIntegralGyroNick)+2,r26
243 0256 B093 0000 sts (ReadingIntegralGyroNick)+3,r27
244 025a 5C01 movw r10,r24
245 025c 6D01 movw r12,r26
246 .L14:
247 025e BE01 movw r22,r28
248 0260 8827 clr r24
249 0262 77FD sbrc r23,7
250 0264 8095 com r24
251 0266 982F mov r25,r24
252 0268 6219 sub r22,r2
253 026a 7309 sbc r23,r3
254 026c 8409 sbc r24,r4
255 026e 9509 sbc r25,r5
256 0270 24E0 ldi r18,lo8(4)
257 0272 30E0 ldi r19,hi8(4)
258 0274 40E0 ldi r20,hlo8(4)
259 0276 50E0 ldi r21,hhi8(4)
260 0278 0E94 0000 call __divmodsi4
261 027c 4427 clr r20
262 027e 37FD sbrc r19,7
263 0280 4095 com r20
264 0282 542F mov r21,r20
265 0284 E090 0000 lds r14,ReadingIntegralGyroRoll
266 0288 F090 0000 lds r15,(ReadingIntegralGyroRoll)+1
267 028c 0091 0000 lds r16,(ReadingIntegralGyroRoll)+2
268 0290 1091 0000 lds r17,(ReadingIntegralGyroRoll)+3
269 0294 E20E add r14,r18
270 0296 F31E adc r15,r19
271 0298 041F adc r16,r20
272 029a 151F adc r17,r21
273 029c E092 0000 sts ReadingIntegralGyroRoll,r14
274 02a0 F092 0000 sts (ReadingIntegralGyroRoll)+1,r15
275 02a4 0093 0000 sts (ReadingIntegralGyroRoll)+2,r16
276 02a8 1093 0000 sts (ReadingIntegralGyroRoll)+3,r17
277 02ac 2091 0000 lds r18,TurnOver180Roll
278 02b0 3091 0000 lds r19,(TurnOver180Roll)+1
279 02b4 4091 0000 lds r20,(TurnOver180Roll)+2
280 02b8 5091 0000 lds r21,(TurnOver180Roll)+3
281 02bc 2E15 cp r18,r14
282 02be 3F05 cpc r19,r15
283 02c0 4007 cpc r20,r16
284 02c2 5107 cpc r21,r17
285 02c4 04F4 brge .+2
286 02c6 00C0 rjmp .L19
287 02c8 8827 clr r24
288 02ca 9927 clr r25
289 02cc DC01 movw r26,r24
290 02ce 821B sub r24,r18
291 02d0 930B sbc r25,r19
292 02d2 A40B sbc r26,r20
293 02d4 B50B sbc r27,r21
294 02d6 E816 cp r14,r24
295 02d8 F906 cpc r15,r25
296 02da 0A07 cpc r16,r26
297 02dc 1B07 cpc r17,r27
298 02de 04F4 brge .L16
299 02e0 2093 0000 sts ReadingIntegralGyroRoll,r18
300 02e4 3093 0000 sts (ReadingIntegralGyroRoll)+1,r19
301 02e8 4093 0000 sts (ReadingIntegralGyroRoll)+2,r20
302 02ec 5093 0000 sts (ReadingIntegralGyroRoll)+3,r21
303 02f0 7901 movw r14,r18
304 02f2 8A01 movw r16,r20
305 .L16:
306 02f4 A092 0000 sts IntegralGyroNick,r10
307 02f8 B092 0000 sts (IntegralGyroNick)+1,r11
308 02fc C092 0000 sts (IntegralGyroNick)+2,r12
309 0300 D092 0000 sts (IntegralGyroNick)+3,r13
310 0304 E092 0000 sts IntegralGyroRoll,r14
311 0308 F092 0000 sts (IntegralGyroRoll)+1,r15
312 030c 0093 0000 sts (IntegralGyroRoll)+2,r16
313 0310 1093 0000 sts (IntegralGyroRoll)+3,r17
314 0314 6092 0000 sts IntegralGyroYaw,r6
315 0318 7092 0000 sts (IntegralGyroYaw)+1,r7
316 031c 8092 0000 sts (IntegralGyroYaw)+2,r8
317 0320 9092 0000 sts (IntegralGyroYaw)+3,r9
318 /* epilogue start */
319 0324 DF91 pop r29
320 0326 CF91 pop r28
321 0328 1F91 pop r17
322 032a 0F91 pop r16
323 032c FF90 pop r15
324 032e EF90 pop r14
325 0330 DF90 pop r13
326 0332 CF90 pop r12
327 0334 BF90 pop r11
328 0336 AF90 pop r10
329 0338 9F90 pop r9
330 033a 8F90 pop r8
331 033c 7F90 pop r7
332 033e 6F90 pop r6
333 0340 5F90 pop r5
334 0342 4F90 pop r4
335 0344 3F90 pop r3
336 0346 2F90 pop r2
337 0348 0895 ret
338 .L13:
339 034a 8827 clr r24
340 034c 9927 clr r25
341 034e DC01 movw r26,r24
342 0350 8E19 sub r24,r14
343 0352 9F09 sbc r25,r15
344 0354 A00B sbc r26,r16
345 0356 B10B sbc r27,r17
346 0358 A816 cp r10,r24
347 035a B906 cpc r11,r25
348 035c CA06 cpc r12,r26
349 035e DB06 cpc r13,r27
350 0360 04F0 brlt .+2
351 0362 00C0 rjmp .L14
352 0364 E092 0000 sts ReadingIntegralGyroNick,r14
353 0368 F092 0000 sts (ReadingIntegralGyroNick)+1,r15
354 036c 0093 0000 sts (ReadingIntegralGyroNick)+2,r16
355 0370 1093 0000 sts (ReadingIntegralGyroNick)+3,r17
356 0374 5701 movw r10,r14
357 0376 6801 movw r12,r16
358 0378 00C0 rjmp .L14
359 .L9:
360 037a 8091 0000 lds r24,hiResRollGyro
361 037e 9091 0000 lds r25,(hiResRollGyro)+1
362 0382 9095 com r25
363 0384 8195 neg r24
364 0386 9F4F sbci r25,lo8(-1)
365 0388 2817 cp r18,r24
366 038a 3907 cpc r19,r25
367 038c 04F0 brlt .+2
368 038e 00C0 rjmp .L10
369 0390 8091 0000 lds r24,hiResRollGyro
370 0394 9091 0000 lds r25,(hiResRollGyro)+1
371 0398 9095 com r25
372 039a 8195 neg r24
373 039c 9F4F sbci r25,lo8(-1)
374 039e 9093 0000 sts (RollNoisePeak)+1,r25
375 03a2 8093 0000 sts RollNoisePeak,r24
376 03a6 00C0 rjmp .L10
377 .L7:
378 03a8 8091 0000 lds r24,hiResPitchGyro
379 03ac 9091 0000 lds r25,(hiResPitchGyro)+1
380 03b0 9095 com r25
381 03b2 8195 neg r24
382 03b4 9F4F sbci r25,lo8(-1)
383 03b6 2817 cp r18,r24
384 03b8 3907 cpc r19,r25
385 03ba 04F0 brlt .+2
386 03bc 00C0 rjmp .L8
387 03be 8091 0000 lds r24,hiResPitchGyro
388 03c2 9091 0000 lds r25,(hiResPitchGyro)+1
389 03c6 9095 com r25
390 03c8 8195 neg r24
391 03ca 9F4F sbci r25,lo8(-1)
392 03cc 9093 0000 sts (NickNoisePeak)+1,r25
393 03d0 8093 0000 sts NickNoisePeak,r24
394 03d4 00C0 rjmp .L8
395 .L19:
396 03d6 8827 clr r24
397 03d8 9927 clr r25
398 03da DC01 movw r26,r24
399 03dc 821B sub r24,r18
400 03de 930B sbc r25,r19
401 03e0 A40B sbc r26,r20
402 03e2 B50B sbc r27,r21
403 03e4 8093 0000 sts ReadingIntegralGyroRoll,r24
404 03e8 9093 0000 sts (ReadingIntegralGyroRoll)+1,r25
405 03ec A093 0000 sts (ReadingIntegralGyroRoll)+2,r26
406 03f0 B093 0000 sts (ReadingIntegralGyroRoll)+3,r27
407 03f4 7C01 movw r14,r24
408 03f6 8D01 movw r16,r26
409 03f8 00C0 rjmp .L16
410 .L18:
411 03fa 2050 subi r18,lo8(-(184320))
412 03fc 3043 sbci r19,hi8(-(184320))
413 03fe 4D4F sbci r20,hlo8(-(184320))
414 0400 5F4F sbci r21,hhi8(-(184320))
415 0402 2093 0000 sts YawGyroHeading,r18
416 0406 3093 0000 sts (YawGyroHeading)+1,r19
417 040a 4093 0000 sts (YawGyroHeading)+2,r20
418 040e 5093 0000 sts (YawGyroHeading)+3,r21
419 0412 00C0 rjmp .L12
421 .global ParameterMapping
423 ParameterMapping:
424 /* prologue: function */
425 /* frame size = 0 */
426 0414 8091 0000 lds r24,RC_Quality
427 0418 9091 0000 lds r25,(RC_Quality)+1
428 041c 813A cpi r24,161
429 041e 9105 cpc r25,__zero_reg__
430 0420 04F0 brlt .L162
431 0422 8091 0000 lds r24,ParamSet+11
432 0426 8B3F cpi r24,lo8(-5)
433 0428 00F4 brsh .L169
434 042a 8093 0000 sts FCParam+1,r24
435 .L24:
436 042e 8091 0000 lds r24,ParamSet+10
437 0432 8B3F cpi r24,lo8(-5)
438 0434 00F0 brlo .L27
439 0436 8B3F cpi r24,lo8(-5)
440 0438 01F4 brne .+2
441 043a 00C0 rjmp .L170
442 043c 8C3F cpi r24,lo8(-4)
443 043e 01F4 brne .+2
444 0440 00C0 rjmp .L171
445 0442 8D3F cpi r24,lo8(-3)
446 0444 01F4 brne .+2
447 0446 00C0 rjmp .L172
448 0448 8E3F cpi r24,lo8(-2)
449 044a 01F4 brne .L29
450 044c 8091 0000 lds r24,Poti4
451 0450 8093 0000 sts FCParam,r24
452 0454 00C0 rjmp .L29
453 .L157:
454 0456 8093 0000 sts FCParam+24,r24
455 .L159:
456 045a 6091 0000 lds r22,FCParam+9
457 045e 70E0 ldi r23,lo8(0)
458 0460 6F5F subi r22,lo8(-(1))
459 0462 7F4F sbci r23,hi8(-(1))
460 0464 8CE3 ldi r24,lo8(10300)
461 0466 98E2 ldi r25,hi8(10300)
462 0468 0E94 0000 call __divmodhi4
463 046c 7093 0000 sts (Ki)+1,r23
464 0470 6093 0000 sts Ki,r22
465 .L162:
466 0474 0895 ret
467 .L169:
468 0476 8B3F cpi r24,lo8(-5)
469 0478 01F4 brne .+2
470 047a 00C0 rjmp .L173
471 047c 8C3F cpi r24,lo8(-4)
472 047e 01F4 brne .+2
473 0480 00C0 rjmp .L174
474 0482 8D3F cpi r24,lo8(-3)
475 0484 01F4 brne .+2
476 0486 00C0 rjmp .L175
477 0488 8E3F cpi r24,lo8(-2)
478 048a 01F4 brne .L24
479 048c 8091 0000 lds r24,Poti4
480 0490 8093 0000 sts FCParam+1,r24
481 0494 00C0 rjmp .L24
482 .L27:
483 0496 8093 0000 sts FCParam,r24
484 .L29:
485 049a 8091 0000 lds r24,FCParam
486 049e 8823 tst r24
487 04a0 01F4 brne .L32
488 04a2 1092 0000 sts FCParam,__zero_reg__
489 .L33:
490 04a6 8091 0000 lds r24,ParamSet+12
491 04aa 8B3F cpi r24,lo8(-5)
492 04ac 00F0 brlo .L34
493 .L179:
494 04ae 8B3F cpi r24,lo8(-5)
495 04b0 01F4 brne .+2
496 04b2 00C0 rjmp .L176
497 04b4 8C3F cpi r24,lo8(-4)
498 04b6 01F4 brne .+2
499 04b8 00C0 rjmp .L177
500 04ba 8D3F cpi r24,lo8(-3)
501 04bc 01F4 brne .+2
502 04be 00C0 rjmp .L178
503 04c0 8E3F cpi r24,lo8(-2)
504 04c2 01F4 brne .L36
505 04c4 8091 0000 lds r24,Poti4
506 04c8 8093 0000 sts FCParam+2,r24
507 04cc 00C0 rjmp .L36
508 .L32:
509 04ce 8436 cpi r24,lo8(100)
510 04d0 00F0 brlo .L33
511 04d2 84E6 ldi r24,lo8(100)
512 04d4 8093 0000 sts FCParam,r24
513 04d8 8091 0000 lds r24,ParamSet+12
514 04dc 8B3F cpi r24,lo8(-5)
515 04de 00F4 brsh .L179
516 .L34:
517 04e0 8093 0000 sts FCParam+2,r24
518 .L36:
519 04e4 8091 0000 lds r24,FCParam+2
520 04e8 8823 tst r24
521 04ea 01F4 brne .L39
522 04ec 1092 0000 sts FCParam+2,__zero_reg__
523 .L40:
524 04f0 8091 0000 lds r24,ParamSet+14
525 04f4 8B3F cpi r24,lo8(-5)
526 04f6 00F0 brlo .L41
527 .L183:
528 04f8 8B3F cpi r24,lo8(-5)
529 04fa 01F4 brne .+2
530 04fc 00C0 rjmp .L180
531 04fe 8C3F cpi r24,lo8(-4)
532 0500 01F4 brne .+2
533 0502 00C0 rjmp .L181
534 0504 8D3F cpi r24,lo8(-3)
535 0506 01F4 brne .+2
536 0508 00C0 rjmp .L182
537 050a 8E3F cpi r24,lo8(-2)
538 050c 01F4 brne .L43
539 050e 8091 0000 lds r24,Poti4
540 0512 8093 0000 sts FCParam+3,r24
541 0516 00C0 rjmp .L43
542 .L39:
543 0518 8436 cpi r24,lo8(100)
544 051a 00F0 brlo .L40
545 051c 84E6 ldi r24,lo8(100)
546 051e 8093 0000 sts FCParam+2,r24
547 0522 8091 0000 lds r24,ParamSet+14
548 0526 8B3F cpi r24,lo8(-5)
549 0528 00F4 brsh .L183
550 .L41:
551 052a 8093 0000 sts FCParam+3,r24
552 .L43:
553 052e 8091 0000 lds r24,ParamSet+21
554 0532 8B3F cpi r24,lo8(-5)
555 0534 00F0 brlo .L46
556 0536 8B3F cpi r24,lo8(-5)
557 0538 01F4 brne .+2
558 053a 00C0 rjmp .L184
559 053c 8C3F cpi r24,lo8(-4)
560 053e 01F4 brne .+2
561 0540 00C0 rjmp .L185
562 0542 8D3F cpi r24,lo8(-3)
563 0544 01F4 brne .+2
564 0546 00C0 rjmp .L186
565 0548 8E3F cpi r24,lo8(-2)
566 054a 01F4 brne .L48
567 054c 8091 0000 lds r24,Poti4
568 0550 8093 0000 sts FCParam+4,r24
569 0554 00C0 rjmp .L48
570 .L46:
571 0556 8093 0000 sts FCParam+4,r24
572 .L48:
573 055a 8091 0000 lds r24,ParamSet+22
574 055e 8B3F cpi r24,lo8(-5)
575 0560 00F0 brlo .L51
576 0562 8B3F cpi r24,lo8(-5)
577 0564 01F4 brne .+2
578 0566 00C0 rjmp .L187
579 0568 8C3F cpi r24,lo8(-4)
580 056a 01F4 brne .+2
581 056c 00C0 rjmp .L188
582 056e 8D3F cpi r24,lo8(-3)
583 0570 01F4 brne .+2
584 0572 00C0 rjmp .L189
585 0574 8E3F cpi r24,lo8(-2)
586 0576 01F4 brne .L53
587 0578 8091 0000 lds r24,Poti4
588 057c 8093 0000 sts FCParam+6,r24
589 0580 00C0 rjmp .L53
590 .L51:
591 0582 8093 0000 sts FCParam+6,r24
592 .L53:
593 0586 8091 0000 lds r24,FCParam+6
594 058a 8B30 cpi r24,lo8(11)
595 058c 00F4 brsh .L56
596 058e 8AE0 ldi r24,lo8(10)
597 0590 8093 0000 sts FCParam+6,r24
598 .L57:
599 0594 8091 0000 lds r24,ParamSet+23
600 0598 8B3F cpi r24,lo8(-5)
601 059a 00F0 brlo .L58
602 059c 8B3F cpi r24,lo8(-5)
603 059e 01F4 brne .+2
604 05a0 00C0 rjmp .L190
605 05a2 8C3F cpi r24,lo8(-4)
606 05a4 01F4 brne .+2
607 05a6 00C0 rjmp .L191
608 05a8 8D3F cpi r24,lo8(-3)
609 05aa 01F4 brne .+2
610 05ac 00C0 rjmp .L192
611 05ae 8E3F cpi r24,lo8(-2)
612 05b0 01F4 brne .L60
613 05b2 8091 0000 lds r24,Poti4
614 05b6 8093 0000 sts FCParam+7,r24
615 05ba 00C0 rjmp .L60
616 .L56:
617 05bc 8F3F cpi r24,lo8(-1)
618 05be 01F4 brne .L57
619 05c0 8093 0000 sts FCParam+6,r24
620 05c4 00C0 rjmp .L57
621 .L58:
622 05c6 8093 0000 sts FCParam+7,r24
623 .L60:
624 05ca 8091 0000 lds r24,ParamSet+24
625 05ce 8B3F cpi r24,lo8(-5)
626 05d0 00F0 brlo .L63
627 05d2 8B3F cpi r24,lo8(-5)
628 05d4 01F4 brne .+2
629 05d6 00C0 rjmp .L193
630 05d8 8C3F cpi r24,lo8(-4)
631 05da 01F4 brne .+2
632 05dc 00C0 rjmp .L194
633 05de 8D3F cpi r24,lo8(-3)
634 05e0 01F4 brne .+2
635 05e2 00C0 rjmp .L195
636 05e4 8E3F cpi r24,lo8(-2)
637 05e6 01F4 brne .L65
638 05e8 8091 0000 lds r24,Poti4
639 05ec 8093 0000 sts FCParam+5,r24
640 05f0 00C0 rjmp .L65
641 .L63:
642 05f2 8093 0000 sts FCParam+5,r24
643 .L65:
644 05f6 8091 0000 lds r24,ParamSet+29
645 05fa 8B3F cpi r24,lo8(-5)
646 05fc 00F0 brlo .L68
647 05fe 8B3F cpi r24,lo8(-5)
648 0600 01F4 brne .+2
649 0602 00C0 rjmp .L196
650 0604 8C3F cpi r24,lo8(-4)
651 0606 01F4 brne .+2
652 0608 00C0 rjmp .L197
653 060a 8D3F cpi r24,lo8(-3)
654 060c 01F4 brne .+2
655 060e 00C0 rjmp .L198
656 0610 8E3F cpi r24,lo8(-2)
657 0612 01F4 brne .L70
658 0614 8091 0000 lds r24,Poti4
659 0618 8093 0000 sts FCParam+9,r24
660 061c 00C0 rjmp .L70
661 .L68:
662 061e 8093 0000 sts FCParam+9,r24
663 .L70:
664 0622 8091 0000 lds r24,ParamSet+30
665 0626 8B3F cpi r24,lo8(-5)
666 0628 00F0 brlo .L73
667 062a 8B3F cpi r24,lo8(-5)
668 062c 01F4 brne .+2
669 062e 00C0 rjmp .L199
670 0630 8C3F cpi r24,lo8(-4)
671 0632 01F4 brne .+2
672 0634 00C0 rjmp .L200
673 0636 8D3F cpi r24,lo8(-3)
674 0638 01F4 brne .+2
675 063a 00C0 rjmp .L201
676 063c 8E3F cpi r24,lo8(-2)
677 063e 01F4 brne .L75
678 0640 8091 0000 lds r24,Poti4
679 0644 8093 0000 sts FCParam+10,r24
680 0648 00C0 rjmp .L75
681 .L73:
682 064a 8093 0000 sts FCParam+10,r24
683 .L75:
684 064e 8091 0000 lds r24,ParamSet+31
685 0652 8B3F cpi r24,lo8(-5)
686 0654 00F0 brlo .L78
687 0656 8B3F cpi r24,lo8(-5)
688 0658 01F4 brne .+2
689 065a 00C0 rjmp .L202
690 065c 8C3F cpi r24,lo8(-4)
691 065e 01F4 brne .+2
692 0660 00C0 rjmp .L203
693 0662 8D3F cpi r24,lo8(-3)
694 0664 01F4 brne .+2
695 0666 00C0 rjmp .L204
696 0668 8E3F cpi r24,lo8(-2)
697 066a 01F4 brne .L80
698 066c 8091 0000 lds r24,Poti4
699 0670 8093 0000 sts FCParam+11,r24
700 0674 00C0 rjmp .L80
701 .L78:
702 0676 8093 0000 sts FCParam+11,r24
703 .L80:
704 067a 8091 0000 lds r24,ParamSet+32
705 067e 8B3F cpi r24,lo8(-5)
706 0680 00F0 brlo .L83
707 0682 8B3F cpi r24,lo8(-5)
708 0684 01F4 brne .+2
709 0686 00C0 rjmp .L205
710 0688 8C3F cpi r24,lo8(-4)
711 068a 01F4 brne .+2
712 068c 00C0 rjmp .L206
713 068e 8D3F cpi r24,lo8(-3)
714 0690 01F4 brne .+2
715 0692 00C0 rjmp .L207
716 0694 8E3F cpi r24,lo8(-2)
717 0696 01F4 brne .L85
718 0698 8091 0000 lds r24,Poti4
719 069c 8093 0000 sts FCParam+12,r24
720 06a0 00C0 rjmp .L85
721 .L83:
722 06a2 8093 0000 sts FCParam+12,r24
723 .L85:
724 06a6 8091 0000 lds r24,ParamSet+33
725 06aa 8B3F cpi r24,lo8(-5)
726 06ac 00F0 brlo .L88
727 06ae 8B3F cpi r24,lo8(-5)
728 06b0 01F4 brne .+2
729 06b2 00C0 rjmp .L208
730 06b4 8C3F cpi r24,lo8(-4)
731 06b6 01F4 brne .+2
732 06b8 00C0 rjmp .L209
733 06ba 8D3F cpi r24,lo8(-3)
734 06bc 01F4 brne .+2
735 06be 00C0 rjmp .L210
736 06c0 8E3F cpi r24,lo8(-2)
737 06c2 01F4 brne .L90
738 06c4 8091 0000 lds r24,Poti4
739 06c8 8093 0000 sts FCParam+13,r24
740 06cc 00C0 rjmp .L90
741 .L88:
742 06ce 8093 0000 sts FCParam+13,r24
743 .L90:
744 06d2 8091 0000 lds r24,ParamSet+50
745 06d6 8B3F cpi r24,lo8(-5)
746 06d8 00F0 brlo .L93
747 06da 8B3F cpi r24,lo8(-5)
748 06dc 01F4 brne .+2
749 06de 00C0 rjmp .L211
750 06e0 8C3F cpi r24,lo8(-4)
751 06e2 01F4 brne .+2
752 06e4 00C0 rjmp .L212
753 06e6 8D3F cpi r24,lo8(-3)
754 06e8 01F4 brne .+2
755 06ea 00C0 rjmp .L213
756 06ec 8E3F cpi r24,lo8(-2)
757 06ee 01F4 brne .L95
758 06f0 8091 0000 lds r24,Poti4
759 06f4 8093 0000 sts FCParam+14,r24
760 06f8 00C0 rjmp .L95
761 .L93:
762 06fa 8093 0000 sts FCParam+14,r24
763 .L95:
764 06fe 8091 0000 lds r24,ParamSet+51
765 0702 8B3F cpi r24,lo8(-5)
766 0704 00F0 brlo .L98
767 0706 8B3F cpi r24,lo8(-5)
768 0708 01F4 brne .+2
769 070a 00C0 rjmp .L214
770 070c 8C3F cpi r24,lo8(-4)
771 070e 01F4 brne .+2
772 0710 00C0 rjmp .L215
773 0712 8D3F cpi r24,lo8(-3)
774 0714 01F4 brne .+2
775 0716 00C0 rjmp .L216
776 0718 8E3F cpi r24,lo8(-2)
777 071a 01F4 brne .L100
778 071c 8091 0000 lds r24,Poti4
779 0720 8093 0000 sts FCParam+15,r24
780 0724 00C0 rjmp .L100
781 .L98:
782 0726 8093 0000 sts FCParam+15,r24
783 .L100:
784 072a 8091 0000 lds r24,ParamSet+52
785 072e 8B3F cpi r24,lo8(-5)
786 0730 00F0 brlo .L103
787 0732 8B3F cpi r24,lo8(-5)
788 0734 01F4 brne .+2
789 0736 00C0 rjmp .L217
790 0738 8C3F cpi r24,lo8(-4)
791 073a 01F4 brne .+2
792 073c 00C0 rjmp .L218
793 073e 8D3F cpi r24,lo8(-3)
794 0740 01F4 brne .+2
795 0742 00C0 rjmp .L219
796 0744 8E3F cpi r24,lo8(-2)
797 0746 01F4 brne .L105
798 0748 8091 0000 lds r24,Poti4
799 074c 8093 0000 sts FCParam+16,r24
800 0750 00C0 rjmp .L105
801 .L103:
802 0752 8093 0000 sts FCParam+16,r24
803 .L105:
804 0756 8091 0000 lds r24,ParamSet+53
805 075a 8B3F cpi r24,lo8(-5)
806 075c 00F0 brlo .L108
807 075e 8B3F cpi r24,lo8(-5)
808 0760 01F4 brne .+2
809 0762 00C0 rjmp .L220
810 0764 8C3F cpi r24,lo8(-4)
811 0766 01F4 brne .+2
812 0768 00C0 rjmp .L221
813 076a 8D3F cpi r24,lo8(-3)
814 076c 01F4 brne .+2
815 076e 00C0 rjmp .L222
816 0770 8E3F cpi r24,lo8(-2)
817 0772 01F4 brne .L110
818 0774 8091 0000 lds r24,Poti4
819 0778 8093 0000 sts FCParam+17,r24
820 077c 00C0 rjmp .L110
821 .L108:
822 077e 8093 0000 sts FCParam+17,r24
823 .L110:
824 0782 8091 0000 lds r24,ParamSet+34
825 0786 8B3F cpi r24,lo8(-5)
826 0788 00F0 brlo .L113
827 078a 8B3F cpi r24,lo8(-5)
828 078c 01F4 brne .+2
829 078e 00C0 rjmp .L223
830 0790 8C3F cpi r24,lo8(-4)
831 0792 01F4 brne .+2
832 0794 00C0 rjmp .L224
833 0796 8D3F cpi r24,lo8(-3)
834 0798 01F4 brne .+2
835 079a 00C0 rjmp .L225
836 079c 8E3F cpi r24,lo8(-2)
837 079e 01F4 brne .L115
838 07a0 8091 0000 lds r24,Poti4
839 07a4 8093 0000 sts FCParam+18,r24
840 07a8 00C0 rjmp .L115
841 .L113:
842 07aa 8093 0000 sts FCParam+18,r24
843 .L115:
844 07ae 8091 0000 lds r24,ParamSet+39
845 07b2 8B3F cpi r24,lo8(-5)
846 07b4 00F0 brlo .L118
847 07b6 8B3F cpi r24,lo8(-5)
848 07b8 01F4 brne .+2
849 07ba 00C0 rjmp .L226
850 07bc 8C3F cpi r24,lo8(-4)
851 07be 01F4 brne .+2
852 07c0 00C0 rjmp .L227
853 07c2 8D3F cpi r24,lo8(-3)
854 07c4 01F4 brne .+2
855 07c6 00C0 rjmp .L228
856 07c8 8E3F cpi r24,lo8(-2)
857 07ca 01F4 brne .L120
858 07cc 8091 0000 lds r24,Poti4
859 07d0 8093 0000 sts FCParam+19,r24
860 07d4 00C0 rjmp .L120
861 .L118:
862 07d6 8093 0000 sts FCParam+19,r24
863 .L120:
864 07da 8091 0000 lds r24,ParamSet+42
865 07de 8B3F cpi r24,lo8(-5)
866 07e0 00F0 brlo .L123
867 07e2 8B3F cpi r24,lo8(-5)
868 07e4 01F4 brne .+2
869 07e6 00C0 rjmp .L229
870 07e8 8C3F cpi r24,lo8(-4)
871 07ea 01F4 brne .+2
872 07ec 00C0 rjmp .L230
873 07ee 8D3F cpi r24,lo8(-3)
874 07f0 01F4 brne .+2
875 07f2 00C0 rjmp .L231
876 07f4 8E3F cpi r24,lo8(-2)
877 07f6 01F4 brne .L125
878 07f8 8091 0000 lds r24,Poti4
879 07fc 8093 0000 sts FCParam+20,r24
880 0800 00C0 rjmp .L125
881 .L123:
882 0802 8093 0000 sts FCParam+20,r24
883 .L125:
884 0806 8091 0000 lds r24,ParamSet+43
885 080a 8B3F cpi r24,lo8(-5)
886 080c 00F0 brlo .L128
887 080e 8B3F cpi r24,lo8(-5)
888 0810 01F4 brne .+2
889 0812 00C0 rjmp .L232
890 0814 8C3F cpi r24,lo8(-4)
891 0816 01F4 brne .+2
892 0818 00C0 rjmp .L233
893 081a 8D3F cpi r24,lo8(-3)
894 081c 01F4 brne .+2
895 081e 00C0 rjmp .L234
896 0820 8E3F cpi r24,lo8(-2)
897 0822 01F4 brne .L130
898 0824 8091 0000 lds r24,Poti4
899 0828 8093 0000 sts FCParam+21,r24
900 082c 00C0 rjmp .L130
901 .L128:
902 082e 8093 0000 sts FCParam+21,r24
903 .L130:
904 0832 8091 0000 lds r24,ParamSet+44
905 0836 8B3F cpi r24,lo8(-5)
906 0838 00F0 brlo .L133
907 083a 8B3F cpi r24,lo8(-5)
908 083c 01F4 brne .+2
909 083e 00C0 rjmp .L235
910 0840 8C3F cpi r24,lo8(-4)
911 0842 01F4 brne .+2
912 0844 00C0 rjmp .L236
913 0846 8D3F cpi r24,lo8(-3)
914 0848 01F4 brne .+2
915 084a 00C0 rjmp .L237
916 084c 8E3F cpi r24,lo8(-2)
917 084e 01F4 brne .L135
918 0850 8091 0000 lds r24,Poti4
919 0854 8093 0000 sts FCParam+22,r24
920 0858 00C0 rjmp .L135
921 .L133:
922 085a 8093 0000 sts FCParam+22,r24
923 .L135:
924 085e 8091 0000 lds r24,ParamSet+49
925 0862 8B3F cpi r24,lo8(-5)
926 0864 00F0 brlo .L138
927 0866 8B3F cpi r24,lo8(-5)
928 0868 01F4 brne .+2
929 086a 00C0 rjmp .L238
930 086c 8C3F cpi r24,lo8(-4)
931 086e 01F4 brne .+2
932 0870 00C0 rjmp .L239
933 0872 8D3F cpi r24,lo8(-3)
934 0874 01F4 brne .+2
935 0876 00C0 rjmp .L240
936 0878 8E3F cpi r24,lo8(-2)
937 087a 01F4 brne .L140
938 087c 8091 0000 lds r24,Poti4
939 0880 8093 0000 sts FCParam+23,r24
940 0884 00C0 rjmp .L140
941 .L138:
942 0886 8093 0000 sts FCParam+23,r24
943 .L140:
944 088a 8091 0000 lds r24,ParamSet+55
945 088e 8B3F cpi r24,lo8(-5)
946 0890 00F0 brlo .L143
947 0892 8B3F cpi r24,lo8(-5)
948 0894 01F4 brne .+2
949 0896 00C0 rjmp .L241
950 0898 8C3F cpi r24,lo8(-4)
951 089a 01F4 brne .+2
952 089c 00C0 rjmp .L242
953 089e 8D3F cpi r24,lo8(-3)
954 08a0 01F4 brne .+2
955 08a2 00C0 rjmp .L243
956 08a4 8E3F cpi r24,lo8(-2)
957 08a6 01F4 brne .L145
958 08a8 8091 0000 lds r24,Poti4
959 08ac 8093 0000 sts FCParam+25,r24
960 08b0 00C0 rjmp .L145
961 .L143:
962 08b2 8093 0000 sts FCParam+25,r24
963 .L145:
964 08b6 8091 0000 lds r24,FCParam+25
965 08ba 8230 cpi r24,lo8(2)
966 08bc 00F4 brsh .+2
967 08be 00C0 rjmp .L244
968 08c0 8F3F cpi r24,lo8(-1)
969 08c2 01F4 brne .+2
970 08c4 00C0 rjmp .L245
971 .L149:
972 08c6 8091 0000 lds r24,ParamSet+57
973 08ca 8B3F cpi r24,lo8(-5)
974 08cc 00F0 brlo .L150
975 08ce 8B3F cpi r24,lo8(-5)
976 08d0 01F0 breq .L246
977 08d2 8C3F cpi r24,lo8(-4)
978 08d4 01F4 brne .+2
979 08d6 00C0 rjmp .L247
980 08d8 8D3F cpi r24,lo8(-3)
981 08da 01F4 brne .+2
982 08dc 00C0 rjmp .L248
983 08de 8E3F cpi r24,lo8(-2)
984 08e0 01F4 brne .L152
985 08e2 8091 0000 lds r24,Poti4
986 08e6 8093 0000 sts FCParam+26,r24
987 08ea 00C0 rjmp .L152
988 .L150:
989 08ec 8093 0000 sts FCParam+26,r24
990 .L152:
991 08f0 8091 0000 lds r24,FCParam+26
992 08f4 8230 cpi r24,lo8(2)
993 08f6 00F4 brsh .+2
994 08f8 00C0 rjmp .L249
995 08fa 8F3F cpi r24,lo8(-1)
996 08fc 01F4 brne .+2
997 08fe 00C0 rjmp .L250
998 .L156:
999 0900 8091 0000 lds r24,ParamSet+74
1000 0904 8B3F cpi r24,lo8(-5)
1001 0906 00F4 brsh .+2
1002 0908 00C0 rjmp .L157
1003 090a 8B3F cpi r24,lo8(-5)
1004 090c 01F4 brne .+2
1005 090e 00C0 rjmp .L251
1006 0910 8C3F cpi r24,lo8(-4)
1007 0912 01F4 brne .+2
1008 0914 00C0 rjmp .L252
1009 0916 8D3F cpi r24,lo8(-3)
1010 0918 01F4 brne .+2
1011 091a 00C0 rjmp .L253
1012 091c 8E3F cpi r24,lo8(-2)
1013 091e 01F0 breq .+2
1014 0920 00C0 rjmp .L159
1015 0922 8091 0000 lds r24,Poti4
1016 0926 8093 0000 sts FCParam+24,r24
1017 092a 00C0 rjmp .L159
1018 .L246:
1019 092c 8091 0000 lds r24,Poti1
1020 0930 8093 0000 sts FCParam+26,r24
1021 0934 00C0 rjmp .L152
1022 .L241:
1023 0936 8091 0000 lds r24,Poti1
1024 093a 8093 0000 sts FCParam+25,r24
1025 093e 00C0 rjmp .L145
1026 .L238:
1027 0940 8091 0000 lds r24,Poti1
1028 0944 8093 0000 sts FCParam+23,r24
1029 0948 00C0 rjmp .L140
1030 .L235:
1031 094a 8091 0000 lds r24,Poti1
1032 094e 8093 0000 sts FCParam+22,r24
1033 0952 00C0 rjmp .L135
1034 .L232:
1035 0954 8091 0000 lds r24,Poti1
1036 0958 8093 0000 sts FCParam+21,r24
1037 095c 00C0 rjmp .L130
1038 .L229:
1039 095e 8091 0000 lds r24,Poti1
1040 0962 8093 0000 sts FCParam+20,r24
1041 0966 00C0 rjmp .L125
1042 .L226:
1043 0968 8091 0000 lds r24,Poti1
1044 096c 8093 0000 sts FCParam+19,r24
1045 0970 00C0 rjmp .L120
1046 .L223:
1047 0972 8091 0000 lds r24,Poti1
1048 0976 8093 0000 sts FCParam+18,r24
1049 097a 00C0 rjmp .L115
1050 .L220:
1051 097c 8091 0000 lds r24,Poti1
1052 0980 8093 0000 sts FCParam+17,r24
1053 0984 00C0 rjmp .L110
1054 .L217:
1055 0986 8091 0000 lds r24,Poti1
1056 098a 8093 0000 sts FCParam+16,r24
1057 098e 00C0 rjmp .L105
1058 .L251:
1059 0990 8091 0000 lds r24,Poti1
1060 0994 8093 0000 sts FCParam+24,r24
1061 0998 00C0 rjmp .L159
1062 .L184:
1063 099a 8091 0000 lds r24,Poti1
1064 099e 8093 0000 sts FCParam+4,r24
1065 09a2 00C0 rjmp .L48
1066 .L190:
1067 09a4 8091 0000 lds r24,Poti1
1068 09a8 8093 0000 sts FCParam+7,r24
1069 09ac 00C0 rjmp .L60
1070 .L187:
1071 09ae 8091 0000 lds r24,Poti1
1072 09b2 8093 0000 sts FCParam+6,r24
1073 09b6 00C0 rjmp .L53
1074 .L173:
1075 09b8 8091 0000 lds r24,Poti1
1076 09bc 8093 0000 sts FCParam+1,r24
1077 09c0 00C0 rjmp .L24
1078 .L170:
1079 09c2 8091 0000 lds r24,Poti1
1080 09c6 8093 0000 sts FCParam,r24
1081 09ca 00C0 rjmp .L29
1082 .L176:
1083 09cc 8091 0000 lds r24,Poti1
1084 09d0 8093 0000 sts FCParam+2,r24
1085 09d4 00C0 rjmp .L36
1086 .L180:
1087 09d6 8091 0000 lds r24,Poti1
1088 09da 8093 0000 sts FCParam+3,r24
1089 09de 00C0 rjmp .L43
1090 .L193:
1091 09e0 8091 0000 lds r24,Poti1
1092 09e4 8093 0000 sts FCParam+5,r24
1093 09e8 00C0 rjmp .L65
1094 .L196:
1095 09ea 8091 0000 lds r24,Poti1
1096 09ee 8093 0000 sts FCParam+9,r24
1097 09f2 00C0 rjmp .L70
1098 .L202:
1099 09f4 8091 0000 lds r24,Poti1
1100 09f8 8093 0000 sts FCParam+11,r24
1101 09fc 00C0 rjmp .L80
1102 .L199:
1103 09fe 8091 0000 lds r24,Poti1
1104 0a02 8093 0000 sts FCParam+10,r24
1105 0a06 00C0 rjmp .L75
1106 .L214:
1107 0a08 8091 0000 lds r24,Poti1
1108 0a0c 8093 0000 sts FCParam+15,r24
1109 0a10 00C0 rjmp .L100
1110 .L211:
1111 0a12 8091 0000 lds r24,Poti1
1112 0a16 8093 0000 sts FCParam+14,r24
1113 0a1a 00C0 rjmp .L95
1114 .L208:
1115 0a1c 8091 0000 lds r24,Poti1
1116 0a20 8093 0000 sts FCParam+13,r24
1117 0a24 00C0 rjmp .L90
1118 .L205:
1119 0a26 8091 0000 lds r24,Poti1
1120 0a2a 8093 0000 sts FCParam+12,r24
1121 0a2e 00C0 rjmp .L85
1122 .L249:
1123 0a30 81E0 ldi r24,lo8(1)
1124 0a32 8093 0000 sts FCParam+26,r24
1125 0a36 00C0 rjmp .L156
1126 .L244:
1127 0a38 81E0 ldi r24,lo8(1)
1128 0a3a 8093 0000 sts FCParam+25,r24
1129 0a3e 00C0 rjmp .L149
1130 .L245:
1131 0a40 8093 0000 sts FCParam+25,r24
1132 0a44 00C0 rjmp .L149
1133 .L250:
1134 0a46 8093 0000 sts FCParam+26,r24
1135 0a4a 00C0 rjmp .L156
1136 .L177:
1137 0a4c 8091 0000 lds r24,Poti2
1138 0a50 8093 0000 sts FCParam+2,r24
1139 0a54 00C0 rjmp .L36
1140 .L171:
1141 0a56 8091 0000 lds r24,Poti2
1142 0a5a 8093 0000 sts FCParam,r24
1143 0a5e 00C0 rjmp .L29
1144 .L206:
1145 0a60 8091 0000 lds r24,Poti2
1146 0a64 8093 0000 sts FCParam+12,r24
1147 0a68 00C0 rjmp .L85
1148 .L212:
1149 0a6a 8091 0000 lds r24,Poti2
1150 0a6e 8093 0000 sts FCParam+14,r24
1151 0a72 00C0 rjmp .L95
1152 .L209:
1153 0a74 8091 0000 lds r24,Poti2
1154 0a78 8093 0000 sts FCParam+13,r24
1155 0a7c 00C0 rjmp .L90
1156 .L194:
1157 0a7e 8091 0000 lds r24,Poti2
1158 0a82 8093 0000 sts FCParam+5,r24
1159 0a86 00C0 rjmp .L65
1160 .L181:
1161 0a88 8091 0000 lds r24,Poti2
1162 0a8c 8093 0000 sts FCParam+3,r24
1163 0a90 00C0 rjmp .L43
1164 .L185:
1165 0a92 8091 0000 lds r24,Poti2
1166 0a96 8093 0000 sts FCParam+4,r24
1167 0a9a 00C0 rjmp .L48
1168 .L197:
1169 0a9c 8091 0000 lds r24,Poti2
1170 0aa0 8093 0000 sts FCParam+9,r24
1171 0aa4 00C0 rjmp .L70
1172 .L200:
1173 0aa6 8091 0000 lds r24,Poti2
1174 0aaa 8093 0000 sts FCParam+10,r24
1175 0aae 00C0 rjmp .L75
1176 .L203:
1177 0ab0 8091 0000 lds r24,Poti2
1178 0ab4 8093 0000 sts FCParam+11,r24
1179 0ab8 00C0 rjmp .L80
1180 .L227:
1181 0aba 8091 0000 lds r24,Poti2
1182 0abe 8093 0000 sts FCParam+19,r24
1183 0ac2 00C0 rjmp .L120
1184 .L233:
1185 0ac4 8091 0000 lds r24,Poti2
1186 0ac8 8093 0000 sts FCParam+21,r24
1187 0acc 00C0 rjmp .L130
1188 .L230:
1189 0ace 8091 0000 lds r24,Poti2
1190 0ad2 8093 0000 sts FCParam+20,r24
1191 0ad6 00C0 rjmp .L125
1192 .L218:
1193 0ad8 8091 0000 lds r24,Poti2
1194 0adc 8093 0000 sts FCParam+16,r24
1195 0ae0 00C0 rjmp .L105
1196 .L224:
1197 0ae2 8091 0000 lds r24,Poti2
1198 0ae6 8093 0000 sts FCParam+18,r24
1199 0aea 00C0 rjmp .L115
1200 .L221:
1201 0aec 8091 0000 lds r24,Poti2
1202 0af0 8093 0000 sts FCParam+17,r24
1203 0af4 00C0 rjmp .L110
1204 .L236:
1205 0af6 8091 0000 lds r24,Poti2
1206 0afa 8093 0000 sts FCParam+22,r24
1207 0afe 00C0 rjmp .L135
1208 .L215:
1209 0b00 8091 0000 lds r24,Poti2
1210 0b04 8093 0000 sts FCParam+15,r24
1211 0b08 00C0 rjmp .L100
1212 .L174:
1213 0b0a 8091 0000 lds r24,Poti2
1214 0b0e 8093 0000 sts FCParam+1,r24
1215 0b12 00C0 rjmp .L24
1216 .L191:
1217 0b14 8091 0000 lds r24,Poti2
1218 0b18 8093 0000 sts FCParam+7,r24
1219 0b1c 00C0 rjmp .L60
1220 .L188:
1221 0b1e 8091 0000 lds r24,Poti2
1222 0b22 8093 0000 sts FCParam+6,r24
1223 0b26 00C0 rjmp .L53
1224 .L252:
1225 0b28 8091 0000 lds r24,Poti2
1226 0b2c 8093 0000 sts FCParam+24,r24
1227 0b30 00C0 rjmp .L159
1228 .L247:
1229 0b32 8091 0000 lds r24,Poti2
1230 0b36 8093 0000 sts FCParam+26,r24
1231 0b3a 00C0 rjmp .L152
1232 .L242:
1233 0b3c 8091 0000 lds r24,Poti2
1234 0b40 8093 0000 sts FCParam+25,r24
1235 0b44 00C0 rjmp .L145
1236 .L239:
1237 0b46 8091 0000 lds r24,Poti2
1238 0b4a 8093 0000 sts FCParam+23,r24
1239 0b4e 00C0 rjmp .L140
1240 .L248:
1241 0b50 8091 0000 lds r24,Poti3
1242 0b54 8093 0000 sts FCParam+26,r24
1243 0b58 00C0 rjmp .L152
1244 .L253:
1245 0b5a 8091 0000 lds r24,Poti3
1246 0b5e 8093 0000 sts FCParam+24,r24
1247 0b62 00C0 rjmp .L159
1248 .L219:
1249 0b64 8091 0000 lds r24,Poti3
1250 0b68 8093 0000 sts FCParam+16,r24
1251 0b6c 00C0 rjmp .L105
1252 .L231:
1253 0b6e 8091 0000 lds r24,Poti3
1254 0b72 8093 0000 sts FCParam+20,r24
1255 0b76 00C0 rjmp .L125
1256 .L222:
1257 0b78 8091 0000 lds r24,Poti3
1258 0b7c 8093 0000 sts FCParam+17,r24
1259 0b80 00C0 rjmp .L110
1260 .L234:
1261 0b82 8091 0000 lds r24,Poti3
1262 0b86 8093 0000 sts FCParam+21,r24
1263 0b8a 00C0 rjmp .L130
1264 .L240:
1265 0b8c 8091 0000 lds r24,Poti3
1266 0b90 8093 0000 sts FCParam+23,r24
1267 0b94 00C0 rjmp .L140
1268 .L243:
1269 0b96 8091 0000 lds r24,Poti3
1270 0b9a 8093 0000 sts FCParam+25,r24
1271 0b9e 00C0 rjmp .L145
1272 .L225:
1273 0ba0 8091 0000 lds r24,Poti3
1274 0ba4 8093 0000 sts FCParam+18,r24
1275 0ba8 00C0 rjmp .L115
1276 .L237:
1277 0baa 8091 0000 lds r24,Poti3
1278 0bae 8093 0000 sts FCParam+22,r24
1279 0bb2 00C0 rjmp .L135
1280 .L178:
1281 0bb4 8091 0000 lds r24,Poti3
1282 0bb8 8093 0000 sts FCParam+2,r24
1283 0bbc 00C0 rjmp .L36
1284 .L201:
1285 0bbe 8091 0000 lds r24,Poti3
1286 0bc2 8093 0000 sts FCParam+10,r24
1287 0bc6 00C0 rjmp .L75
1288 .L186:
1289 0bc8 8091 0000 lds r24,Poti3
1290 0bcc 8093 0000 sts FCParam+4,r24
1291 0bd0 00C0 rjmp .L48
1292 .L207:
1293 0bd2 8091 0000 lds r24,Poti3
1294 0bd6 8093 0000 sts FCParam+12,r24
1295 0bda 00C0 rjmp .L85
1296 .L216:
1297 0bdc 8091 0000 lds r24,Poti3
1298 0be0 8093 0000 sts FCParam+15,r24
1299 0be4 00C0 rjmp .L100
1300 .L228:
1301 0be6 8091 0000 lds r24,Poti3
1302 0bea 8093 0000 sts FCParam+19,r24
1303 0bee 00C0 rjmp .L120
1304 .L192:
1305 0bf0 8091 0000 lds r24,Poti3
1306 0bf4 8093 0000 sts FCParam+7,r24
1307 0bf8 00C0 rjmp .L60
1308 .L213:
1309 0bfa 8091 0000 lds r24,Poti3
1310 0bfe 8093 0000 sts FCParam+14,r24
1311 0c02 00C0 rjmp .L95
1312 .L172:
1313 0c04 8091 0000 lds r24,Poti3
1314 0c08 8093 0000 sts FCParam,r24
1315 0c0c 00C0 rjmp .L29
1316 .L198:
1317 0c0e 8091 0000 lds r24,Poti3
1318 0c12 8093 0000 sts FCParam+9,r24
1319 0c16 00C0 rjmp .L70
1320 .L182:
1321 0c18 8091 0000 lds r24,Poti3
1322 0c1c 8093 0000 sts FCParam+3,r24
1323 0c20 00C0 rjmp .L43
1324 .L204:
1325 0c22 8091 0000 lds r24,Poti3
1326 0c26 8093 0000 sts FCParam+11,r24
1327 0c2a 00C0 rjmp .L80
1328 .L175:
1329 0c2c 8091 0000 lds r24,Poti3
1330 0c30 8093 0000 sts FCParam+1,r24
1331 0c34 00C0 rjmp .L24
1332 .L195:
1333 0c36 8091 0000 lds r24,Poti3
1334 0c3a 8093 0000 sts FCParam+5,r24
1335 0c3e 00C0 rjmp .L65
1336 .L189:
1337 0c40 8091 0000 lds r24,Poti3
1338 0c44 8093 0000 sts FCParam+6,r24
1339 0c48 00C0 rjmp .L53
1340 .L210:
1341 0c4a 8091 0000 lds r24,Poti3
1342 0c4e 8093 0000 sts FCParam+13,r24
1343 0c52 00C0 rjmp .L90
1345 .global SendMotorData
1347 SendMotorData:
1348 /* prologue: function */
1349 /* frame size = 0 */
1350 0c54 8091 0000 lds r24,MKFlags
1351 0c58 80FD sbrc r24,0
1352 0c5a 00C0 rjmp .L255
1353 0c5c 8091 0000 lds r24,MKFlags
1354 0c60 857F andi r24,lo8(-11)
1355 0c62 8093 0000 sts MKFlags,r24
1356 0c66 9091 0000 lds r25,MotorTest_Active
1357 0c6a E0E0 ldi r30,lo8(Motor)
1358 0c6c F0E0 ldi r31,hi8(Motor)
1359 0c6e A0E0 ldi r26,lo8(MotorTest)
1360 0c70 B0E0 ldi r27,hi8(MotorTest)
1361 0c72 00C0 rjmp .L258
1362 .L263:
1363 0c74 1082 st Z,__zero_reg__
1364 0c76 3596 adiw r30,5
1365 0c78 1196 adiw r26,1
1366 0c7a 80E0 ldi r24,hi8(Motor+60)
1367 0c7c E030 cpi r30,lo8(Motor+60)
1368 0c7e F807 cpc r31,r24
1369 0c80 01F0 breq .L262
1370 .L258:
1371 0c82 9923 tst r25
1372 0c84 01F0 breq .L263
1373 0c86 8C91 ld r24,X
1374 0c88 8083 st Z,r24
1375 0c8a 3596 adiw r30,5
1376 0c8c 1196 adiw r26,1
1377 0c8e 80E0 ldi r24,hi8(Motor+60)
1378 0c90 E030 cpi r30,lo8(Motor+60)
1379 0c92 F807 cpc r31,r24
1380 0c94 01F4 brne .L258
1381 .L262:
1382 0c96 9923 tst r25
1383 0c98 01F0 breq .L255
1384 0c9a 9150 subi r25,lo8(-(-1))
1385 0c9c 9093 0000 sts MotorTest_Active,r25
1386 .L255:
1387 0ca0 E0E0 ldi r30,lo8(DebugOut+26)
1388 0ca2 F0E0 ldi r31,hi8(DebugOut+26)
1389 0ca4 8091 0000 lds r24,Motor
1390 0ca8 8093 0000 sts DebugOut+26,r24
1391 0cac 1092 0000 sts DebugOut+27,__zero_reg__
1392 0cb0 8091 0000 lds r24,Motor+5
1393 0cb4 8093 0000 sts DebugOut+28,r24
1394 0cb8 1382 std Z+3,__zero_reg__
1395 0cba 8091 0000 lds r24,Motor+15
1396 0cbe 8093 0000 sts DebugOut+30,r24
1397 0cc2 1582 std Z+5,__zero_reg__
1398 0cc4 8091 0000 lds r24,Motor+10
1399 0cc8 8093 0000 sts DebugOut+32,r24
1400 0ccc 1782 std Z+7,__zero_reg__
1401 0cce 80E0 ldi r24,lo8(0)
1402 0cd0 0E94 0000 call I2C_Start
1403 /* epilogue start */
1404 0cd4 0895 ret
1406 .global SetNeutral
1408 SetNeutral:
1409 0cd6 1F93 push r17
1410 /* prologue: function */
1411 /* frame size = 0 */
1412 0cd8 182F mov r17,r24
1413 0cda 0E94 0000 call Servo_Off
1414 0cde 1092 0000 sts FCParam+20,__zero_reg__
1415 0ce2 1092 0000 sts FCParam+21,__zero_reg__
1416 0ce6 0E94 0000 call analog_calibrate
1417 0cea 1092 0000 sts (GyroNick)+1,__zero_reg__
1418 0cee 1092 0000 sts GyroNick,__zero_reg__
1419 0cf2 1092 0000 sts (GyroRoll)+1,__zero_reg__
1420 0cf6 1092 0000 sts GyroRoll,__zero_reg__
1421 0cfa 1092 0000 sts (GyroYaw)+1,__zero_reg__
1422 0cfe 1092 0000 sts GyroYaw,__zero_reg__
1423 0d02 1092 0000 sts IntegralGyroNick,__zero_reg__
1424 0d06 1092 0000 sts (IntegralGyroNick)+1,__zero_reg__
1425 0d0a 1092 0000 sts (IntegralGyroNick)+2,__zero_reg__
1426 0d0e 1092 0000 sts (IntegralGyroNick)+3,__zero_reg__
1427 0d12 1092 0000 sts ReadingIntegralGyroNick,__zero_reg__
1428 0d16 1092 0000 sts (ReadingIntegralGyroNick)+1,__zero_reg__
1429 0d1a 1092 0000 sts (ReadingIntegralGyroNick)+2,__zero_reg__
1430 0d1e 1092 0000 sts (ReadingIntegralGyroNick)+3,__zero_reg__
1431 0d22 1092 0000 sts IntegralGyroRoll,__zero_reg__
1432 0d26 1092 0000 sts (IntegralGyroRoll)+1,__zero_reg__
1433 0d2a 1092 0000 sts (IntegralGyroRoll)+2,__zero_reg__
1434 0d2e 1092 0000 sts (IntegralGyroRoll)+3,__zero_reg__
1435 0d32 1092 0000 sts ReadingIntegralGyroRoll,__zero_reg__
1436 0d36 1092 0000 sts (ReadingIntegralGyroRoll)+1,__zero_reg__
1437 0d3a 1092 0000 sts (ReadingIntegralGyroRoll)+2,__zero_reg__
1438 0d3e 1092 0000 sts (ReadingIntegralGyroRoll)+3,__zero_reg__
1439 0d42 1092 0000 sts IntegralGyroYaw,__zero_reg__
1440 0d46 1092 0000 sts (IntegralGyroYaw)+1,__zero_reg__
1441 0d4a 1092 0000 sts (IntegralGyroYaw)+2,__zero_reg__
1442 0d4e 1092 0000 sts (IntegralGyroYaw)+3,__zero_reg__
1443 0d52 1092 0000 sts ReadingIntegralGyroYaw,__zero_reg__
1444 0d56 1092 0000 sts (ReadingIntegralGyroYaw)+1,__zero_reg__
1445 0d5a 1092 0000 sts (ReadingIntegralGyroYaw)+2,__zero_reg__
1446 0d5e 1092 0000 sts (ReadingIntegralGyroYaw)+3,__zero_reg__
1447 0d62 8091 0000 lds r24,CompassHeading
1448 0d66 9091 0000 lds r25,(CompassHeading)+1
1449 0d6a 9093 0000 sts (CompassCourse)+1,r25
1450 0d6e 8093 0000 sts CompassCourse,r24
1451 0d72 AA27 clr r26
1452 0d74 97FD sbrc r25,7
1453 0d76 A095 com r26
1454 0d78 BA2F mov r27,r26
1455 0d7a 59E0 ldi r21,9
1456 0d7c 880F 1: lsl r24
1457 0d7e 991F rol r25
1458 0d80 AA1F rol r26
1459 0d82 BB1F rol r27
1460 0d84 5A95 dec r21
1461 0d86 01F4 brne 1b
1462 0d88 8093 0000 sts YawGyroHeading,r24
1463 0d8c 9093 0000 sts (YawGyroHeading)+1,r25
1464 0d90 A093 0000 sts (YawGyroHeading)+2,r26
1465 0d94 B093 0000 sts (YawGyroHeading)+3,r27
1466 0d98 82E3 ldi r24,lo8(50)
1467 0d9a 90E0 ldi r25,hi8(50)
1468 0d9c 9093 0000 sts (BeepTime)+1,r25
1469 0da0 8093 0000 sts BeepTime,r24
1470 0da4 1092 0000 sts (GPSStickNick)+1,__zero_reg__
1471 0da8 1092 0000 sts GPSStickNick,__zero_reg__
1472 0dac 1092 0000 sts (GPSStickRoll)+1,__zero_reg__
1473 0db0 1092 0000 sts GPSStickRoll,__zero_reg__
1474 0db4 8091 0000 lds r24,MKFlags
1475 0db8 8460 ori r24,lo8(4)
1476 0dba 8093 0000 sts MKFlags,r24
1477 0dbe 8FEF ldi r24,lo8(-1)
1478 0dc0 8093 0000 sts FCParam+27,r24
1479 0dc4 1092 0000 sts FCParam+28,__zero_reg__
1480 0dc8 80E2 ldi r24,lo8(32)
1481 0dca 8093 0000 sts FCParam+29,r24
1482 0dce E091 0000 lds r30,ParamSet+4
1483 0dd2 F0E0 ldi r31,lo8(0)
1484 0dd4 EE0F lsl r30
1485 0dd6 FF1F rol r31
1486 0dd8 E050 subi r30,lo8(-(PPM_in))
1487 0dda F040 sbci r31,hi8(-(PPM_in))
1488 0ddc 8081 ld r24,Z
1489 0dde 9181 ldd r25,Z+1
1490 0de0 8259 subi r24,lo8(-(110))
1491 0de2 9F4F sbci r25,hi8(-(110))
1492 0de4 9093 0000 sts (Poti1)+1,r25
1493 0de8 8093 0000 sts Poti1,r24
1494 0dec E091 0000 lds r30,ParamSet+5
1495 0df0 F0E0 ldi r31,lo8(0)
1496 0df2 EE0F lsl r30
1497 0df4 FF1F rol r31
1498 0df6 E050 subi r30,lo8(-(PPM_in))
1499 0df8 F040 sbci r31,hi8(-(PPM_in))
1500 0dfa 8081 ld r24,Z
1501 0dfc 9181 ldd r25,Z+1
1502 0dfe 8259 subi r24,lo8(-(110))
1503 0e00 9F4F sbci r25,hi8(-(110))
1504 0e02 9093 0000 sts (Poti2)+1,r25
1505 0e06 8093 0000 sts Poti2,r24
1506 0e0a E091 0000 lds r30,ParamSet+6
1507 0e0e F0E0 ldi r31,lo8(0)
1508 0e10 EE0F lsl r30
1509 0e12 FF1F rol r31
1510 0e14 E050 subi r30,lo8(-(PPM_in))
1511 0e16 F040 sbci r31,hi8(-(PPM_in))
1512 0e18 8081 ld r24,Z
1513 0e1a 9181 ldd r25,Z+1
1514 0e1c 8259 subi r24,lo8(-(110))
1515 0e1e 9F4F sbci r25,hi8(-(110))
1516 0e20 9093 0000 sts (Poti3)+1,r25
1517 0e24 8093 0000 sts Poti3,r24
1518 0e28 E091 0000 lds r30,ParamSet+7
1519 0e2c F0E0 ldi r31,lo8(0)
1520 0e2e EE0F lsl r30
1521 0e30 FF1F rol r31
1522 0e32 E050 subi r30,lo8(-(PPM_in))
1523 0e34 F040 sbci r31,hi8(-(PPM_in))
1524 0e36 8081 ld r24,Z
1525 0e38 9181 ldd r25,Z+1
1526 0e3a 8259 subi r24,lo8(-(110))
1527 0e3c 9F4F sbci r25,hi8(-(110))
1528 0e3e 9093 0000 sts (Poti4)+1,r25
1529 0e42 8093 0000 sts Poti4,r24
1530 0e46 1123 tst r17
1531 0e48 01F0 breq .L265
1532 0e4a 8091 0000 lds r24,stickOffsetNick
1533 0e4e 9091 0000 lds r25,(stickOffsetNick)+1
1534 0e52 2091 0000 lds r18,StickNick
1535 0e56 3091 0000 lds r19,(StickNick)+1
1536 0e5a 820F add r24,r18
1537 0e5c 931F adc r25,r19
1538 0e5e 9093 0000 sts (stickOffsetNick)+1,r25
1539 0e62 8093 0000 sts stickOffsetNick,r24
1540 0e66 8091 0000 lds r24,stickOffsetRoll
1541 0e6a 9091 0000 lds r25,(stickOffsetRoll)+1
1542 0e6e 2091 0000 lds r18,StickRoll
1543 0e72 3091 0000 lds r19,(StickRoll)+1
1544 0e76 820F add r24,r18
1545 0e78 931F adc r25,r19
1546 0e7a 9093 0000 sts (stickOffsetRoll)+1,r25
1547 0e7e 8093 0000 sts stickOffsetRoll,r24
1548 .L265:
1549 0e82 0E94 0000 call Servo_On
1550 0e86 84E6 ldi r24,lo8(100)
1551 0e88 90E0 ldi r25,hi8(100)
1552 0e8a 9093 0000 sts (RC_Quality)+1,r25
1553 0e8e 8093 0000 sts RC_Quality,r24
1554 /* epilogue start */
1555 0e92 1F91 pop r17
1556 0e94 0895 ret
1558 .global Beep
1560 Beep:
1561 0e96 1F93 push r17
1562 0e98 CF93 push r28
1563 0e9a DF93 push r29
1564 /* prologue: function */
1565 /* frame size = 0 */
1566 0e9c 982F mov r25,r24
1567 0e9e 8823 tst r24
1568 0ea0 01F0 breq .L271
1569 0ea2 8091 0000 lds r24,MKFlags
1570 0ea6 80FD sbrc r24,0
1571 0ea8 00C0 rjmp .L271
1572 0eaa 192F mov r17,r25
1573 0eac 1150 subi r17,lo8(-(-1))
1574 0eae C4E6 ldi r28,lo8(100)
1575 0eb0 D0E0 ldi r29,hi8(100)
1576 0eb2 00C0 rjmp .L269
1577 .L270:
1578 0eb4 8091 0000 lds r24,MKFlags
1579 0eb8 1150 subi r17,lo8(-(-1))
1580 0eba 80FD sbrc r24,0
1581 0ebc 00C0 rjmp .L271
1582 .L269:
1583 0ebe D093 0000 sts (BeepTime)+1,r29
1584 0ec2 C093 0000 sts BeepTime,r28
1585 0ec6 8AEF ldi r24,lo8(250)
1586 0ec8 90E0 ldi r25,hi8(250)
1587 0eca 0E94 0000 call Delay_ms
1588 0ece 1123 tst r17
1589 0ed0 01F4 brne .L270
1590 .L271:
1591 /* epilogue start */
1592 0ed2 DF91 pop r29
1593 0ed4 CF91 pop r28
1594 0ed6 1F91 pop r17
1595 0ed8 0895 ret
1597 .global SetCompassCalState
1599 SetCompassCalState:
1600 /* prologue: function */
1601 /* frame size = 0 */
1602 0eda 8091 0000 lds r24,ParamSet
1603 0ede A82F mov r26,r24
1604 0ee0 B0E0 ldi r27,lo8(0)
1605 0ee2 AA0F lsl r26
1606 0ee4 BB1F rol r27
1607 0ee6 FD01 movw r30,r26
1608 0ee8 E050 subi r30,lo8(-(PPM_in))
1609 0eea F040 sbci r31,hi8(-(PPM_in))
1610 0eec 8081 ld r24,Z
1611 0eee 9181 ldd r25,Z+1
1612 0ef0 8D5E subi r24,lo8(-19)
1613 0ef2 9F4F sbci r25,hi8(-19)
1614 0ef4 04F0 brlt .L273
1615 0ef6 1092 0000 sts stick.1951,__zero_reg__
1616 .L273:
1617 0efa A050 subi r26,lo8(-(PPM_in))
1618 0efc B040 sbci r27,hi8(-(PPM_in))
1619 0efe 8D91 ld r24,X+
1620 0f00 9C91 ld r25,X
1621 0f02 8A5B subi r24,lo8(-70)
1622 0f04 9F4F sbci r25,hi8(-70)
1623 0f06 04F4 brge .L276
1624 0f08 8091 0000 lds r24,stick.1951
1625 0f0c 8823 tst r24
1626 0f0e 01F4 brne .L276
1627 0f10 81E0 ldi r24,lo8(1)
1628 0f12 8093 0000 sts stick.1951,r24
1629 0f16 8091 0000 lds r24,CompassCalState
1630 0f1a 8F5F subi r24,lo8(-(1))
1631 0f1c 8093 0000 sts CompassCalState,r24
1632 0f20 8530 cpi r24,lo8(5)
1633 0f22 00F0 brlo .L277
1634 0f24 88EE ldi r24,lo8(1000)
1635 0f26 93E0 ldi r25,hi8(1000)
1636 0f28 9093 0000 sts (BeepTime)+1,r25
1637 0f2c 8093 0000 sts BeepTime,r24
1638 .L276:
1639 0f30 0895 ret
1640 .L277:
1641 0f32 0E94 0000 call Beep
1642 0f36 0895 ret
1644 .global MotorControl
1646 MotorControl:
1647 0f38 2F92 push r2
1648 0f3a 3F92 push r3
1649 0f3c 4F92 push r4
1650 0f3e 5F92 push r5
1651 0f40 6F92 push r6
1652 0f42 7F92 push r7
1653 0f44 8F92 push r8
1654 0f46 9F92 push r9
1655 0f48 AF92 push r10
1656 0f4a BF92 push r11
1657 0f4c CF92 push r12
1658 0f4e DF92 push r13
1659 0f50 EF92 push r14
1660 0f52 FF92 push r15
1661 0f54 0F93 push r16
1662 0f56 1F93 push r17
1663 0f58 DF93 push r29
1664 0f5a CF93 push r28
1665 0f5c CDB7 in r28,__SP_L__
1666 0f5e DEB7 in r29,__SP_H__
1667 0f60 6B97 sbiw r28,27
1668 0f62 0FB6 in __tmp_reg__,__SREG__
1669 0f64 F894 cli
1670 0f66 DEBF out __SP_H__,r29
1671 0f68 0FBE out __SREG__,__tmp_reg__
1672 0f6a CDBF out __SP_L__,r28
1673 /* prologue: function */
1674 /* frame size = 27 */
1675 0f6c 0E94 0000 call Mean
1676 0f70 D090 0000 lds r13,BoardRelease
1677 0f74 2BE0 ldi r18,lo8(11)
1678 0f76 2D15 cp r18,r13
1679 0f78 00F4 brsh .+4
1680 0f7a 0C94 0000 jmp .L279
1681 0f7e 299A sbi 37-0x20,1
1682 .L280:
1683 0f80 4091 0000 lds r20,StickGas
1684 0f84 5091 0000 lds r21,(StickGas)+1
1685 0f88 5887 std Y+8,r21
1686 0f8a 4F83 std Y+7,r20
1687 0f8c 8091 0000 lds r24,ParamSet+18
1688 0f90 282F mov r18,r24
1689 0f92 30E0 ldi r19,lo8(0)
1690 0f94 C901 movw r24,r18
1691 0f96 0996 adiw r24,9
1692 0f98 8417 cp r24,r20
1693 0f9a 9507 cpc r25,r21
1694 0f9c 04F0 brlt .L281
1695 0f9e B901 movw r22,r18
1696 0fa0 665F subi r22,lo8(-(10))
1697 0fa2 7F4F sbci r23,hi8(-(10))
1698 0fa4 7887 std Y+8,r23
1699 0fa6 6F83 std Y+7,r22
1700 .L281:
1701 0fa8 8091 0000 lds r24,RC_Quality
1702 0fac 9091 0000 lds r25,(RC_Quality)+1
1703 0fb0 8837 cpi r24,120
1704 0fb2 9105 cpc r25,__zero_reg__
1705 0fb4 04F0 brlt .+2
1706 0fb6 00C0 rjmp .L282
1707 0fb8 8091 0000 lds r24,PcAccess
1708 0fbc 8823 tst r24
1709 0fbe 01F4 brne .L283
1710 0fc0 8091 0000 lds r24,BeepModulation
1711 0fc4 9091 0000 lds r25,(BeepModulation)+1
1712 0fc8 8F5F subi r24,lo8(-1)
1713 0fca 9F4F sbci r25,hi8(-1)
1714 0fcc 01F4 brne .+4
1715 0fce 0C94 0000 jmp .L481
1716 .L283:
1717 0fd2 8091 0000 lds r24,RcLostTimer.1981
1718 0fd6 9091 0000 lds r25,(RcLostTimer.1981)+1
1719 0fda 0097 sbiw r24,0
1720 0fdc 01F0 breq .+4
1721 0fde 0C94 0000 jmp .L482
1722 0fe2 8091 0000 lds r24,MKFlags
1723 0fe6 8E7E andi r24,lo8(-18)
1724 0fe8 8093 0000 sts MKFlags,r24
1725 0fec 8AE0 ldi r24,lo8(10)
1726 0fee D816 cp r13,r24
1727 0ff0 01F4 brne .+4
1728 0ff2 0C94 0000 jmp .L286
1729 .L541:
1730 0ff6 94E1 ldi r25,lo8(20)
1731 0ff8 D916 cp r13,r25
1732 0ffa 01F4 brne .+4
1733 0ffc 0C94 0000 jmp .L286
1734 1000 2898 cbi 37-0x20,0
1735 .L288:
1736 1002 8091 0000 lds r24,ModelIsFlying
1737 1006 9091 0000 lds r25,(ModelIsFlying)+1
1738 100a 895E subi r24,lo8(1001)
1739 100c 9340 sbci r25,hi8(1001)
1740 100e 00F4 brsh .+4
1741 1010 0C94 0000 jmp .L478
1742 1014 8091 0000 lds r24,ParamSet+26
1743 1018 282F mov r18,r24
1744 101a 30E0 ldi r19,lo8(0)
1745 101c 3887 std Y+8,r19
1746 101e 2F83 std Y+7,r18
1747 1020 8091 0000 lds r24,MKFlags
1748 1024 8061 ori r24,lo8(16)
1749 1026 8093 0000 sts MKFlags,r24
1750 102a E091 0000 lds r30,ParamSet
1751 102e 2E2F mov r18,r30
1752 1030 30E0 ldi r19,lo8(0)
1753 1032 220F lsl r18
1754 1034 331F rol r19
1755 1036 F901 movw r30,r18
1756 1038 E050 subi r30,lo8(-(PPM_diff))
1757 103a F040 sbci r31,hi8(-(PPM_diff))
1758 103c 1182 std Z+1,__zero_reg__
1759 103e 1082 st Z,__zero_reg__
1760 1040 A091 0000 lds r26,ParamSet+1
1761 1044 B0E0 ldi r27,lo8(0)
1762 1046 AA0F lsl r26
1763 1048 BB1F rol r27
1764 104a FD01 movw r30,r26
1765 104c E050 subi r30,lo8(-(PPM_diff))
1766 104e F040 sbci r31,hi8(-(PPM_diff))
1767 1050 1182 std Z+1,__zero_reg__
1768 1052 1082 st Z,__zero_reg__
1769 1054 E091 0000 lds r30,ParamSet+3
1770 1058 4E2F mov r20,r30
1771 105a 50E0 ldi r21,lo8(0)
1772 105c 440F lsl r20
1773 105e 551F rol r21
1774 1060 CA01 movw r24,r20
1775 1062 8050 subi r24,lo8(-(PPM_diff))
1776 1064 9040 sbci r25,hi8(-(PPM_diff))
1777 1066 FC01 movw r30,r24
1778 1068 1182 std Z+1,__zero_reg__
1779 106a 1082 st Z,__zero_reg__
1780 106c 2050 subi r18,lo8(-(PPM_in))
1781 106e 3040 sbci r19,hi8(-(PPM_in))
1782 1070 F901 movw r30,r18
1783 1072 1182 std Z+1,__zero_reg__
1784 1074 1082 st Z,__zero_reg__
1785 1076 A050 subi r26,lo8(-(PPM_in))
1786 1078 B040 sbci r27,hi8(-(PPM_in))
1787 107a 1196 adiw r26,1
1788 107c 1C92 st X,__zero_reg__
1789 107e 1E92 st -X,__zero_reg__
1790 1080 4050 subi r20,lo8(-(PPM_in))
1791 1082 5040 sbci r21,hi8(-(PPM_in))
1792 1084 FA01 movw r30,r20
1793 1086 1182 std Z+1,__zero_reg__
1794 1088 1082 st Z,__zero_reg__
1795 .L290:
1796 108a 8091 0000 lds r24,NewPpmData
1797 108e 8150 subi r24,lo8(-(-1))
1798 1090 8093 0000 sts NewPpmData,r24
1799 1094 8F5F subi r24,lo8(-(1))
1800 1096 01F4 brne .+2
1801 1098 00C0 rjmp .L355
1802 109a 8091 0000 lds r24,MKFlags
1803 109e 84FD sbrc r24,4
1804 10a0 00C0 rjmp .L355
1805 .L356:
1806 10a2 8091 0000 lds r24,LoopingRoll
1807 10a6 8823 tst r24
1808 10a8 01F0 breq .+4
1809 10aa 0C94 0000 jmp .L386
1810 .L528:
1811 10ae 8091 0000 lds r24,LoopingNick
1812 10b2 8823 tst r24
1813 10b4 01F0 breq .+4
1814 10b6 0C94 0000 jmp .L386
1815 .L387:
1816 10ba 8091 0000 lds r24,MKFlags
1817 10be 84FF sbrs r24,4
1818 10c0 00C0 rjmp .L389
1819 10c2 1092 0000 sts (StickYaw)+1,__zero_reg__
1820 10c6 1092 0000 sts StickYaw,__zero_reg__
1821 10ca 1092 0000 sts (StickNick)+1,__zero_reg__
1822 10ce 1092 0000 sts StickNick,__zero_reg__
1823 10d2 1092 0000 sts (StickRoll)+1,__zero_reg__
1824 10d6 1092 0000 sts StickRoll,__zero_reg__
1825 10da 9AE5 ldi r25,lo8(90)
1826 10dc 9093 0000 sts GyroPFactor,r25
1827 10e0 88E7 ldi r24,lo8(120)
1828 10e2 8093 0000 sts GyroIFactor,r24
1829 10e6 9093 0000 sts GyroYawPFactor,r25
1830 10ea 8093 0000 sts GyroYawIFactor,r24
1831 10ee 1092 0000 sts LoopingRoll,__zero_reg__
1832 10f2 1092 0000 sts LoopingNick,__zero_reg__
1833 10f6 1092 0000 sts (MaxStickNick)+1,__zero_reg__
1834 10fa 1092 0000 sts MaxStickNick,__zero_reg__
1835 10fe 1092 0000 sts (MaxStickRoll)+1,__zero_reg__
1836 1102 1092 0000 sts MaxStickRoll,__zero_reg__
1837 .L389:
1838 1106 3090 0000 lds r3,LoopingNick
1839 110a 3320 tst r3
1840 110c 01F0 breq .+4
1841 110e 0C94 0000 jmp .L390
1842 1112 8091 0000 lds r24,LoopingRoll
1843 1116 8823 tst r24
1844 1118 01F0 breq .+4
1845 111a 0C94 0000 jmp .L390
1846 .L391:
1847 111e 2091 0000 lds r18,ADCycleCount
1848 1122 3091 0000 lds r19,(ADCycleCount)+1
1849 1126 8091 0000 lds r24,FCParam+11
1850 112a 4AE0 ldi r20,lo8(10)
1851 112c 849F mul r24,r20
1852 112e C001 movw r24,r0
1853 1130 1124 clr r1
1854 1132 2817 cp r18,r24
1855 1134 3907 cpc r19,r25
1856 1136 00F4 brsh .+2
1857 1138 00C0 rjmp .L392
1858 113a 8091 0000 lds r24,FCParam+12
1859 113e 849F mul r24,r20
1860 1140 4001 movw r8,r0
1861 1142 1124 clr r1
1862 1144 8091 0000 lds r24,FCParam+13
1863 1148 849F mul r24,r20
1864 114a 3001 movw r6,r0
1865 114c 1124 clr r1
1866 114e E090 0000 lds r14,vibrationOffsetNick
1867 1152 F090 0000 lds r15,(vibrationOffsetNick)+1
1868 1156 0091 0000 lds r16,(vibrationOffsetNick)+2
1869 115a 1091 0000 lds r17,(vibrationOffsetNick)+3
1870 115e 8091 0000 lds r24,FCParam+10
1871 1162 C82E mov r12,r24
1872 1164 DD24 clr r13
1873 1166 8091 0000 lds r24,savedVibrationOffsetNick
1874 116a 9091 0000 lds r25,(savedVibrationOffsetNick)+1
1875 116e A701 movw r20,r14
1876 1170 481B sub r20,r24
1877 1172 590B sbc r21,r25
1878 1174 CA01 movw r24,r20
1879 1176 57FF sbrs r21,7
1880 1178 00C0 rjmp .+4
1881 117a 0C94 0000 jmp .L483
1882 117e 8C15 cp r24,r12
1883 1180 9D05 cpc r25,r13
1884 1182 04F0 brlt .+2
1885 1184 00C0 rjmp .L393
1886 .L542:
1887 1186 8091 0000 lds r24,FCParam+15
1888 118a 8823 tst r24
1889 118c 01F0 breq .+2
1890 118e 00C0 rjmp .L393
1891 1190 8091 0000 lds r24,filteredHiResPitchGyro
1892 1194 9091 0000 lds r25,(filteredHiResPitchGyro)+1
1893 1198 AA27 clr r26
1894 119a 97FD sbrc r25,7
1895 119c A095 com r26
1896 119e BA2F mov r27,r26
1897 11a0 E816 cp r14,r24
1898 11a2 F906 cpc r15,r25
1899 11a4 0A07 cpc r16,r26
1900 11a6 1B07 cpc r17,r27
1901 11a8 04F0 brlt .+4
1902 11aa 0C94 0000 jmp .L395
1903 11ae 2091 0000 lds r18,IntegralGyroNick
1904 11b2 3091 0000 lds r19,(IntegralGyroNick)+1
1905 11b6 4091 0000 lds r20,(IntegralGyroNick)+2
1906 11ba 5091 0000 lds r21,(IntegralGyroNick)+3
1907 11be 1216 cp __zero_reg__,r18
1908 11c0 1306 cpc __zero_reg__,r19
1909 11c2 1406 cpc __zero_reg__,r20
1910 11c4 1506 cpc __zero_reg__,r21
1911 11c6 04F0 brlt .+4
1912 11c8 0C94 0000 jmp .L395
1913 11cc A090 0000 lds r10,StickNick
1914 11d0 B090 0000 lds r11,(StickNick)+1
1915 11d4 A814 cp r10,r8
1916 11d6 B904 cpc r11,r9
1917 11d8 04F4 brge .+4
1918 11da 0C94 0000 jmp .L395
1919 11de 6A14 cp r6,r10
1920 11e0 7B04 cpc r7,r11
1921 11e2 04F4 brge .+4
1922 11e4 0C94 0000 jmp .L395
1923 11e8 8091 0000 lds r24,FCParam+16
1924 11ec 8058 subi r24,lo8(-(-128))
1925 11ee 9927 clr r25
1926 11f0 87FD sbrc r24,7
1927 11f2 9095 com r25
1928 11f4 A92F mov r26,r25
1929 11f6 B92F mov r27,r25
1930 11f8 8E0D add r24,r14
1931 11fa 9F1D adc r25,r15
1932 11fc A01F adc r26,r16
1933 11fe B11F adc r27,r17
1934 1200 8093 0000 sts vibrationOffsetNick,r24
1935 1204 9093 0000 sts (vibrationOffsetNick)+1,r25
1936 1208 A093 0000 sts (vibrationOffsetNick)+2,r26
1937 120c B093 0000 sts (vibrationOffsetNick)+3,r27
1938 .L479:
1939 1210 6091 0000 lds r22,FCParam+14
1940 1214 70E0 ldi r23,lo8(0)
1941 1216 80E0 ldi r24,lo8(0)
1942 1218 90E0 ldi r25,hi8(0)
1943 121a 0E94 0000 call __mulsi3
1944 121e 24E6 ldi r18,lo8(100)
1945 1220 30E0 ldi r19,hi8(100)
1946 1222 40E0 ldi r20,hlo8(100)
1947 1224 50E0 ldi r21,hhi8(100)
1948 1226 0E94 0000 call __divmodsi4
1949 122a 2093 0000 sts IntegralGyroNick,r18
1950 122e 3093 0000 sts (IntegralGyroNick)+1,r19
1951 1232 4093 0000 sts (IntegralGyroNick)+2,r20
1952 1236 5093 0000 sts (IntegralGyroNick)+3,r21
1953 .L393:
1954 123a E090 0000 lds r14,vibrationOffsetRoll
1955 123e F090 0000 lds r15,(vibrationOffsetRoll)+1
1956 1242 0091 0000 lds r16,(vibrationOffsetRoll)+2
1957 1246 1091 0000 lds r17,(vibrationOffsetRoll)+3
1958 124a 8091 0000 lds r24,savedVibrationOffsetRoll
1959 124e 9091 0000 lds r25,(savedVibrationOffsetRoll)+1
1960 1252 B701 movw r22,r14
1961 1254 681B sub r22,r24
1962 1256 790B sbc r23,r25
1963 1258 CB01 movw r24,r22
1964 125a 77FF sbrs r23,7
1965 125c 00C0 rjmp .+4
1966 125e 0C94 0000 jmp .L484
1967 1262 C816 cp r12,r24
1968 1264 D906 cpc r13,r25
1969 1266 04F0 brlt .+4
1970 1268 0C94 0000 jmp .L485
1971 .L396:
1972 126c 8091 0000 lds r24,NickNoisePeak
1973 1270 9091 0000 lds r25,(NickNoisePeak)+1
1974 1274 9093 0000 sts (debugNickNoisePeak)+1,r25
1975 1278 8093 0000 sts debugNickNoisePeak,r24
1976 127c 8091 0000 lds r24,RollNoisePeak
1977 1280 9091 0000 lds r25,(RollNoisePeak)+1
1978 1284 9093 0000 sts (debugRollNoisePeak)+1,r25
1979 1288 8093 0000 sts debugRollNoisePeak,r24
1980 128c 1092 0000 sts (RollNoisePeak)+1,__zero_reg__
1981 1290 1092 0000 sts RollNoisePeak,__zero_reg__
1982 1294 1092 0000 sts (NickNoisePeak)+1,__zero_reg__
1983 1298 1092 0000 sts NickNoisePeak,__zero_reg__
1984 129c 1092 0000 sts (ADCycleCount)+1,__zero_reg__
1985 12a0 1092 0000 sts ADCycleCount,__zero_reg__
1986 .L392:
1987 12a4 4091 0000 lds r20,StickYaw
1988 12a8 5091 0000 lds r21,(StickYaw)+1
1989 12ac CA01 movw r24,r20
1990 12ae 57FF sbrs r21,7
1991 12b0 00C0 rjmp .+4
1992 12b2 0C94 0000 jmp .L486
1993 .L400:
1994 12b6 4097 sbiw r24,16
1995 12b8 04F0 brlt .L399
1996 12ba 88EE ldi r24,lo8(1000)
1997 12bc 93E0 ldi r25,hi8(1000)
1998 12be 9093 0000 sts (BadCompassHeading)+1,r25
1999 12c2 8093 0000 sts BadCompassHeading,r24
2000 12c6 8091 0000 lds r24,ParamSet+8
2001 12ca 84FD sbrc r24,4
2002 12cc 00C0 rjmp .+4
2003 12ce 0C94 0000 jmp .L487
2004 .L399:
2005 12d2 8091 0000 lds r24,ParamSet+17
2006 12d6 90E0 ldi r25,lo8(0)
2007 12d8 849F mul r24,r20
2008 12da 9001 movw r18,r0
2009 12dc 859F mul r24,r21
2010 12de 300D add r19,r0
2011 12e0 949F mul r25,r20
2012 12e2 300D add r19,r0
2013 12e4 1124 clr r1
2014 12e6 37FF sbrs r19,7
2015 12e8 00C0 rjmp .+4
2016 12ea 0C94 0000 jmp .L488
2017 .L401:
2018 12ee 3595 asr r19
2019 12f0 2795 ror r18
2020 12f2 3595 asr r19
2021 12f4 2795 ror r18
2022 12f6 C901 movw r24,r18
2023 12f8 AA27 clr r26
2024 12fa 97FD sbrc r25,7
2025 12fc A095 com r26
2026 12fe BA2F mov r27,r26
2027 1300 8093 0000 sts SetPointYaw.1980,r24
2028 1304 9093 0000 sts (SetPointYaw.1980)+1,r25
2029 1308 A093 0000 sts (SetPointYaw.1980)+2,r26
2030 130c B093 0000 sts (SetPointYaw.1980)+3,r27
2031 1310 2091 0000 lds r18,ReadingIntegralGyroYaw
2032 1314 3091 0000 lds r19,(ReadingIntegralGyroYaw)+1
2033 1318 4091 0000 lds r20,(ReadingIntegralGyroYaw)+2
2034 131c 5091 0000 lds r21,(ReadingIntegralGyroYaw)+3
2035 1320 281B sub r18,r24
2036 1322 390B sbc r19,r25
2037 1324 4A0B sbc r20,r26
2038 1326 5B0B sbc r21,r27
2039 1328 2093 0000 sts ReadingIntegralGyroYaw,r18
2040 132c 3093 0000 sts (ReadingIntegralGyroYaw)+1,r19
2041 1330 4093 0000 sts (ReadingIntegralGyroYaw)+2,r20
2042 1334 5093 0000 sts (ReadingIntegralGyroYaw)+3,r21
2043 1338 203B cpi r18,lo8(-50000)
2044 133a 7CE3 ldi r23,hi8(-50000)
2045 133c 3707 cpc r19,r23
2046 133e 7FEF ldi r23,hlo8(-50000)
2047 1340 4707 cpc r20,r23
2048 1342 7FEF ldi r23,hhi8(-50000)
2049 1344 5707 cpc r21,r23
2050 1346 04F0 brlt .+4
2051 1348 0C94 0000 jmp .L402
2052 134c 80EB ldi r24,lo8(-50000)
2053 134e 9CE3 ldi r25,hi8(-50000)
2054 1350 AFEF ldi r26,hlo8(-50000)
2055 1352 BFEF ldi r27,hhi8(-50000)
2056 1354 8093 0000 sts ReadingIntegralGyroYaw,r24
2057 1358 9093 0000 sts (ReadingIntegralGyroYaw)+1,r25
2058 135c A093 0000 sts (ReadingIntegralGyroYaw)+2,r26
2059 1360 B093 0000 sts (ReadingIntegralGyroYaw)+3,r27
2060 .L403:
2061 1364 8091 0000 lds r24,ParamSet+8
2062 1368 8872 andi r24,lo8(40)
2063 136a 01F0 breq .+4
2064 136c 0C94 0000 jmp .L474
2065 1370 E091 0000 lds r30,GyroYaw
2066 1374 F091 0000 lds r31,(GyroYaw)+1
2067 1378 FC87 std Y+12,r31
2068 137a EB87 std Y+11,r30
2069 .L404:
2070 137c 8091 0000 lds r24,TimerDebugOut.1985
2071 1380 8150 subi r24,lo8(-(-1))
2072 1382 8093 0000 sts TimerDebugOut.1985,r24
2073 1386 8F5F subi r24,lo8(-(1))
2074 1388 01F4 brne .+4
2075 138a 0C94 0000 jmp .L475
2076 138e A090 0000 lds r10,StickNick
2077 1392 B090 0000 lds r11,(StickNick)+1
2078 1396 C090 0000 lds r12,StickRoll
2079 139a D090 0000 lds r13,(StickRoll)+1
2080 139e 4090 0000 lds r4,GyroNick
2081 13a2 5090 0000 lds r5,(GyroNick)+1
2082 13a6 8090 0000 lds r8,GyroRoll
2083 13aa 9090 0000 lds r9,(GyroRoll)+1
2084 .L422:
2085 13ae 8091 0000 lds r24,FunnelCourse
2086 13b2 8823 tst r24
2087 13b4 01F0 breq .L423
2088 13b6 1092 0000 sts IPartNick.1972,__zero_reg__
2089 13ba 1092 0000 sts (IPartNick.1972)+1,__zero_reg__
2090 13be 1092 0000 sts (IPartNick.1972)+2,__zero_reg__
2091 13c2 1092 0000 sts (IPartNick.1972)+3,__zero_reg__
2092 13c6 1092 0000 sts IPartRoll.1973,__zero_reg__
2093 13ca 1092 0000 sts (IPartRoll.1973)+1,__zero_reg__
2094 13ce 1092 0000 sts (IPartRoll.1973)+2,__zero_reg__
2095 13d2 1092 0000 sts (IPartRoll.1973)+3,__zero_reg__
2096 .L423:
2097 13d6 3320 tst r3
2098 13d8 01F4 brne .+4
2099 13da 0C94 0000 jmp .L424
2100 13de 1E82 std Y+6,__zero_reg__
2101 13e0 1D82 std Y+5,__zero_reg__
2102 13e2 2224 clr r2
2103 13e4 3324 clr r3
2104 13e6 7091 0000 lds r23,GyroIFactor
2105 13ea 7D87 std Y+13,r23
2106 .L425:
2107 13ec 8091 0000 lds r24,GyroPFactor
2108 13f0 E82E mov r14,r24
2109 13f2 FF24 clr r15
2110 13f4 00E0 ldi r16,lo8(0)
2111 13f6 10E0 ldi r17,hi8(0)
2112 13f8 8091 0000 lds r24,LoopingRoll
2113 13fc 8823 tst r24
2114 13fe 01F4 brne .+4
2115 1400 0C94 0000 jmp .L426
2116 1404 1C82 std Y+4,__zero_reg__
2117 1406 1B82 std Y+3,__zero_reg__
2118 1408 6624 clr r6
2119 140a 7724 clr r7
2120 .L427:
2121 140c B401 movw r22,r8
2122 140e 8827 clr r24
2123 1410 77FD sbrc r23,7
2124 1412 8095 com r24
2125 1414 982F mov r25,r24
2126 1416 A801 movw r20,r16
2127 1418 9701 movw r18,r14
2128 141a 0E94 0000 call __mulsi3
2129 141e 20E4 ldi r18,lo8(64)
2130 1420 30E0 ldi r19,hi8(64)
2131 1422 40E0 ldi r20,hlo8(64)
2132 1424 50E0 ldi r21,hhi8(64)
2133 1426 0E94 0000 call __divmodsi4
2134 142a B301 movw r22,r6
2135 142c 620F add r22,r18
2136 142e 731F adc r23,r19
2137 1430 7B8F std Y+27,r23
2138 1432 6A8F std Y+26,r22
2139 1434 7091 0000 lds r23,GyroYawPFactor
2140 1438 7A87 std Y+10,r23
2141 143a 8091 0000 lds r24,GyroYawIFactor
2142 143e 8987 std Y+9,r24
2143 1440 6090 0000 lds r6,IntegralGyroYaw
2144 1444 7090 0000 lds r7,(IntegralGyroYaw)+1
2145 1448 8090 0000 lds r8,(IntegralGyroYaw)+2
2146 144c 9090 0000 lds r9,(IntegralGyroYaw)+3
2147 1450 9201 movw r18,r4
2148 1452 4427 clr r20
2149 1454 37FD sbrc r19,7
2150 1456 4095 com r20
2151 1458 542F mov r21,r20
2152 145a C801 movw r24,r16
2153 145c B701 movw r22,r14
2154 145e 0E94 0000 call __mulsi3
2155 1462 20E4 ldi r18,lo8(64)
2156 1464 30E0 ldi r19,hi8(64)
2157 1466 40E0 ldi r20,hlo8(64)
2158 1468 50E0 ldi r21,hhi8(64)
2159 146a 0E94 0000 call __divmodsi4
2160 146e 220D add r18,r2
2161 1470 331D adc r19,r3
2162 1472 90E4 ldi r25,hi8(16385)
2163 1474 2130 cpi r18,lo8(16385)
2164 1476 3907 cpc r19,r25
2165 1478 04F0 brlt .L428
2166 147a 20E0 ldi r18,lo8(16384)
2167 147c 30E4 ldi r19,hi8(16384)
2168 .L428:
2169 147e 1901 movw r2,r18
2170 1480 E0EC ldi r30,hi8(-16384)
2171 1482 2030 cpi r18,lo8(-16384)
2172 1484 3E07 cpc r19,r30
2173 1486 04F4 brge .L429
2174 1488 212C mov r2,__zero_reg__
2175 148a A0EC ldi r26,hi8(-16384)
2176 148c 3A2E mov r3,r26
2177 .L429:
2178 148e 2A8D ldd r18,Y+26
2179 1490 3B8D ldd r19,Y+27
2180 1492 F0E4 ldi r31,hi8(16385)
2181 1494 2130 cpi r18,lo8(16385)
2182 1496 3F07 cpc r19,r31
2183 1498 04F0 brlt .L430
2184 149a 20E0 ldi r18,lo8(16384)
2185 149c 30E4 ldi r19,hi8(16384)
2186 .L430:
2187 149e 3A83 std Y+2,r19
2188 14a0 2983 std Y+1,r18
2189 14a2 2050 subi r18,lo8(-16384)
2190 14a4 304C sbci r19,hi8(-16384)
2191 14a6 04F4 brge .L431
2192 14a8 60E0 ldi r22,lo8(-16384)
2193 14aa 70EC ldi r23,hi8(-16384)
2194 14ac 7A83 std Y+2,r23
2195 14ae 6983 std Y+1,r22
2196 .L431:
2197 14b0 8091 0000 lds r24,MissingMotor
2198 14b4 8823 tst r24
2199 14b6 01F0 breq .L432
2200 14b8 8091 0000 lds r24,ModelIsFlying
2201 14bc 9091 0000 lds r25,(ModelIsFlying)+1
2202 14c0 0297 sbiw r24,2
2203 14c2 C097 sbiw r24,48
2204 14c4 00F4 brsh .+4
2205 14c6 0C94 0000 jmp .L489
2206 .L433:
2207 14ca 8091 0000 lds r24,ParamSet+18
2208 14ce E82F mov r30,r24
2209 14d0 F0E0 ldi r31,lo8(0)
2210 14d2 F887 std Y+8,r31
2211 14d4 EF83 std Y+7,r30
2212 .L432:
2213 14d6 8091 0000 lds r24,ParamSet+19
2214 14da 90E0 ldi r25,lo8(0)
2215 14dc 4497 sbiw r24,20
2216 14de 880F lsl r24
2217 14e0 991F rol r25
2218 14e2 880F lsl r24
2219 14e4 991F rol r25
2220 14e6 4F80 ldd r4,Y+7
2221 14e8 5884 ldd r5,Y+8
2222 14ea 440C lsl r4
2223 14ec 551C rol r5
2224 14ee 440C lsl r4
2225 14f0 551C rol r5
2226 14f2 8415 cp r24,r4
2227 14f4 9505 cpc r25,r5
2228 14f6 04F4 brge .L434
2229 14f8 2C01 movw r4,r24
2230 .L434:
2231 14fa 5092 0000 sts (DebugOut+16)+1,r5
2232 14fe 4092 0000 sts DebugOut+16,r4
2233 1502 2B85 ldd r18,Y+11
2234 1504 3C85 ldd r19,Y+12
2235 1506 220F lsl r18
2236 1508 331F rol r19
2237 150a B901 movw r22,r18
2238 150c 8827 clr r24
2239 150e 77FD sbrc r23,7
2240 1510 8095 com r24
2241 1512 982F mov r25,r24
2242 1514 EA85 ldd r30,Y+10
2243 1516 2E2F mov r18,r30
2244 1518 30E0 ldi r19,lo8(0)
2245 151a 40E0 ldi r20,lo8(0)
2246 151c 50E0 ldi r21,hi8(0)
2247 151e 0E94 0000 call __mulsi3
2248 1522 20E4 ldi r18,lo8(64)
2249 1524 30E0 ldi r19,hi8(64)
2250 1526 40E0 ldi r20,hlo8(64)
2251 1528 50E0 ldi r21,hhi8(64)
2252 152a 0E94 0000 call __divmodsi4
2253 152e 7901 movw r14,r18
2254 1530 8A01 movw r16,r20
2255 1532 F985 ldd r31,Y+9
2256 1534 6F2F mov r22,r31
2257 1536 70E0 ldi r23,lo8(0)
2258 1538 80E0 ldi r24,lo8(0)
2259 153a 90E0 ldi r25,hi8(0)
2260 153c A401 movw r20,r8
2261 153e 9301 movw r18,r6
2262 1540 0E94 0000 call __mulsi3
2263 1544 20EF ldi r18,lo8(22000)
2264 1546 35E5 ldi r19,hi8(22000)
2265 1548 40E0 ldi r20,hlo8(22000)
2266 154a 50E0 ldi r21,hhi8(22000)
2267 154c 0E94 0000 call __divmodsi4
2268 1550 C701 movw r24,r14
2269 1552 820F add r24,r18
2270 1554 931F adc r25,r19
2271 1556 20E4 ldi r18,hi8(16385)
2272 1558 8130 cpi r24,lo8(16385)
2273 155a 9207 cpc r25,r18
2274 155c 04F0 brlt .L435
2275 155e 80E0 ldi r24,lo8(16384)
2276 1560 90E4 ldi r25,hi8(16384)
2277 .L435:
2278 1562 8C01 movw r16,r24
2279 1564 30EC ldi r19,hi8(-16384)
2280 1566 8030 cpi r24,lo8(-16384)
2281 1568 9307 cpc r25,r19
2282 156a 04F4 brge .L436
2283 156c 00E0 ldi r16,lo8(-16384)
2284 156e 10EC ldi r17,hi8(-16384)
2285 .L436:
2286 1570 6091 0000 lds r22,SetPointYaw.1980
2287 1574 7091 0000 lds r23,(SetPointYaw.1980)+1
2288 1578 8091 0000 lds r24,(SetPointYaw.1980)+2
2289 157c 9091 0000 lds r25,(SetPointYaw.1980)+3
2290 1580 2CEF ldi r18,lo8(-4)
2291 1582 3FEF ldi r19,hi8(-4)
2292 1584 4FEF ldi r20,hlo8(-4)
2293 1586 5FEF ldi r21,hhi8(-4)
2294 1588 0E94 0000 call __mulsi3
2295 158c 7801 movw r14,r16
2296 158e E60E add r14,r22
2297 1590 F71E adc r15,r23
2298 1592 41EA ldi r20,lo8(161)
2299 1594 4416 cp r4,r20
2300 1596 5104 cpc r5,__zero_reg__
2301 1598 04F4 brge .+2
2302 159a 00C0 rjmp .L437
2303 159c C201 movw r24,r4
2304 159e 57FE sbrs r5,7
2305 15a0 00C0 rjmp .+4
2306 15a2 0C94 0000 jmp .L490
2307 .L438:
2308 15a6 9595 asr r25
2309 15a8 8795 ror r24
2310 15aa 2227 clr r18
2311 15ac 3327 clr r19
2312 15ae 281B sub r18,r24
2313 15b0 390B sbc r19,r25
2314 15b2 E216 cp r14,r18
2315 15b4 F306 cpc r15,r19
2316 15b6 04F0 brlt .+4
2317 15b8 0C94 0000 jmp .L491
2318 .L439:
2319 15bc 8091 0000 lds r24,ParamSet+19
2320 15c0 90E0 ldi r25,lo8(0)
2321 15c2 880F lsl r24
2322 15c4 991F rol r25
2323 15c6 880F lsl r24
2324 15c8 991F rol r25
2325 15ca 3201 movw r6,r4
2326 15cc 681A sub r6,r24
2327 15ce 790A sbc r7,r25
2328 15d0 2615 cp r18,r6
2329 15d2 3705 cpc r19,r7
2330 15d4 04F0 brlt .L443
2331 15d6 8419 sub r24,r4
2332 15d8 9509 sbc r25,r5
2333 15da 3901 movw r6,r18
2334 15dc 8217 cp r24,r18
2335 15de 9307 cpc r25,r19
2336 15e0 04F4 brge .+4
2337 15e2 0C94 0000 jmp .L492
2338 .L443:
2339 15e6 6D85 ldd r22,Y+13
2340 15e8 6623 tst r22
2341 15ea 01F4 brne .+2
2342 15ec 00C0 rjmp .L445
2343 15ee 8D81 ldd r24,Y+5
2344 15f0 9E81 ldd r25,Y+6
2345 15f2 8A19 sub r24,r10
2346 15f4 9B09 sbc r25,r11
2347 15f6 FC01 movw r30,r24
2348 15f8 CF01 movw r24,r30
2349 15fa AA27 clr r26
2350 15fc 97FD sbrc r25,7
2351 15fe A095 com r26
2352 1600 BA2F mov r27,r26
2353 1602 E090 0000 lds r14,IPartNick.1972
2354 1606 F090 0000 lds r15,(IPartNick.1972)+1
2355 160a 0091 0000 lds r16,(IPartNick.1972)+2
2356 160e 1091 0000 lds r17,(IPartNick.1972)+3
2357 1612 E80E add r14,r24
2358 1614 F91E adc r15,r25
2359 1616 0A1F adc r16,r26
2360 1618 1B1F adc r17,r27
2361 161a E092 0000 sts IPartNick.1972,r14
2362 161e F092 0000 sts (IPartNick.1972)+1,r15
2363 1622 0093 0000 sts (IPartNick.1972)+2,r16
2364 1626 1093 0000 sts (IPartNick.1972)+3,r17
2365 .L446:
2366 162a F0E0 ldi r31,lo8(-64000)
2367 162c EF16 cp r14,r31
2368 162e F6E0 ldi r31,hi8(-64000)
2369 1630 FF06 cpc r15,r31
2370 1632 FFEF ldi r31,hlo8(-64000)
2371 1634 0F07 cpc r16,r31
2372 1636 FFEF ldi r31,hhi8(-64000)
2373 1638 1F07 cpc r17,r31
2374 163a 04F0 brlt .+2
2375 163c 00C0 rjmp .L447
2376 163e 80E0 ldi r24,lo8(-64000)
2377 1640 96E0 ldi r25,hi8(-64000)
2378 1642 AFEF ldi r26,hlo8(-64000)
2379 1644 BFEF ldi r27,hhi8(-64000)
2380 1646 8093 0000 sts IPartNick.1972,r24
2381 164a 9093 0000 sts (IPartNick.1972)+1,r25
2382 164e A093 0000 sts (IPartNick.1972)+2,r26
2383 1652 B093 0000 sts (IPartNick.1972)+3,r27
2384 1656 E12C mov r14,__zero_reg__
2385 1658 26E0 ldi r18,hi8(-64000)
2386 165a F22E mov r15,r18
2387 165c 2FEF ldi r18,hlo8(-64000)
2388 165e 022F mov r16,r18
2389 1660 2FEF ldi r18,hhi8(-64000)
2390 1662 122F mov r17,r18
2391 .L448:
2392 1664 A101 movw r20,r2
2393 1666 4A19 sub r20,r10
2394 1668 5B09 sbc r21,r11
2395 166a 5A01 movw r10,r20
2396 166c 8090 0000 lds r8,Ki
2397 1670 9090 0000 lds r9,(Ki)+1
2398 1674 9401 movw r18,r8
2399 1676 4427 clr r20
2400 1678 37FD sbrc r19,7
2401 167a 4095 com r20
2402 167c 542F mov r21,r20
2403 167e C801 movw r24,r16
2404 1680 B701 movw r22,r14
2405 1682 0E94 0000 call __divmodsi4
2406 1686 A20E add r10,r18
2407 1688 B31E adc r11,r19
2408 168a 5D85 ldd r21,Y+13
2409 168c 5523 tst r21
2410 168e 01F4 brne .+2
2411 1690 00C0 rjmp .L449
2412 1692 6B81 ldd r22,Y+3
2413 1694 7C81 ldd r23,Y+4
2414 1696 6C19 sub r22,r12
2415 1698 7D09 sbc r23,r13
2416 169a CB01 movw r24,r22
2417 169c AA27 clr r26
2418 169e 97FD sbrc r25,7
2419 16a0 A095 com r26
2420 16a2 BA2F mov r27,r26
2421 16a4 E090 0000 lds r14,IPartRoll.1973
2422 16a8 F090 0000 lds r15,(IPartRoll.1973)+1
2423 16ac 0091 0000 lds r16,(IPartRoll.1973)+2
2424 16b0 1091 0000 lds r17,(IPartRoll.1973)+3
2425 16b4 E80E add r14,r24
2426 16b6 F91E adc r15,r25
2427 16b8 0A1F adc r16,r26
2428 16ba 1B1F adc r17,r27
2429 16bc E092 0000 sts IPartRoll.1973,r14
2430 16c0 F092 0000 sts (IPartRoll.1973)+1,r15
2431 16c4 0093 0000 sts (IPartRoll.1973)+2,r16
2432 16c8 1093 0000 sts (IPartRoll.1973)+3,r17
2433 .L450:
2434 16cc 70E0 ldi r23,lo8(-64000)
2435 16ce E716 cp r14,r23
2436 16d0 76E0 ldi r23,hi8(-64000)
2437 16d2 F706 cpc r15,r23
2438 16d4 7FEF ldi r23,hlo8(-64000)
2439 16d6 0707 cpc r16,r23
2440 16d8 7FEF ldi r23,hhi8(-64000)
2441 16da 1707 cpc r17,r23
2442 16dc 04F0 brlt .+2
2443 16de 00C0 rjmp .L451
2444 16e0 80E0 ldi r24,lo8(-64000)
2445 16e2 96E0 ldi r25,hi8(-64000)
2446 16e4 AFEF ldi r26,hlo8(-64000)
2447 16e6 BFEF ldi r27,hhi8(-64000)
2448 16e8 8093 0000 sts IPartRoll.1973,r24
2449 16ec 9093 0000 sts (IPartRoll.1973)+1,r25
2450 16f0 A093 0000 sts (IPartRoll.1973)+2,r26
2451 16f4 B093 0000 sts (IPartRoll.1973)+3,r27
2452 16f8 E12C mov r14,__zero_reg__
2453 16fa 86E0 ldi r24,hi8(-64000)
2454 16fc F82E mov r15,r24
2455 16fe 8FEF ldi r24,hlo8(-64000)
2456 1700 082F mov r16,r24
2457 1702 8FEF ldi r24,hhi8(-64000)
2458 1704 182F mov r17,r24
2459 .L452:
2460 1706 E981 ldd r30,Y+1
2461 1708 FA81 ldd r31,Y+2
2462 170a EC19 sub r30,r12
2463 170c FD09 sbc r31,r13
2464 170e 6F01 movw r12,r30
2465 1710 9401 movw r18,r8
2466 1712 4427 clr r20
2467 1714 37FD sbrc r19,7
2468 1716 4095 com r20
2469 1718 542F mov r21,r20
2470 171a C801 movw r24,r16
2471 171c B701 movw r22,r14
2472 171e 0E94 0000 call __divmodsi4
2473 1722 C20E add r12,r18
2474 1724 D31E adc r13,r19
2475 1726 9301 movw r18,r6
2476 1728 77FE sbrs r7,7
2477 172a 00C0 rjmp .+4
2478 172c 0C94 0000 jmp .L493
2479 .L453:
2480 1730 3595 asr r19
2481 1732 2795 ror r18
2482 1734 240D add r18,r4
2483 1736 351D adc r19,r5
2484 1738 B901 movw r22,r18
2485 173a 8827 clr r24
2486 173c 77FD sbrc r23,7
2487 173e 8095 com r24
2488 1740 982F mov r25,r24
2489 1742 2091 0000 lds r18,FCParam+23
2490 1746 30E0 ldi r19,lo8(0)
2491 1748 40E0 ldi r20,lo8(0)
2492 174a 50E0 ldi r21,hi8(0)
2493 174c 0E94 0000 call __mulsi3
2494 1750 20E4 ldi r18,lo8(64)
2495 1752 30E0 ldi r19,hi8(64)
2496 1754 40E0 ldi r20,hlo8(64)
2497 1756 50E0 ldi r21,hhi8(64)
2498 1758 0E94 0000 call __divmodsi4
2499 175c B901 movw r22,r18
2500 175e 8827 clr r24
2501 1760 9927 clr r25
2502 1762 821B sub r24,r18
2503 1764 930B sbc r25,r19
2504 1766 A816 cp r10,r24
2505 1768 B906 cpc r11,r25
2506 176a 04F0 brlt .+2
2507 176c 00C0 rjmp .L454
2508 176e 9C01 movw r18,r24
2509 .L455:
2510 1770 C816 cp r12,r24
2511 1772 D906 cpc r13,r25
2512 1774 04F0 brlt .L458
2513 1776 CB01 movw r24,r22
2514 1778 C616 cp r12,r22
2515 177a D706 cpc r13,r23
2516 177c 04F4 brge .+4
2517 177e 0C94 0000 jmp .L494
2518 .L458:
2519 1782 B0E0 ldi r27,lo8(Mixer+13)
2520 1784 2B2E mov r2,r27
2521 1786 B0E0 ldi r27,hi8(Mixer+13)
2522 1788 3B2E mov r3,r27
2523 178a A0E0 ldi r26,lo8(Motor)
2524 178c 8A2E mov r8,r26
2525 178e A0E0 ldi r26,hi8(Motor)
2526 1790 9A2E mov r9,r26
2527 1792 F0E0 ldi r31,lo8(MotorValue.1987)
2528 1794 AF2E mov r10,r31
2529 1796 F0E0 ldi r31,hi8(MotorValue.1987)
2530 1798 BF2E mov r11,r31
2531 179a A301 movw r20,r6
2532 179c 6627 clr r22
2533 179e 57FD sbrc r21,7
2534 17a0 6095 com r22
2535 17a2 762F mov r23,r22
2536 17a4 4E87 std Y+14,r20
2537 17a6 5F87 std Y+15,r21
2538 17a8 688B std Y+16,r22
2539 17aa 798B std Y+17,r23
2540 17ac A201 movw r20,r4
2541 17ae 6627 clr r22
2542 17b0 57FD sbrc r21,7
2543 17b2 6095 com r22
2544 17b4 762F mov r23,r22
2545 17b6 4A8B std Y+18,r20
2546 17b8 5B8B std Y+19,r21
2547 17ba 6C8B std Y+20,r22
2548 17bc 7D8B std Y+21,r23
2549 17be A901 movw r20,r18
2550 17c0 6627 clr r22
2551 17c2 57FD sbrc r21,7
2552 17c4 6095 com r22
2553 17c6 762F mov r23,r22
2554 17c8 4E8B std Y+22,r20
2555 17ca 5F8B std Y+23,r21
2556 17cc 688F std Y+24,r22
2557 17ce 798F std Y+25,r23
2558 17d0 2C01 movw r4,r24
2559 17d2 6624 clr r6
2560 17d4 57FC sbrc r5,7
2561 17d6 6094 com r6
2562 17d8 762C mov r7,r6
2563 17da 00C0 rjmp .L466
2564 .L498:
2565 17dc 2381 ldd r18,Z+3
2566 17de 3327 clr r19
2567 17e0 27FD sbrc r18,7
2568 17e2 3095 com r19
2569 17e4 432F mov r20,r19
2570 17e6 532F mov r21,r19
2571 17e8 6E85 ldd r22,Y+14
2572 17ea 7F85 ldd r23,Y+15
2573 17ec 8889 ldd r24,Y+16
2574 17ee 9989 ldd r25,Y+17
2575 17f0 0E94 0000 call __mulsi3
2576 17f4 20E4 ldi r18,lo8(64)
2577 17f6 30E0 ldi r19,hi8(64)
2578 17f8 40E0 ldi r20,hlo8(64)
2579 17fa 50E0 ldi r21,hhi8(64)
2580 17fc 0E94 0000 call __divmodsi4
2581 1800 7901 movw r14,r18
2582 1802 8A01 movw r16,r20
2583 1804 2D2D mov r18,r13
2584 1806 3327 clr r19
2585 1808 27FD sbrc r18,7
2586 180a 3095 com r19
2587 180c 432F mov r20,r19
2588 180e 532F mov r21,r19
2589 1810 6A89 ldd r22,Y+18
2590 1812 7B89 ldd r23,Y+19
2591 1814 8C89 ldd r24,Y+20
2592 1816 9D89 ldd r25,Y+21
2593 1818 0E94 0000 call __mulsi3
2594 181c 20E4 ldi r18,lo8(64)
2595 181e 30E0 ldi r19,hi8(64)
2596 1820 40E0 ldi r20,hlo8(64)
2597 1822 50E0 ldi r21,hhi8(64)
2598 1824 0E94 0000 call __divmodsi4
2599 1828 E20E add r14,r18
2600 182a F31E adc r15,r19
2601 182c F101 movw r30,r2
2602 182e 2181 ldd r18,Z+1
2603 1830 3327 clr r19
2604 1832 27FD sbrc r18,7
2605 1834 3095 com r19
2606 1836 432F mov r20,r19
2607 1838 532F mov r21,r19
2608 183a 6E89 ldd r22,Y+22
2609 183c 7F89 ldd r23,Y+23
2610 183e 888D ldd r24,Y+24
2611 1840 998D ldd r25,Y+25
2612 1842 0E94 0000 call __mulsi3
2613 1846 20E4 ldi r18,lo8(64)
2614 1848 30E0 ldi r19,hi8(64)
2615 184a 40E0 ldi r20,hlo8(64)
2616 184c 50E0 ldi r21,hhi8(64)
2617 184e 0E94 0000 call __divmodsi4
2618 1852 E20E add r14,r18
2619 1854 F31E adc r15,r19
2620 1856 F101 movw r30,r2
2621 1858 2281 ldd r18,Z+2
2622 185a 3327 clr r19
2623 185c 27FD sbrc r18,7
2624 185e 3095 com r19
2625 1860 432F mov r20,r19
2626 1862 532F mov r21,r19
2627 1864 C301 movw r24,r6
2628 1866 B201 movw r22,r4
2629 1868 0E94 0000 call __mulsi3
2630 186c 20E4 ldi r18,lo8(64)
2631 186e 30E0 ldi r19,hi8(64)
2632 1870 40E0 ldi r20,hlo8(64)
2633 1872 50E0 ldi r21,hhi8(64)
2634 1874 0E94 0000 call __divmodsi4
2635 1878 E20E add r14,r18
2636 187a F31E adc r15,r19
2637 187c F501 movw r30,r10
2638 187e 8081 ld r24,Z
2639 1880 9181 ldd r25,Z+1
2640 1882 E80E add r14,r24
2641 1884 F91E adc r15,r25
2642 1886 F7FC sbrc r15,7
2643 1888 00C0 rjmp .L495
2644 .L461:
2645 188a C701 movw r24,r14
2646 188c 9595 asr r25
2647 188e 8795 ror r24
2648 1890 F501 movw r30,r10
2649 1892 8083 st Z,r24
2650 1894 9183 std Z+1,r25
2651 1896 97FD sbrc r25,7
2652 1898 00C0 rjmp .L496
2653 .L462:
2654 189a AC01 movw r20,r24
2655 189c 5595 asr r21
2656 189e 4795 ror r20
2657 18a0 5595 asr r21
2658 18a2 4795 ror r20
2659 18a4 8091 0000 lds r24,ParamSet+18
2660 18a8 282F mov r18,r24
2661 18aa 30E0 ldi r19,lo8(0)
2662 18ac 4217 cp r20,r18
2663 18ae 5307 cpc r21,r19
2664 18b0 04F0 brlt .L463
2665 18b2 8091 0000 lds r24,ParamSet+19
2666 18b6 90E0 ldi r25,lo8(0)
2667 18b8 9A01 movw r18,r20
2668 18ba 8417 cp r24,r20
2669 18bc 9507 cpc r25,r21
2670 18be 04F4 brge .L463
2671 18c0 9C01 movw r18,r24
2672 .L463:
2673 18c2 F401 movw r30,r8
2674 18c4 2083 st Z,r18
2675 .L465:
2676 18c6 24E0 ldi r18,lo8(4)
2677 18c8 30E0 ldi r19,hi8(4)
2678 18ca 220E add r2,r18
2679 18cc 331E adc r3,r19
2680 18ce 45E0 ldi r20,lo8(5)
2681 18d0 50E0 ldi r21,hi8(5)
2682 18d2 840E add r8,r20
2683 18d4 951E adc r9,r21
2684 18d6 62E0 ldi r22,lo8(2)
2685 18d8 70E0 ldi r23,hi8(2)
2686 18da A60E add r10,r22
2687 18dc B71E adc r11,r23
2688 18de 70E0 ldi r23,lo8(Mixer+61)
2689 18e0 2716 cp r2,r23
2690 18e2 70E0 ldi r23,hi8(Mixer+61)
2691 18e4 3706 cpc r3,r23
2692 18e6 01F4 brne .+2
2693 18e8 00C0 rjmp .L497
2694 .L466:
2695 18ea F101 movw r30,r2
2696 18ec D080 ld r13,Z
2697 18ee 1D14 cp __zero_reg__,r13
2698 18f0 04F4 brge .+2
2699 18f2 00C0 rjmp .L498
2700 18f4 F401 movw r30,r8
2701 18f6 1082 st Z,__zero_reg__
2702 18f8 00C0 rjmp .L465
2703 .L282:
2704 18fa 8091 0000 lds r24,RC_Quality
2705 18fe 9091 0000 lds r25,(RC_Quality)+1
2706 1902 8D38 cpi r24,141
2707 1904 9105 cpc r25,__zero_reg__
2708 1906 04F4 brge .+2
2709 1908 00C0 rjmp .L290
2710 190a 8091 0000 lds r24,MKFlags
2711 190e 8F7E andi r24,lo8(-17)
2712 1910 8093 0000 sts MKFlags,r24
2713 1914 8091 0000 lds r24,ParamSet+27
2714 1918 22E3 ldi r18,lo8(50)
2715 191a 829F mul r24,r18
2716 191c C001 movw r24,r0
2717 191e 1124 clr r1
2718 1920 9093 0000 sts (RcLostTimer.1981)+1,r25
2719 1924 8093 0000 sts RcLostTimer.1981,r24
2720 1928 2F81 ldd r18,Y+7
2721 192a 3885 ldd r19,Y+8
2722 192c 2932 cpi r18,41
2723 192e 3105 cpc r19,__zero_reg__
2724 1930 04F0 brlt .L291
2725 1932 8091 0000 lds r24,MKFlags
2726 1936 80FF sbrs r24,0
2727 1938 00C0 rjmp .L291
2728 193a 8091 0000 lds r24,ModelIsFlying
2729 193e 9091 0000 lds r25,(ModelIsFlying)+1
2730 1942 3FEF ldi r19,hi8(-1)
2731 1944 8F3F cpi r24,lo8(-1)
2732 1946 9307 cpc r25,r19
2733 1948 01F4 brne .+4
2734 194a 0C94 0000 jmp .L292
2735 194e 0196 adiw r24,1
2736 1950 9093 0000 sts (ModelIsFlying)+1,r25
2737 1954 8093 0000 sts ModelIsFlying,r24
2738 .L291:
2739 1958 8091 0000 lds r24,ModelIsFlying
2740 195c 9091 0000 lds r25,(ModelIsFlying)+1
2741 1960 8F3F cpi r24,255
2742 1962 9105 cpc r25,__zero_reg__
2743 1964 01F0 breq .+6
2744 1966 00F0 brlo .+4
2745 1968 0C94 0000 jmp .L292
2746 196c 1092 0000 sts IPartNick.1972,__zero_reg__
2747 1970 1092 0000 sts (IPartNick.1972)+1,__zero_reg__
2748 1974 1092 0000 sts (IPartNick.1972)+2,__zero_reg__
2749 1978 1092 0000 sts (IPartNick.1972)+3,__zero_reg__
2750 197c 1092 0000 sts IPartRoll.1973,__zero_reg__
2751 1980 1092 0000 sts (IPartRoll.1973)+1,__zero_reg__
2752 1984 1092 0000 sts (IPartRoll.1973)+2,__zero_reg__
2753 1988 1092 0000 sts (IPartRoll.1973)+3,__zero_reg__
2754 198c 1092 0000 sts (StickYaw)+1,__zero_reg__
2755 1990 1092 0000 sts StickYaw,__zero_reg__
2756 1994 8A3F cpi r24,250
2757 1996 9105 cpc r25,__zero_reg__
2758 1998 01F4 brne .+4
2759 199a 0C94 0000 jmp .L499
2760 .L293:
2761 199e 8091 0000 lds r24,ParamSet+4
2762 19a2 E82F mov r30,r24
2763 19a4 F0E0 ldi r31,lo8(0)
2764 19a6 EE0F lsl r30
2765 19a8 FF1F rol r31
2766 19aa E050 subi r30,lo8(-(PPM_in))
2767 19ac F040 sbci r31,hi8(-(PPM_in))
2768 19ae 8081 ld r24,Z
2769 19b0 9181 ldd r25,Z+1
2770 19b2 E090 0000 lds r14,Poti1
2771 19b6 F090 0000 lds r15,(Poti1)+1
2772 19ba 8359 subi r24,lo8(-(109))
2773 19bc 9F4F sbci r25,hi8(-(109))
2774 19be 8E15 cp r24,r14
2775 19c0 9F05 cpc r25,r15
2776 19c2 04F4 brge .+4
2777 19c4 0C94 0000 jmp .L294
2778 19c8 C701 movw r24,r14
2779 19ca 0196 adiw r24,1
2780 19cc 9093 0000 sts (Poti1)+1,r25
2781 19d0 8093 0000 sts Poti1,r24
2782 19d4 7C01 movw r14,r24
2783 .L295:
2784 19d6 8091 0000 lds r24,ParamSet+5
2785 19da E82F mov r30,r24
2786 19dc F0E0 ldi r31,lo8(0)
2787 19de EE0F lsl r30
2788 19e0 FF1F rol r31
2789 19e2 E050 subi r30,lo8(-(PPM_in))
2790 19e4 F040 sbci r31,hi8(-(PPM_in))
2791 19e6 8081 ld r24,Z
2792 19e8 9181 ldd r25,Z+1
2793 19ea 0091 0000 lds r16,Poti2
2794 19ee 1091 0000 lds r17,(Poti2)+1
2795 19f2 8359 subi r24,lo8(-(109))
2796 19f4 9F4F sbci r25,hi8(-(109))
2797 19f6 8017 cp r24,r16
2798 19f8 9107 cpc r25,r17
2799 19fa 04F4 brge .+4
2800 19fc 0C94 0000 jmp .L296
2801 1a00 C801 movw r24,r16
2802 1a02 0196 adiw r24,1
2803 1a04 9093 0000 sts (Poti2)+1,r25
2804 1a08 8093 0000 sts Poti2,r24
2805 1a0c 8C01 movw r16,r24
2806 .L297:
2807 1a0e 8091 0000 lds r24,ParamSet+6
2808 1a12 E82F mov r30,r24
2809 1a14 F0E0 ldi r31,lo8(0)
2810 1a16 EE0F lsl r30
2811 1a18 FF1F rol r31
2812 1a1a E050 subi r30,lo8(-(PPM_in))
2813 1a1c F040 sbci r31,hi8(-(PPM_in))
2814 1a1e 8081 ld r24,Z
2815 1a20 9181 ldd r25,Z+1
2816 1a22 A090 0000 lds r10,Poti3
2817 1a26 B090 0000 lds r11,(Poti3)+1
2818 1a2a 8359 subi r24,lo8(-(109))
2819 1a2c 9F4F sbci r25,hi8(-(109))
2820 1a2e 8A15 cp r24,r10
2821 1a30 9B05 cpc r25,r11
2822 1a32 04F4 brge .+4
2823 1a34 0C94 0000 jmp .L298
2824 1a38 C501 movw r24,r10
2825 1a3a 0196 adiw r24,1
2826 1a3c 9093 0000 sts (Poti3)+1,r25
2827 1a40 8093 0000 sts Poti3,r24
2828 1a44 5C01 movw r10,r24
2829 .L299:
2830 1a46 8091 0000 lds r24,ParamSet+7
2831 1a4a E82F mov r30,r24
2832 1a4c F0E0 ldi r31,lo8(0)
2833 1a4e EE0F lsl r30
2834 1a50 FF1F rol r31
2835 1a52 E050 subi r30,lo8(-(PPM_in))
2836 1a54 F040 sbci r31,hi8(-(PPM_in))
2837 1a56 8081 ld r24,Z
2838 1a58 9181 ldd r25,Z+1
2839 1a5a A091 0000 lds r26,Poti4
2840 1a5e B091 0000 lds r27,(Poti4)+1
2841 1a62 8359 subi r24,lo8(-(109))
2842 1a64 9F4F sbci r25,hi8(-(109))
2843 1a66 8A17 cp r24,r26
2844 1a68 9B07 cpc r25,r27
2845 1a6a 04F4 brge .+4
2846 1a6c 0C94 0000 jmp .L300
2847 1a70 CD01 movw r24,r26
2848 1a72 0196 adiw r24,1
2849 1a74 9093 0000 sts (Poti4)+1,r25
2850 1a78 8093 0000 sts Poti4,r24
2851 1a7c DC01 movw r26,r24
2852 .L301:
2853 1a7e 8091 0000 lds r24,PPM_in+18
2854 1a82 9091 0000 lds r25,(PPM_in+18)+1
2855 1a86 E091 0000 lds r30,Poti5
2856 1a8a F091 0000 lds r31,(Poti5)+1
2857 1a8e 8359 subi r24,lo8(-(109))
2858 1a90 9F4F sbci r25,hi8(-(109))
2859 1a92 8E17 cp r24,r30
2860 1a94 9F07 cpc r25,r31
2861 1a96 04F4 brge .+4
2862 1a98 0C94 0000 jmp .L302
2863 1a9c CF01 movw r24,r30
2864 1a9e 0196 adiw r24,1
2865 1aa0 9093 0000 sts (Poti5)+1,r25
2866 1aa4 8093 0000 sts Poti5,r24
2867 1aa8 FC01 movw r30,r24
2868 .L303:
2869 1aaa 8091 0000 lds r24,PPM_in+20
2870 1aae 9091 0000 lds r25,(PPM_in+20)+1
2871 1ab2 6091 0000 lds r22,Poti6
2872 1ab6 7091 0000 lds r23,(Poti6)+1
2873 1aba 8359 subi r24,lo8(-(109))
2874 1abc 9F4F sbci r25,hi8(-(109))
2875 1abe 8617 cp r24,r22
2876 1ac0 9707 cpc r25,r23
2877 1ac2 04F4 brge .+4
2878 1ac4 0C94 0000 jmp .L304
2879 1ac8 CB01 movw r24,r22
2880 1aca 0196 adiw r24,1
2881 1acc 9093 0000 sts (Poti6)+1,r25
2882 1ad0 8093 0000 sts Poti6,r24
2883 1ad4 BC01 movw r22,r24
2884 .L305:
2885 1ad6 8091 0000 lds r24,PPM_in+22
2886 1ada 9091 0000 lds r25,(PPM_in+22)+1
2887 1ade 4091 0000 lds r20,Poti7
2888 1ae2 5091 0000 lds r21,(Poti7)+1
2889 1ae6 8359 subi r24,lo8(-(109))
2890 1ae8 9F4F sbci r25,hi8(-(109))
2891 1aea 8417 cp r24,r20
2892 1aec 9507 cpc r25,r21
2893 1aee 04F4 brge .+4
2894 1af0 0C94 0000 jmp .L306
2895 1af4 CA01 movw r24,r20
2896 1af6 0196 adiw r24,1
2897 1af8 9093 0000 sts (Poti7)+1,r25
2898 1afc 8093 0000 sts Poti7,r24
2899 1b00 AC01 movw r20,r24
2900 .L307:
2901 1b02 8091 0000 lds r24,PPM_in+24
2902 1b06 9091 0000 lds r25,(PPM_in+24)+1
2903 1b0a 2091 0000 lds r18,Poti8
2904 1b0e 3091 0000 lds r19,(Poti8)+1
2905 1b12 8359 subi r24,lo8(-(109))
2906 1b14 9F4F sbci r25,hi8(-(109))
2907 1b16 8217 cp r24,r18
2908 1b18 9307 cpc r25,r19
2909 1b1a 04F4 brge .+4
2910 1b1c 0C94 0000 jmp .L308
2911 1b20 C901 movw r24,r18
2912 1b22 0196 adiw r24,1
2913 1b24 9093 0000 sts (Poti8)+1,r25
2914 1b28 8093 0000 sts Poti8,r24
2915 1b2c 9C01 movw r18,r24
2916 .L309:
2917 1b2e F7FE sbrs r15,7
2918 1b30 00C0 rjmp .+4
2919 1b32 0C94 0000 jmp .L500
2920 1b36 8FEF ldi r24,lo8(255)
2921 1b38 E816 cp r14,r24
2922 1b3a F104 cpc r15,__zero_reg__
2923 1b3c 01F0 breq .L311
2924 1b3e 04F0 brlt .L311
2925 1b40 8FEF ldi r24,lo8(255)
2926 1b42 90E0 ldi r25,hi8(255)
2927 1b44 9093 0000 sts (Poti1)+1,r25
2928 1b48 8093 0000 sts Poti1,r24
2929 .L311:
2930 1b4c 17FF sbrs r17,7
2931 1b4e 00C0 rjmp .+4
2932 1b50 0C94 0000 jmp .L501
2933 1b54 0F3F cpi r16,255
2934 1b56 1105 cpc r17,__zero_reg__
2935 1b58 01F0 breq .L313
2936 1b5a 04F0 brlt .L313
2937 1b5c 8FEF ldi r24,lo8(255)
2938 1b5e 90E0 ldi r25,hi8(255)
2939 1b60 9093 0000 sts (Poti2)+1,r25
2940 1b64 8093 0000 sts Poti2,r24
2941 .L313:
2942 1b68 B7FE sbrs r11,7
2943 1b6a 00C0 rjmp .+4
2944 1b6c 0C94 0000 jmp .L502
2945 1b70 9FEF ldi r25,lo8(255)
2946 1b72 A916 cp r10,r25
2947 1b74 B104 cpc r11,__zero_reg__
2948 1b76 01F0 breq .L315
2949 1b78 04F0 brlt .L315
2950 1b7a 8FEF ldi r24,lo8(255)
2951 1b7c 90E0 ldi r25,hi8(255)
2952 1b7e 9093 0000 sts (Poti3)+1,r25
2953 1b82 8093 0000 sts Poti3,r24
2954 .L315:
2955 1b86 B7FF sbrs r27,7
2956 1b88 00C0 rjmp .+4
2957 1b8a 0C94 0000 jmp .L503
2958 1b8e AF3F cpi r26,255
2959 1b90 B105 cpc r27,__zero_reg__
2960 1b92 01F0 breq .L317
2961 1b94 04F0 brlt .L317
2962 1b96 8FEF ldi r24,lo8(255)
2963 1b98 90E0 ldi r25,hi8(255)
2964 1b9a 9093 0000 sts (Poti4)+1,r25
2965 1b9e 8093 0000 sts Poti4,r24
2966 .L317:
2967 1ba2 F7FF sbrs r31,7
2968 1ba4 00C0 rjmp .+4
2969 1ba6 0C94 0000 jmp .L504
2970 1baa EF3F cpi r30,255
2971 1bac F105 cpc r31,__zero_reg__
2972 1bae 01F0 breq .L319
2973 1bb0 04F0 brlt .L319
2974 1bb2 8FEF ldi r24,lo8(255)
2975 1bb4 90E0 ldi r25,hi8(255)
2976 1bb6 9093 0000 sts (Poti5)+1,r25
2977 1bba 8093 0000 sts Poti5,r24
2978 .L319:
2979 1bbe 77FF sbrs r23,7
2980 1bc0 00C0 rjmp .+4
2981 1bc2 0C94 0000 jmp .L505
2982 1bc6 6F3F cpi r22,255
2983 1bc8 7105 cpc r23,__zero_reg__
2984 1bca 01F0 breq .L321
2985 1bcc 04F0 brlt .L321
2986 1bce 8FEF ldi r24,lo8(255)
2987 1bd0 90E0 ldi r25,hi8(255)
2988 1bd2 9093 0000 sts (Poti6)+1,r25
2989 1bd6 8093 0000 sts Poti6,r24
2990 .L321:
2991 1bda 57FF sbrs r21,7
2992 1bdc 00C0 rjmp .+4
2993 1bde 0C94 0000 jmp .L506
2994 1be2 4F3F cpi r20,255
2995 1be4 5105 cpc r21,__zero_reg__
2996 1be6 01F0 breq .L323
2997 1be8 04F0 brlt .L323
2998 1bea 8FEF ldi r24,lo8(255)
2999 1bec 90E0 ldi r25,hi8(255)
3000 1bee 9093 0000 sts (Poti7)+1,r25
3001 1bf2 8093 0000 sts Poti7,r24
3002 .L323:
3003 1bf6 37FF sbrs r19,7
3004 1bf8 00C0 rjmp .+4
3005 1bfa 0C94 0000 jmp .L507
3006 1bfe 2F3F cpi r18,255
3007 1c00 3105 cpc r19,__zero_reg__
3008 1c02 01F0 breq .L325
3009 1c04 04F0 brlt .L325
3010 1c06 8FEF ldi r24,lo8(255)
3011 1c08 90E0 ldi r25,hi8(255)
3012 1c0a 9093 0000 sts (Poti8)+1,r25
3013 1c0e 8093 0000 sts Poti8,r24
3014 .L325:
3015 1c12 8091 0000 lds r24,ParamSet+2
3016 1c16 A82F mov r26,r24
3017 1c18 B0E0 ldi r27,lo8(0)
3018 1c1a AA0F lsl r26
3019 1c1c BB1F rol r27
3020 1c1e FD01 movw r30,r26
3021 1c20 E050 subi r30,lo8(-(PPM_in))
3022 1c22 F040 sbci r31,hi8(-(PPM_in))
3023 1c24 8081 ld r24,Z
3024 1c26 9181 ldd r25,Z+1
3025 1c28 8135 cpi r24,81
3026 1c2a 9105 cpc r25,__zero_reg__
3027 1c2c 04F4 brge .+2
3028 1c2e 00C0 rjmp .L326
3029 1c30 8091 0000 lds r24,MKFlags
3030 1c34 80FD sbrc r24,0
3031 1c36 00C0 rjmp .L326
3032 1c38 8091 0000 lds r24,ParamSet+3
3033 1c3c E82F mov r30,r24
3034 1c3e F0E0 ldi r31,lo8(0)
3035 1c40 EE0F lsl r30
3036 1c42 FF1F rol r31
3037 1c44 E050 subi r30,lo8(-(PPM_in))
3038 1c46 F040 sbci r31,hi8(-(PPM_in))
3039 1c48 8081 ld r24,Z
3040 1c4a 9181 ldd r25,Z+1
3041 1c4c 8C34 cpi r24,76
3042 1c4e 9105 cpc r25,__zero_reg__
3043 1c50 04F4 brge .+4
3044 1c52 0C94 0000 jmp .L327
3045 1c56 8091 0000 lds r24,delay_neutral.1982
3046 1c5a 8F5F subi r24,lo8(-(1))
3047 1c5c 8093 0000 sts delay_neutral.1982,r24
3048 1c60 893C cpi r24,lo8(-55)
3049 1c62 00F4 brsh .+2
3050 1c64 00C0 rjmp .L326
3051 1c66 1092 0000 sts delay_neutral.1982,__zero_reg__
3052 1c6a EBE0 ldi r30,lo8(11)
3053 1c6c ED15 cp r30,r13
3054 1c6e 00F4 brsh .+4
3055 1c70 0C94 0000 jmp .L329
3056 1c74 2998 cbi 37-0x20,1
3057 .L330:
3058 1c76 1092 0000 sts (ModelIsFlying)+1,__zero_reg__
3059 1c7a 1092 0000 sts ModelIsFlying,__zero_reg__
3060 1c7e 8091 0000 lds r24,ParamSet
3061 1c82 482F mov r20,r24
3062 1c84 50E0 ldi r21,lo8(0)
3063 1c86 440F lsl r20
3064 1c88 551F rol r21
3065 1c8a FA01 movw r30,r20
3066 1c8c E050 subi r30,lo8(-(PPM_in))
3067 1c8e F040 sbci r31,hi8(-(PPM_in))
3068 1c90 8081 ld r24,Z
3069 1c92 9181 ldd r25,Z+1
3070 1c94 8734 cpi r24,71
3071 1c96 9105 cpc r25,__zero_reg__
3072 1c98 04F4 brge .+4
3073 1c9a 0C94 0000 jmp .L331
3074 1c9e 8091 0000 lds r24,ParamSet+1
3075 1ca2 A82F mov r26,r24
3076 1ca4 B0E0 ldi r27,lo8(0)
3077 1ca6 AA0F lsl r26
3078 1ca8 BB1F rol r27
3079 .L332:
3080 1caa FD01 movw r30,r26
3081 1cac E050 subi r30,lo8(-(PPM_in))
3082 1cae F040 sbci r31,hi8(-(PPM_in))
3083 1cb0 8081 ld r24,Z
3084 1cb2 9181 ldd r25,Z+1
3085 1cb4 8734 cpi r24,71
3086 1cb6 9105 cpc r25,__zero_reg__
3087 1cb8 04F0 brlt .L335
3088 1cba FA01 movw r30,r20
3089 1cbc E050 subi r30,lo8(-(PPM_in))
3090 1cbe F040 sbci r31,hi8(-(PPM_in))
3091 1cc0 0190 ld __tmp_reg__,Z+
3092 1cc2 F081 ld r31,Z
3093 1cc4 E02D mov r30,__tmp_reg__
3094 .L335:
3095 1cc6 FD01 movw r30,r26
3096 1cc8 E050 subi r30,lo8(-(PPM_in))
3097 1cca F040 sbci r31,hi8(-(PPM_in))
3098 1ccc 8081 ld r24,Z
3099 1cce 9181 ldd r25,Z+1
3100 1cd0 8734 cpi r24,71
3101 1cd2 9105 cpc r25,__zero_reg__
3102 1cd4 04F4 brge .+4
3103 1cd6 0C94 0000 jmp .L336
3104 1cda FA01 movw r30,r20
3105 1cdc E050 subi r30,lo8(-(PPM_in))
3106 1cde F040 sbci r31,hi8(-(PPM_in))
3107 1ce0 8081 ld r24,Z
3108 1ce2 9181 ldd r25,Z+1
3109 1ce4 8734 cpi r24,71
3110 1ce6 9105 cpc r25,__zero_reg__
3111 1ce8 04F4 brge .+4
3112 1cea 0C94 0000 jmp .L336
3113 1cee 22E0 ldi r18,lo8(2)
3114 .L337:
3115 1cf0 FD01 movw r30,r26
3116 1cf2 E050 subi r30,lo8(-(PPM_in))
3117 1cf4 F040 sbci r31,hi8(-(PPM_in))
3118 1cf6 8081 ld r24,Z
3119 1cf8 9181 ldd r25,Z+1
3120 1cfa 8634 cpi r24,70
3121 1cfc 9105 cpc r25,__zero_reg__
3122 1cfe 04F4 brge .L338
3123 1d00 FA01 movw r30,r20
3124 1d02 E050 subi r30,lo8(-(PPM_in))
3125 1d04 F040 sbci r31,hi8(-(PPM_in))
3126 1d06 8081 ld r24,Z
3127 1d08 9181 ldd r25,Z+1
3128 1d0a 8734 cpi r24,71
3129 1d0c 9105 cpc r25,__zero_reg__
3130 1d0e 04F0 brlt .L338
3131 1d10 23E0 ldi r18,lo8(3)
3132 .L338:
3133 1d12 FD01 movw r30,r26
3134 1d14 E050 subi r30,lo8(-(PPM_in))
3135 1d16 F040 sbci r31,hi8(-(PPM_in))
3136 1d18 8081 ld r24,Z
3137 1d1a 9181 ldd r25,Z+1
3138 1d1c 8A5B subi r24,lo8(-70)
3139 1d1e 9F4F sbci r25,hi8(-70)
3140 1d20 04F4 brge .L339
3141 1d22 FA01 movw r30,r20
3142 1d24 E050 subi r30,lo8(-(PPM_in))
3143 1d26 F040 sbci r31,hi8(-(PPM_in))
3144 1d28 8081 ld r24,Z
3145 1d2a 9181 ldd r25,Z+1
3146 1d2c 8734 cpi r24,71
3147 1d2e 9105 cpc r25,__zero_reg__
3148 1d30 04F0 brlt .L339
3149 1d32 24E0 ldi r18,lo8(4)
3150 .L339:
3151 1d34 A050 subi r26,lo8(-(PPM_in))
3152 1d36 B040 sbci r27,hi8(-(PPM_in))
3153 1d38 8D91 ld r24,X+
3154 1d3a 9C91 ld r25,X
3155 1d3c 8A5B subi r24,lo8(-70)
3156 1d3e 9F4F sbci r25,hi8(-70)
3157 1d40 04F4 brge .L340
3158 1d42 4050 subi r20,lo8(-(PPM_in))
3159 1d44 5040 sbci r21,hi8(-(PPM_in))
3160 1d46 FA01 movw r30,r20
3161 1d48 8081 ld r24,Z
3162 1d4a 9181 ldd r25,Z+1
3163 1d4c 8634 cpi r24,70
3164 1d4e 9105 cpc r25,__zero_reg__
3165 1d50 04F4 brge .L340
3166 1d52 25E0 ldi r18,lo8(5)
3167 .L340:
3168 1d54 822F mov r24,r18
3169 1d56 0E94 0000 call SetActiveParamSet
3170 1d5a 0E94 0000 call GetActiveParamSet
3171 1d5e 0E94 0000 call ParamSet_ReadFromEEProm
3172 .L476:
3173 1d62 80E0 ldi r24,lo8(0)
3174 .L477:
3175 1d64 0E94 0000 call SetNeutral
3176 1d68 0E94 0000 call GetActiveParamSet
3177 1d6c 0E94 0000 call Beep
3178 1d70 8091 0000 lds r24,ParamSet+2
3179 1d74 A82F mov r26,r24
3180 1d76 B0E0 ldi r27,lo8(0)
3181 1d78 AA0F lsl r26
3182 1d7a BB1F rol r27
3183 .L326:
3184 1d7c A050 subi r26,lo8(-(PPM_in))
3185 1d7e B040 sbci r27,hi8(-(PPM_in))
3186 1d80 8D91 ld r24,X+
3187 1d82 9C91 ld r25,X
3188 1d84 8B5A subi r24,lo8(-85)
3189 1d86 9F4F sbci r25,hi8(-85)
3190 1d88 04F0 brlt .+2
3191 1d8a 00C0 rjmp .L290
3192 1d8c E091 0000 lds r30,ParamSet+3
3193 1d90 F0E0 ldi r31,lo8(0)
3194 1d92 EE0F lsl r30
3195 1d94 FF1F rol r31
3196 1d96 E050 subi r30,lo8(-(PPM_in))
3197 1d98 F040 sbci r31,hi8(-(PPM_in))
3198 1d9a 8081 ld r24,Z
3199 1d9c 9181 ldd r25,Z+1
3200 1d9e 855B subi r24,lo8(-75)
3201 1da0 9F4F sbci r25,hi8(-75)
3202 1da2 04F0 brlt .+2
3203 1da4 00C0 rjmp .L348
3204 1da6 8091 0000 lds r24,delay_startmotors.1983
3205 1daa 9091 0000 lds r25,(delay_startmotors.1983)+1
3206 1dae 883C cpi r24,200
3207 1db0 9105 cpc r25,__zero_reg__
3208 1db2 00F4 brsh .+4
3209 1db4 0C94 0000 jmp .L508
3210 1db8 883C cpi r24,200
3211 1dba 9105 cpc r25,__zero_reg__
3212 1dbc 01F4 brne .+4
3213 1dbe 0C94 0000 jmp .L509
3214 1dc2 8091 0000 lds r24,hiResPitchGyro
3215 1dc6 9091 0000 lds r25,(hiResPitchGyro)+1
3216 1dca AA27 clr r26
3217 1dcc 97FD sbrc r25,7
3218 1dce A095 com r26
3219 1dd0 BA2F mov r27,r26
3220 1dd2 4091 0000 lds r20,vibrationCalNick.1974
3221 1dd6 5091 0000 lds r21,(vibrationCalNick.1974)+1
3222 1dda 6091 0000 lds r22,(vibrationCalNick.1974)+2
3223 1dde 7091 0000 lds r23,(vibrationCalNick.1974)+3
3224 1de2 480F add r20,r24
3225 1de4 591F adc r21,r25
3226 1de6 6A1F adc r22,r26
3227 1de8 7B1F adc r23,r27
3228 1dea 4093 0000 sts vibrationCalNick.1974,r20
3229 1dee 5093 0000 sts (vibrationCalNick.1974)+1,r21
3230 1df2 6093 0000 sts (vibrationCalNick.1974)+2,r22
3231 1df6 7093 0000 sts (vibrationCalNick.1974)+3,r23
3232 1dfa 8091 0000 lds r24,hiResRollGyro
3233 1dfe 9091 0000 lds r25,(hiResRollGyro)+1
3234 1e02 AA27 clr r26
3235 1e04 97FD sbrc r25,7
3236 1e06 A095 com r26
3237 1e08 BA2F mov r27,r26
3238 1e0a A090 0000 lds r10,vibrationCalRoll.1975
3239 1e0e B090 0000 lds r11,(vibrationCalRoll.1975)+1
3240 1e12 C090 0000 lds r12,(vibrationCalRoll.1975)+2
3241 1e16 D090 0000 lds r13,(vibrationCalRoll.1975)+3
3242 1e1a A80E add r10,r24
3243 1e1c B91E adc r11,r25
3244 1e1e CA1E adc r12,r26
3245 1e20 DB1E adc r13,r27
3246 1e22 A092 0000 sts vibrationCalRoll.1975,r10
3247 1e26 B092 0000 sts (vibrationCalRoll.1975)+1,r11
3248 1e2a C092 0000 sts (vibrationCalRoll.1975)+2,r12
3249 1e2e D092 0000 sts (vibrationCalRoll.1975)+3,r13
3250 1e32 8091 0000 lds r24,rawYawGyroSum
3251 1e36 9091 0000 lds r25,(rawYawGyroSum)+1
3252 1e3a AA27 clr r26
3253 1e3c 97FD sbrc r25,7
3254 1e3e A095 com r26
3255 1e40 BA2F mov r27,r26
3256 1e42 6090 0000 lds r6,vibrationCalYaw.1976
3257 1e46 7090 0000 lds r7,(vibrationCalYaw.1976)+1
3258 1e4a 8090 0000 lds r8,(vibrationCalYaw.1976)+2
3259 1e4e 9090 0000 lds r9,(vibrationCalYaw.1976)+3
3260 1e52 680E add r6,r24
3261 1e54 791E adc r7,r25
3262 1e56 8A1E adc r8,r26
3263 1e58 9B1E adc r9,r27
3264 1e5a 6092 0000 sts vibrationCalYaw.1976,r6
3265 1e5e 7092 0000 sts (vibrationCalYaw.1976)+1,r7
3266 1e62 8092 0000 sts (vibrationCalYaw.1976)+2,r8
3267 1e66 9092 0000 sts (vibrationCalYaw.1976)+3,r9
3268 1e6a 2091 0000 lds r18,vibrationCalCount.1977
3269 1e6e 3091 0000 lds r19,(vibrationCalCount.1977)+1
3270 1e72 2F5F subi r18,lo8(-(1))
3271 1e74 3F4F sbci r19,hi8(-(1))
3272 1e76 3093 0000 sts (vibrationCalCount.1977)+1,r19
3273 1e7a 2093 0000 sts vibrationCalCount.1977,r18
3274 1e7e 8091 0000 lds r24,FCParam+15
3275 1e82 8823 tst r24
3276 1e84 01F4 brne .+4
3277 1e86 0C94 0000 jmp .L352
3278 1e8a 8091 0000 lds r24,FCParam+16
3279 1e8e 90E0 ldi r25,lo8(0)
3280 1e90 8058 subi r24,lo8(-(-128))
3281 1e92 9040 sbci r25,hi8(-(-128))
3282 1e94 AA27 clr r26
3283 1e96 97FD sbrc r25,7
3284 1e98 A095 com r26
3285 1e9a BA2F mov r27,r26
3286 1e9c 8093 0000 sts vibrationOffsetNick,r24
3287 1ea0 9093 0000 sts (vibrationOffsetNick)+1,r25
3288 1ea4 A093 0000 sts (vibrationOffsetNick)+2,r26
3289 1ea8 B093 0000 sts (vibrationOffsetNick)+3,r27
3290 1eac 8091 0000 lds r24,FCParam+17
3291 1eb0 90E0 ldi r25,lo8(0)
3292 1eb2 8058 subi r24,lo8(-(-128))
3293 1eb4 9040 sbci r25,hi8(-(-128))
3294 1eb6 AA27 clr r26
3295 1eb8 97FD sbrc r25,7
3296 1eba A095 com r26
3297 1ebc BA2F mov r27,r26
3298 1ebe 8093 0000 sts vibrationOffsetRoll,r24
3299 1ec2 9093 0000 sts (vibrationOffsetRoll)+1,r25
3300 1ec6 A093 0000 sts (vibrationOffsetRoll)+2,r26
3301 1eca B093 0000 sts (vibrationOffsetRoll)+3,r27
3302 .L353:
3303 1ece 1092 0000 sts SetPointYaw.1980,__zero_reg__
3304 1ed2 1092 0000 sts (SetPointYaw.1980)+1,__zero_reg__
3305 1ed6 1092 0000 sts (SetPointYaw.1980)+2,__zero_reg__
3306 1eda 1092 0000 sts (SetPointYaw.1980)+3,__zero_reg__
3307 1ede 1092 0000 sts ReadingIntegralGyroYaw,__zero_reg__
3308 1ee2 1092 0000 sts (ReadingIntegralGyroYaw)+1,__zero_reg__
3309 1ee6 1092 0000 sts (ReadingIntegralGyroYaw)+2,__zero_reg__
3310 1eea 1092 0000 sts (ReadingIntegralGyroYaw)+3,__zero_reg__
3311 1eee 1092 0000 sts ReadingIntegralGyroNick,__zero_reg__
3312 1ef2 1092 0000 sts (ReadingIntegralGyroNick)+1,__zero_reg__
3313 1ef6 1092 0000 sts (ReadingIntegralGyroNick)+2,__zero_reg__
3314 1efa 1092 0000 sts (ReadingIntegralGyroNick)+3,__zero_reg__
3315 1efe 1092 0000 sts ReadingIntegralGyroRoll,__zero_reg__
3316 1f02 1092 0000 sts (ReadingIntegralGyroRoll)+1,__zero_reg__
3317 1f06 1092 0000 sts (ReadingIntegralGyroRoll)+2,__zero_reg__
3318 1f0a 1092 0000 sts (ReadingIntegralGyroRoll)+3,__zero_reg__
3319 1f0e 1092 0000 sts IPartNick.1972,__zero_reg__
3320 1f12 1092 0000 sts (IPartNick.1972)+1,__zero_reg__
3321 1f16 1092 0000 sts (IPartNick.1972)+2,__zero_reg__
3322 1f1a 1092 0000 sts (IPartNick.1972)+3,__zero_reg__
3323 1f1e 1092 0000 sts IPartRoll.1973,__zero_reg__
3324 1f22 1092 0000 sts (IPartRoll.1973)+1,__zero_reg__
3325 1f26 1092 0000 sts (IPartRoll.1973)+2,__zero_reg__
3326 1f2a 1092 0000 sts (IPartRoll.1973)+3,__zero_reg__
3327 1f2e 00C0 rjmp .L350
3328 .L355:
3329 1f30 0E94 0000 call ParameterMapping
3330 1f34 E091 0000 lds r30,ParamSet
3331 1f38 F0E0 ldi r31,lo8(0)
3332 1f3a EE0F lsl r30
3333 1f3c FF1F rol r31
3334 1f3e DF01 movw r26,r30
3335 1f40 A050 subi r26,lo8(-(PPM_in))
3336 1f42 B040 sbci r27,hi8(-(PPM_in))
3337 1f44 8D91 ld r24,X+
3338 1f46 9C91 ld r25,X
3339 1f48 6091 0000 lds r22,ParamSet+15
3340 1f4c 70E0 ldi r23,lo8(0)
3341 1f4e 869F mul r24,r22
3342 1f50 9001 movw r18,r0
3343 1f52 879F mul r24,r23
3344 1f54 300D add r19,r0
3345 1f56 969F mul r25,r22
3346 1f58 300D add r19,r0
3347 1f5a 1124 clr r1
3348 1f5c 3093 0000 sts (stick_nick.1990)+1,r19
3349 1f60 2093 0000 sts stick_nick.1990,r18
3350 1f64 E050 subi r30,lo8(-(PPM_diff))
3351 1f66 F040 sbci r31,hi8(-(PPM_diff))
3352 1f68 8081 ld r24,Z
3353 1f6a 9181 ldd r25,Z+1
3354 1f6c 4091 0000 lds r20,ParamSet+16
3355 1f70 50E0 ldi r21,lo8(0)
3356 1f72 FC01 movw r30,r24
3357 1f74 E49F mul r30,r20
3358 1f76 C001 movw r24,r0
3359 1f78 E59F mul r30,r21
3360 1f7a 900D add r25,r0
3361 1f7c F49F mul r31,r20
3362 1f7e 900D add r25,r0
3363 1f80 1124 clr r1
3364 1f82 820F add r24,r18
3365 1f84 931F adc r25,r19
3366 1f86 9093 0000 sts (stick_nick.1990)+1,r25
3367 1f8a 8093 0000 sts stick_nick.1990,r24
3368 1f8e 2091 0000 lds r18,stickOffsetNick
3369 1f92 3091 0000 lds r19,(stickOffsetNick)+1
3370 1f96 821B sub r24,r18
3371 1f98 930B sbc r25,r19
3372 1f9a 2091 0000 lds r18,GPSStickNick
3373 1f9e 3091 0000 lds r19,(GPSStickNick)+1
3374 1fa2 821B sub r24,r18
3375 1fa4 930B sbc r25,r19
3376 1fa6 9093 0000 sts (StickNick)+1,r25
3377 1faa 8093 0000 sts StickNick,r24
3378 1fae E091 0000 lds r30,ParamSet+1
3379 1fb2 F0E0 ldi r31,lo8(0)
3380 1fb4 EE0F lsl r30
3381 1fb6 FF1F rol r31
3382 1fb8 DF01 movw r26,r30
3383 1fba A050 subi r26,lo8(-(PPM_in))
3384 1fbc B040 sbci r27,hi8(-(PPM_in))
3385 1fbe 8D91 ld r24,X+
3386 1fc0 9C91 ld r25,X
3387 1fc2 869F mul r24,r22
3388 1fc4 9001 movw r18,r0
3389 1fc6 879F mul r24,r23
3390 1fc8 300D add r19,r0
3391 1fca 969F mul r25,r22
3392 1fcc 300D add r19,r0
3393 1fce 1124 clr r1
3394 1fd0 3093 0000 sts (stick_roll.1991)+1,r19
3395 1fd4 2093 0000 sts stick_roll.1991,r18
3396 1fd8 E050 subi r30,lo8(-(PPM_diff))
3397 1fda F040 sbci r31,hi8(-(PPM_diff))
3398 1fdc 8081 ld r24,Z
3399 1fde 9181 ldd r25,Z+1
3400 1fe0 BC01 movw r22,r24
3401 1fe2 649F mul r22,r20
3402 1fe4 C001 movw r24,r0
3403 1fe6 659F mul r22,r21
3404 1fe8 900D add r25,r0
3405 1fea 749F mul r23,r20
3406 1fec 900D add r25,r0
3407 1fee 1124 clr r1
3408 1ff0 820F add r24,r18
3409 1ff2 931F adc r25,r19
3410 1ff4 9093 0000 sts (stick_roll.1991)+1,r25
3411 1ff8 8093 0000 sts stick_roll.1991,r24
3412 1ffc 2091 0000 lds r18,stickOffsetRoll
3413 2000 3091 0000 lds r19,(stickOffsetRoll)+1
3414 2004 821B sub r24,r18
3415 2006 930B sbc r25,r19
3416 2008 2091 0000 lds r18,GPSStickRoll
3417 200c 3091 0000 lds r19,(GPSStickRoll)+1
3418 2010 821B sub r24,r18
3419 2012 930B sbc r25,r19
3420 2014 9093 0000 sts (StickRoll)+1,r25
3421 2018 8093 0000 sts StickRoll,r24
3422 201c E091 0000 lds r30,ParamSet+3
3423 2020 F0E0 ldi r31,lo8(0)
3424 2022 EE0F lsl r30
3425 2024 FF1F rol r31
3426 2026 E050 subi r30,lo8(-(PPM_in))
3427 2028 F040 sbci r31,hi8(-(PPM_in))
3428 202a 8081 ld r24,Z
3429 202c 9181 ldd r25,Z+1
3430 202e 2227 clr r18
3431 2030 3327 clr r19
3432 2032 281B sub r18,r24
3433 2034 390B sbc r19,r25
3434 2036 3093 0000 sts (StickYaw)+1,r19
3435 203a 2093 0000 sts StickYaw,r18
3436 203e 8091 0000 lds r24,ParamSet+8
3437 2042 8872 andi r24,lo8(40)
3438 2044 01F0 breq .L357
3439 2046 2330 cpi r18,3
3440 2048 3105 cpc r19,__zero_reg__
3441 204a 04F0 brlt .+2
3442 204c 00C0 rjmp .L510
3443 204e 7FEF ldi r23,hi8(-2)
3444 2050 2E3F cpi r18,lo8(-2)
3445 2052 3707 cpc r19,r23
3446 2054 04F0 brlt .+2
3447 2056 00C0 rjmp .L359
3448 2058 2E5F subi r18,lo8(-(2))
3449 205a 3F4F sbci r19,hi8(-(2))
3450 205c 3093 0000 sts (StickYaw)+1,r19
3451 2060 2093 0000 sts StickYaw,r18
3452 .L357:
3453 2064 E091 0000 lds r30,ParamSet+2
3454 2068 F0E0 ldi r31,lo8(0)
3455 206a EE0F lsl r30
3456 206c FF1F rol r31
3457 206e E050 subi r30,lo8(-(PPM_in))
3458 2070 F040 sbci r31,hi8(-(PPM_in))
3459 2072 0190 ld __tmp_reg__,Z+
3460 2074 F081 ld r31,Z
3461 2076 E02D mov r30,__tmp_reg__
3462 2078 E858 subi r30,lo8(-(120))
3463 207a FF4F sbci r31,hi8(-(120))
3464 207c F093 0000 sts (StickGas)+1,r31
3465 2080 E093 0000 sts StickGas,r30
3466 2084 8091 0000 lds r24,FCParam+6
3467 2088 865F subi r24,lo8(-(10))
3468 208a 8093 0000 sts GyroPFactor,r24
3469 208e 9091 0000 lds r25,FCParam+7
3470 2092 9093 0000 sts GyroIFactor,r25
3471 2096 8093 0000 sts GyroYawPFactor,r24
3472 209a 9093 0000 sts GyroYawIFactor,r25
3473 209e 8091 0000 lds r24,ExternControl+10
3474 20a2 80FF sbrs r24,0
3475 20a4 00C0 rjmp .L360
3476 20a6 8091 0000 lds r24,FCParam+24
3477 20aa 8138 cpi r24,lo8(-127)
3478 20ac 00F4 brsh .+2
3479 20ae 00C0 rjmp .L360
3480 20b0 2091 0000 lds r18,ParamSet+15
3481 20b4 30E0 ldi r19,lo8(0)
3482 20b6 8091 0000 lds r24,ExternControl+3
3483 20ba 9927 clr r25
3484 20bc 87FD sbrc r24,7
3485 20be 9095 com r25
3486 20c0 AC01 movw r20,r24
3487 20c2 249F mul r18,r20
3488 20c4 C001 movw r24,r0
3489 20c6 259F mul r18,r21
3490 20c8 900D add r25,r0
3491 20ca 349F mul r19,r20
3492 20cc 900D add r25,r0
3493 20ce 1124 clr r1
3494 20d0 A090 0000 lds r10,StickNick
3495 20d4 B090 0000 lds r11,(StickNick)+1
3496 20d8 A80E add r10,r24
3497 20da B91E adc r11,r25
3498 20dc B092 0000 sts (StickNick)+1,r11
3499 20e0 A092 0000 sts StickNick,r10
3500 20e4 8091 0000 lds r24,ExternControl+4
3501 20e8 9927 clr r25
3502 20ea 87FD sbrc r24,7
3503 20ec 9095 com r25
3504 20ee BC01 movw r22,r24
3505 20f0 269F mul r18,r22
3506 20f2 C001 movw r24,r0
3507 20f4 279F mul r18,r23
3508 20f6 900D add r25,r0
3509 20f8 369F mul r19,r22
3510 20fa 900D add r25,r0
3511 20fc 1124 clr r1
3512 20fe C090 0000 lds r12,StickRoll
3513 2102 D090 0000 lds r13,(StickRoll)+1
3514 2106 C80E add r12,r24
3515 2108 D91E adc r13,r25
3516 210a D092 0000 sts (StickRoll)+1,r13
3517 210e C092 0000 sts StickRoll,r12
3518 2112 2091 0000 lds r18,ExternControl+5
3519 2116 3327 clr r19
3520 2118 27FD sbrc r18,7
3521 211a 3095 com r19
3522 211c 8091 0000 lds r24,StickYaw
3523 2120 9091 0000 lds r25,(StickYaw)+1
3524 2124 820F add r24,r18
3525 2126 931F adc r25,r19
3526 2128 9093 0000 sts (StickYaw)+1,r25
3527 212c 8093 0000 sts StickYaw,r24
3528 2130 2091 0000 lds r18,ParamSet+13
3529 2134 30E0 ldi r19,lo8(0)
3530 2136 8091 0000 lds r24,ExternControl+7
3531 213a 9927 clr r25
3532 213c 87FD sbrc r24,7
3533 213e 9095 com r25
3534 2140 AC01 movw r20,r24
3535 2142 249F mul r18,r20
3536 2144 C001 movw r24,r0
3537 2146 259F mul r18,r21
3538 2148 900D add r25,r0
3539 214a 349F mul r19,r20
3540 214c 900D add r25,r0
3541 214e 1124 clr r1
3542 2150 9093 0000 sts (ExternHeightValue)+1,r25
3543 2154 8093 0000 sts ExternHeightValue,r24
3544 2158 8091 0000 lds r24,ExternControl+6
3545 215c 90E0 ldi r25,lo8(0)
3546 215e 8E17 cp r24,r30
3547 2160 9F07 cpc r25,r31
3548 2162 04F4 brge .L360
3549 2164 9093 0000 sts (StickGas)+1,r25
3550 2168 8093 0000 sts StickGas,r24
3551 .L360:
3552 216c 8091 0000 lds r24,StickGas
3553 2170 9091 0000 lds r25,(StickGas)+1
3554 2174 97FD sbrc r25,7
3555 2176 00C0 rjmp .L511
3556 2178 8091 0000 lds r24,ParamSet+8
3557 217c 82FD sbrc r24,2
3558 217e 00C0 rjmp .L512
3559 .L362:
3560 2180 6091 0000 lds r22,StickNick
3561 2184 7091 0000 lds r23,StickNick+1
3562 2188 4091 0000 lds r20,MaxStickNick
3563 218c 5091 0000 lds r21,(MaxStickNick)+1
3564 2190 CB01 movw r24,r22
3565 2192 9C01 movw r18,r24
3566 2194 97FD sbrc r25,7
3567 2196 00C0 rjmp .L513
3568 2198 3595 asr r19
3569 219a 2795 ror r18
3570 219c 3595 asr r19
3571 219e 2795 ror r18
3572 21a0 37FD sbrc r19,7
3573 21a2 00C0 rjmp .L514
3574 .L365:
3575 21a4 4217 cp r20,r18
3576 21a6 5307 cpc r21,r19
3577 21a8 04F4 brge .+2
3578 21aa 00C0 rjmp .L515
3579 21ac 4150 subi r20,lo8(-(-1))
3580 21ae 5040 sbci r21,hi8(-(-1))
3581 21b0 5093 0000 sts (MaxStickNick)+1,r21
3582 21b4 4093 0000 sts MaxStickNick,r20
3583 .L367:
3584 21b8 6091 0000 lds r22,StickRoll
3585 21bc 7091 0000 lds r23,StickRoll+1
3586 21c0 4091 0000 lds r20,MaxStickRoll
3587 21c4 5091 0000 lds r21,(MaxStickRoll)+1
3588 21c8 CB01 movw r24,r22
3589 21ca 9C01 movw r18,r24
3590 21cc 97FD sbrc r25,7
3591 21ce 00C0 rjmp .L516
3592 .L369:
3593 21d0 3595 asr r19
3594 21d2 2795 ror r18
3595 21d4 3595 asr r19
3596 21d6 2795 ror r18
3597 21d8 37FD sbrc r19,7
3598 21da 00C0 rjmp .L517
3599 .L370:
3600 21dc 4217 cp r20,r18
3601 21de 5307 cpc r21,r19
3602 21e0 04F0 brlt .+2
3603 21e2 00C0 rjmp .L368
3604 21e4 9B01 movw r18,r22
3605 21e6 77FD sbrc r23,7
3606 21e8 00C0 rjmp .L518
3607 .L371:
3608 21ea C901 movw r24,r18
3609 21ec 9595 asr r25
3610 21ee 8795 ror r24
3611 21f0 9595 asr r25
3612 21f2 8795 ror r24
3613 21f4 9093 0000 sts (MaxStickRoll)+1,r25
3614 21f8 8093 0000 sts MaxStickRoll,r24
3615 21fc 8536 cpi r24,101
3616 21fe 9105 cpc r25,__zero_reg__
3617 2200 04F0 brlt .+2
3618 2202 00C0 rjmp .L519
3619 .L372:
3620 2204 8091 0000 lds r24,ParamSet+1
3621 2208 A82F mov r26,r24
3622 220a B0E0 ldi r27,lo8(0)
3623 220c AA0F lsl r26
3624 220e BB1F rol r27
3625 2210 FD01 movw r30,r26
3626 2212 E050 subi r30,lo8(-(PPM_in))
3627 2214 F040 sbci r31,hi8(-(PPM_in))
3628 2216 2081 ld r18,Z
3629 2218 3181 ldd r19,Z+1
3630 221a 8091 0000 lds r24,ParamSet+40
3631 221e 482F mov r20,r24
3632 2220 50E0 ldi r21,lo8(0)
3633 2222 4217 cp r20,r18
3634 2224 5307 cpc r21,r19
3635 2226 04F4 brge .L373
3636 2228 8091 0000 lds r24,ParamSet+75
3637 222c 82FD sbrc r24,2
3638 222e 00C0 rjmp .L520
3639 .L373:
3640 2230 7091 0000 lds r23,LoopingLeft
3641 2234 7723 tst r23
3642 2236 01F0 breq .L374
3643 2238 FD01 movw r30,r26
3644 223a E050 subi r30,lo8(-(PPM_in))
3645 223c F040 sbci r31,hi8(-(PPM_in))
3646 223e 2081 ld r18,Z
3647 2240 3181 ldd r19,Z+1
3648 2242 8091 0000 lds r24,ParamSet+41
3649 2246 FA01 movw r30,r20
3650 2248 E81B sub r30,r24
3651 224a F109 sbc r31,__zero_reg__
3652 224c 2E17 cp r18,r30
3653 224e 3F07 cpc r19,r31
3654 2250 04F4 brge .+2
3655 2252 00C0 rjmp .L521
3656 .L374:
3657 2254 FD01 movw r30,r26
3658 2256 E050 subi r30,lo8(-(PPM_in))
3659 2258 F040 sbci r31,hi8(-(PPM_in))
3660 225a 2081 ld r18,Z
3661 225c 3181 ldd r19,Z+1
3662 225e 8827 clr r24
3663 2260 9927 clr r25
3664 2262 841B sub r24,r20
3665 2264 950B sbc r25,r21
3666 2266 2817 cp r18,r24
3667 2268 3907 cpc r19,r25
3668 226a 04F4 brge .L375
3669 226c 8091 0000 lds r24,ParamSet+75
3670 2270 83FD sbrc r24,3
3671 2272 00C0 rjmp .L522
3672 .L375:
3673 2274 8091 0000 lds r24,LoopingRight
3674 2278 8823 tst r24
3675 227a 01F0 breq .L376
3676 227c A050 subi r26,lo8(-(PPM_in))
3677 227e B040 sbci r27,hi8(-(PPM_in))
3678 2280 2D91 ld r18,X+
3679 2282 3C91 ld r19,X
3680 2284 8091 0000 lds r24,ParamSet+41
3681 2288 90E0 ldi r25,lo8(0)
3682 228a 841B sub r24,r20
3683 228c 950B sbc r25,r21
3684 228e 8217 cp r24,r18
3685 2290 9307 cpc r25,r19
3686 2292 04F4 brge .+2
3687 2294 00C0 rjmp .L523
3688 .L376:
3689 2296 8091 0000 lds r24,ParamSet
3690 229a A82F mov r26,r24
3691 229c B0E0 ldi r27,lo8(0)
3692 229e AA0F lsl r26
3693 22a0 BB1F rol r27
3694 22a2 FD01 movw r30,r26
3695 22a4 E050 subi r30,lo8(-(PPM_in))
3696 22a6 F040 sbci r31,hi8(-(PPM_in))
3697 22a8 8081 ld r24,Z
3698 22aa 9181 ldd r25,Z+1
3699 22ac 4817 cp r20,r24
3700 22ae 5907 cpc r21,r25
3701 22b0 04F4 brge .L377
3702 22b2 8091 0000 lds r24,ParamSet+75
3703 22b6 80FD sbrc r24,0
3704 22b8 00C0 rjmp .L524
3705 .L377:
3706 22ba 6091 0000 lds r22,LoopingTop
3707 22be 6623 tst r22
3708 22c0 01F0 breq .L378
3709 22c2 FD01 movw r30,r26
3710 22c4 E050 subi r30,lo8(-(PPM_in))
3711 22c6 F040 sbci r31,hi8(-(PPM_in))
3712 22c8 2081 ld r18,Z
3713 22ca 3181 ldd r19,Z+1
3714 22cc 8091 0000 lds r24,ParamSet+41
3715 22d0 FA01 movw r30,r20
3716 22d2 E81B sub r30,r24
3717 22d4 F109 sbc r31,__zero_reg__
3718 22d6 2E17 cp r18,r30
3719 22d8 3F07 cpc r19,r31
3720 22da 04F4 brge .+2
3721 22dc 00C0 rjmp .L525
3722 .L378:
3723 22de FD01 movw r30,r26
3724 22e0 E050 subi r30,lo8(-(PPM_in))
3725 22e2 F040 sbci r31,hi8(-(PPM_in))
3726 22e4 2081 ld r18,Z
3727 22e6 3181 ldd r19,Z+1
3728 22e8 8827 clr r24
3729 22ea 9927 clr r25
3730 22ec 841B sub r24,r20
3731 22ee 950B sbc r25,r21
3732 22f0 2817 cp r18,r24
3733 22f2 3907 cpc r19,r25
3734 22f4 04F4 brge .L379
3735 22f6 8091 0000 lds r24,ParamSet+75
3736 22fa 81FD sbrc r24,1
3737 22fc 00C0 rjmp .L526
3738 .L379:
3739 22fe 8091 0000 lds r24,LoopingDown
3740 2302 8823 tst r24
3741 2304 01F0 breq .L380
3742 2306 A050 subi r26,lo8(-(PPM_in))
3743 2308 B040 sbci r27,hi8(-(PPM_in))
3744 230a 2D91 ld r18,X+
3745 230c 3C91 ld r19,X
3746 230e 8091 0000 lds r24,ParamSet+41
3747 2312 90E0 ldi r25,lo8(0)
3748 2314 841B sub r24,r20
3749 2316 950B sbc r25,r21
3750 2318 8217 cp r24,r18
3751 231a 9307 cpc r25,r19
3752 231c 04F4 brge .+2
3753 231e 00C0 rjmp .L527
3754 .L380:
3755 2320 7723 tst r23
3756 2322 01F4 brne .L381
3757 2324 8091 0000 lds r24,LoopingRight
3758 2328 8823 tst r24
3759 232a 01F4 brne .+2
3760 232c 00C0 rjmp .L382
3761 .L381:
3762 232e 81E0 ldi r24,lo8(1)
3763 2330 8093 0000 sts LoopingRoll,r24
3764 .L383:
3765 2334 6623 tst r22
3766 2336 01F4 brne .L384
3767 2338 8091 0000 lds r24,LoopingDown
3768 233c 8823 tst r24
3769 233e 01F4 brne .+2
3770 2340 00C0 rjmp .L385
3771 .L384:
3772 2342 81E0 ldi r24,lo8(1)
3773 2344 8093 0000 sts LoopingNick,r24
3774 2348 1092 0000 sts LoopingRoll,__zero_reg__
3775 234c 1092 0000 sts LoopingLeft,__zero_reg__
3776 2350 1092 0000 sts LoopingRight,__zero_reg__
3777 2354 8091 0000 lds r24,LoopingRoll
3778 2358 8823 tst r24
3779 235a 01F4 brne .+4
3780 235c 0C94 0000 jmp .L528
3781 .L386:
3782 2360 8091 0000 lds r24,ParamSet+39
3783 2364 90E0 ldi r25,lo8(0)
3784 2366 2F81 ldd r18,Y+7
3785 2368 3885 ldd r19,Y+8
3786 236a 8217 cp r24,r18
3787 236c 9307 cpc r25,r19
3788 236e 04F4 brge .+2
3789 2370 00C0 rjmp .L529
3790 2372 81E0 ldi r24,lo8(1)
3791 2374 8093 0000 sts FunnelCourse,r24
3792 2378 0C94 0000 jmp .L387
3793 .L390:
3794 237c 1092 0000 sts (ADCycleCount)+1,__zero_reg__
3795 2380 1092 0000 sts ADCycleCount,__zero_reg__
3796 2384 0C94 0000 jmp .L391
3797 .L496:
3798 2388 0396 adiw r24,3
3799 238a 00C0 rjmp .L462
3800 .L495:
3801 238c 0894 sec
3802 238e E11C adc r14,__zero_reg__
3803 2390 F11C adc r15,__zero_reg__
3804 2392 00C0 rjmp .L461
3805 .L497:
3806 /* epilogue start */
3807 2394 6B96 adiw r28,27
3808 2396 0FB6 in __tmp_reg__,__SREG__
3809 2398 F894 cli
3810 239a DEBF out __SP_H__,r29
3811 239c 0FBE out __SREG__,__tmp_reg__
3812 239e CDBF out __SP_L__,r28
3813 23a0 CF91 pop r28
3814 23a2 DF91 pop r29
3815 23a4 1F91 pop r17
3816 23a6 0F91 pop r16
3817 23a8 FF90 pop r15
3818 23aa EF90 pop r14
3819 23ac DF90 pop r13
3820 23ae CF90 pop r12
3821 23b0 BF90 pop r11
3822 23b2 AF90 pop r10
3823 23b4 9F90 pop r9
3824 23b6 8F90 pop r8
3825 23b8 7F90 pop r7
3826 23ba 6F90 pop r6
3827 23bc 5F90 pop r5
3828 23be 4F90 pop r4
3829 23c0 3F90 pop r3
3830 23c2 2F90 pop r2
3831 23c4 0895 ret
3832 .L454:
3833 23c6 A216 cp r10,r18
3834 23c8 B306 cpc r11,r19
3835 23ca 04F0 brlt .+2
3836 23cc 00C0 rjmp .L455
3837 23ce 9501 movw r18,r10
3838 23d0 00C0 rjmp .L455
3839 .L451:
3840 23d2 81E0 ldi r24,lo8(64001)
3841 23d4 E816 cp r14,r24
3842 23d6 8AEF ldi r24,hi8(64001)
3843 23d8 F806 cpc r15,r24
3844 23da 80E0 ldi r24,hlo8(64001)
3845 23dc 0807 cpc r16,r24
3846 23de 80E0 ldi r24,hhi8(64001)
3847 23e0 1807 cpc r17,r24
3848 23e2 04F4 brge .+2
3849 23e4 00C0 rjmp .L452
3850 23e6 80E0 ldi r24,lo8(64000)
3851 23e8 9AEF ldi r25,hi8(64000)
3852 23ea A0E0 ldi r26,hlo8(64000)
3853 23ec B0E0 ldi r27,hhi8(64000)
3854 23ee 8093 0000 sts IPartRoll.1973,r24
3855 23f2 9093 0000 sts (IPartRoll.1973)+1,r25
3856 23f6 A093 0000 sts (IPartRoll.1973)+2,r26
3857 23fa B093 0000 sts (IPartRoll.1973)+3,r27
3858 23fe E12C mov r14,__zero_reg__
3859 2400 0AEF ldi r16,hi8(64000)
3860 2402 F02E mov r15,r16
3861 2404 012D mov r16,__zero_reg__
3862 2406 112D mov r17,__zero_reg__
3863 2408 00C0 rjmp .L452
3864 .L449:
3865 240a 8981 ldd r24,Y+1
3866 240c 9A81 ldd r25,Y+2
3867 240e 8C19 sub r24,r12
3868 2410 9D09 sbc r25,r13
3869 2412 AA27 clr r26
3870 2414 97FD sbrc r25,7
3871 2416 A095 com r26
3872 2418 BA2F mov r27,r26
3873 241a E090 0000 lds r14,IPartRoll.1973
3874 241e F090 0000 lds r15,(IPartRoll.1973)+1
3875 2422 0091 0000 lds r16,(IPartRoll.1973)+2
3876 2426 1091 0000 lds r17,(IPartRoll.1973)+3
3877 242a E80E add r14,r24
3878 242c F91E adc r15,r25
3879 242e 0A1F adc r16,r26
3880 2430 1B1F adc r17,r27
3881 2432 E092 0000 sts IPartRoll.1973,r14
3882 2436 F092 0000 sts (IPartRoll.1973)+1,r15
3883 243a 0093 0000 sts (IPartRoll.1973)+2,r16
3884 243e 1093 0000 sts (IPartRoll.1973)+3,r17
3885 2442 00C0 rjmp .L450
3886 .L447:
3887 2444 21E0 ldi r18,lo8(64001)
3888 2446 E216 cp r14,r18
3889 2448 2AEF ldi r18,hi8(64001)
3890 244a F206 cpc r15,r18
3891 244c 20E0 ldi r18,hlo8(64001)
3892 244e 0207 cpc r16,r18
3893 2450 20E0 ldi r18,hhi8(64001)
3894 2452 1207 cpc r17,r18
3895 2454 04F4 brge .+2
3896 2456 00C0 rjmp .L448
3897 2458 80E0 ldi r24,lo8(64000)
3898 245a 9AEF ldi r25,hi8(64000)
3899 245c A0E0 ldi r26,hlo8(64000)
3900 245e B0E0 ldi r27,hhi8(64000)
3901 2460 8093 0000 sts IPartNick.1972,r24
3902 2464 9093 0000 sts (IPartNick.1972)+1,r25
3903 2468 A093 0000 sts (IPartNick.1972)+2,r26
3904 246c B093 0000 sts (IPartNick.1972)+3,r27
3905 2470 E12C mov r14,__zero_reg__
3906 2472 9AEF ldi r25,hi8(64000)
3907 2474 F92E mov r15,r25
3908 2476 012D mov r16,__zero_reg__
3909 2478 112D mov r17,__zero_reg__
3910 247a 00C0 rjmp .L448
3911 .L445:
3912 247c C101 movw r24,r2
3913 247e 8A19 sub r24,r10
3914 2480 9B09 sbc r25,r11
3915 2482 AA27 clr r26
3916 2484 97FD sbrc r25,7
3917 2486 A095 com r26
3918 2488 BA2F mov r27,r26
3919 248a E090 0000 lds r14,IPartNick.1972
3920 248e F090 0000 lds r15,(IPartNick.1972)+1
3921 2492 0091 0000 lds r16,(IPartNick.1972)+2
3922 2496 1091 0000 lds r17,(IPartNick.1972)+3
3923 249a E80E add r14,r24
3924 249c F91E adc r15,r25
3925 249e 0A1F adc r16,r26
3926 24a0 1B1F adc r17,r27
3927 24a2 E092 0000 sts IPartNick.1972,r14
3928 24a6 F092 0000 sts (IPartNick.1972)+1,r15
3929 24aa 0093 0000 sts (IPartNick.1972)+2,r16
3930 24ae 1093 0000 sts (IPartNick.1972)+3,r17
3931 24b2 00C0 rjmp .L446
3932 .L437:
3933 24b4 C701 movw r24,r14
3934 24b6 8135 cpi r24,81
3935 24b8 9105 cpc r25,__zero_reg__
3936 24ba 04F0 brlt .+2
3937 24bc 00C0 rjmp .L530
3938 24be 9C01 movw r18,r24
3939 24c0 5FEF ldi r21,hi8(-80)
3940 24c2 803B cpi r24,lo8(-80)
3941 24c4 9507 cpc r25,r21
3942 24c6 04F0 brlt .+2
3943 24c8 00C0 rjmp .L439
3944 .L540:
3945 24ca 20EB ldi r18,lo8(-80)
3946 24cc 3FEF ldi r19,hi8(-80)
3947 24ce 00C0 rjmp .L439
3948 .L426:
3949 24d0 ED85 ldd r30,Y+13
3950 24d2 6E2F mov r22,r30
3951 24d4 70E0 ldi r23,lo8(0)
3952 24d6 80E0 ldi r24,lo8(0)
3953 24d8 90E0 ldi r25,hi8(0)
3954 24da 2091 0000 lds r18,IntegralGyroRoll
3955 24de 3091 0000 lds r19,(IntegralGyroRoll)+1
3956 24e2 4091 0000 lds r20,(IntegralGyroRoll)+2
3957 24e6 5091 0000 lds r21,(IntegralGyroRoll)+3
3958 24ea 0E94 0000 call __mulsi3
3959 24ee 28EF ldi r18,lo8(11000)
3960 24f0 3AE2 ldi r19,hi8(11000)
3961 24f2 40E0 ldi r20,hlo8(11000)
3962 24f4 50E0 ldi r21,hhi8(11000)
3963 24f6 0E94 0000 call __divmodsi4
3964 24fa 3C83 std Y+4,r19
3965 24fc 2B83 std Y+3,r18
3966 24fe 3901 movw r6,r18
3967 2500 0C94 0000 jmp .L427
3968 .L424:
3969 2504 8091 0000 lds r24,GyroIFactor
3970 2508 8D87 std Y+13,r24
3971 250a 682F mov r22,r24
3972 250c 70E0 ldi r23,lo8(0)
3973 250e 80E0 ldi r24,lo8(0)
3974 2510 90E0 ldi r25,hi8(0)
3975 2512 2091 0000 lds r18,IntegralGyroNick
3976 2516 3091 0000 lds r19,(IntegralGyroNick)+1
3977 251a 4091 0000 lds r20,(IntegralGyroNick)+2
3978 251e 5091 0000 lds r21,(IntegralGyroNick)+3
3979 2522 0E94 0000 call __mulsi3
3980 2526 28EF ldi r18,lo8(11000)
3981 2528 3AE2 ldi r19,hi8(11000)
3982 252a 40E0 ldi r20,hlo8(11000)
3983 252c 50E0 ldi r21,hhi8(11000)
3984 252e 0E94 0000 call __divmodsi4
3985 2532 3E83 std Y+6,r19
3986 2534 2D83 std Y+5,r18
3987 2536 1901 movw r2,r18
3988 2538 0C94 0000 jmp .L425
3989 .L475:
3990 253c 88E1 ldi r24,lo8(24)
3991 253e 8093 0000 sts TimerDebugOut.1985,r24
3992 2542 6091 0000 lds r22,IntegralGyroNick
3993 2546 7091 0000 lds r23,(IntegralGyroNick)+1
3994 254a 8091 0000 lds r24,(IntegralGyroNick)+2
3995 254e 9091 0000 lds r25,(IntegralGyroNick)+3
3996 2552 2AE0 ldi r18,lo8(10)
3997 2554 30E0 ldi r19,hi8(10)
3998 2556 40E0 ldi r20,hlo8(10)
3999 2558 50E0 ldi r21,hhi8(10)
4000 255a 0E94 0000 call __mulsi3
4001 255e 2FEB ldi r18,lo8(1215)
4002 2560 34E0 ldi r19,hi8(1215)
4003 2562 40E0 ldi r20,hlo8(1215)
4004 2564 50E0 ldi r21,hhi8(1215)
4005 2566 0E94 0000 call __divmodsi4
4006 256a 3093 0000 sts (DebugOut+2)+1,r19
4007 256e 2093 0000 sts DebugOut+2,r18
4008 2572 6091 0000 lds r22,IntegralGyroRoll
4009 2576 7091 0000 lds r23,(IntegralGyroRoll)+1
4010 257a 8091 0000 lds r24,(IntegralGyroRoll)+2
4011 257e 9091 0000 lds r25,(IntegralGyroRoll)+3
4012 2582 2AE0 ldi r18,lo8(10)
4013 2584 30E0 ldi r19,hi8(10)
4014 2586 40E0 ldi r20,hlo8(10)
4015 2588 50E0 ldi r21,hhi8(10)
4016 258a 0E94 0000 call __mulsi3
4017 258e 2FEB ldi r18,lo8(1215)
4018 2590 34E0 ldi r19,hi8(1215)
4019 2592 40E0 ldi r20,hlo8(1215)
4020 2594 50E0 ldi r21,hhi8(1215)
4021 2596 0E94 0000 call __divmodsi4
4022 259a 3093 0000 sts (DebugOut+4)+1,r19
4023 259e 2093 0000 sts DebugOut+4,r18
4024 25a2 4090 0000 lds r4,GyroNick
4025 25a6 5090 0000 lds r5,(GyroNick)+1
4026 25aa 5092 0000 sts (DebugOut+6)+1,r5
4027 25ae 4092 0000 sts DebugOut+6,r4
4028 25b2 8090 0000 lds r8,GyroRoll
4029 25b6 9090 0000 lds r9,(GyroRoll)+1
4030 25ba 9092 0000 sts (DebugOut+8)+1,r9
4031 25be 8092 0000 sts DebugOut+8,r8
4032 25c2 8B85 ldd r24,Y+11
4033 25c4 9C85 ldd r25,Y+12
4034 25c6 9093 0000 sts (DebugOut+10)+1,r25
4035 25ca 8093 0000 sts DebugOut+10,r24
4036 25ce 8091 0000 lds r24,ReadingHeight
4037 25d2 9091 0000 lds r25,(ReadingHeight)+1
4038 25d6 9093 0000 sts (DebugOut+12)+1,r25
4039 25da 8093 0000 sts DebugOut+12,r24
4040 25de 8091 0000 lds r24,CompassHeading
4041 25e2 9091 0000 lds r25,(CompassHeading)+1
4042 25e6 9093 0000 sts (DebugOut+18)+1,r25
4043 25ea 8093 0000 sts DebugOut+18,r24
4044 25ee 8091 0000 lds r24,UBat
4045 25f2 9091 0000 lds r25,(UBat)+1
4046 25f6 9093 0000 sts (DebugOut+20)+1,r25
4047 25fa 8093 0000 sts DebugOut+20,r24
4048 25fe 8091 0000 lds r24,RC_Quality
4049 2602 9091 0000 lds r25,(RC_Quality)+1
4050 2606 9093 0000 sts (DebugOut+22)+1,r25
4051 260a 8093 0000 sts DebugOut+22,r24
4052 260e 6091 0000 lds r22,YawGyroHeading
4053 2612 7091 0000 lds r23,(YawGyroHeading)+1
4054 2616 8091 0000 lds r24,(YawGyroHeading)+2
4055 261a 9091 0000 lds r25,(YawGyroHeading)+3
4056 261e 20E0 ldi r18,lo8(512)
4057 2620 32E0 ldi r19,hi8(512)
4058 2622 40E0 ldi r20,hlo8(512)
4059 2624 50E0 ldi r21,hhi8(512)
4060 2626 0E94 0000 call __divmodsi4
4061 262a 3093 0000 sts (DebugOut+24)+1,r19
4062 262e 2093 0000 sts DebugOut+24,r18
4063 2632 80E1 ldi r24,lo8(16)
4064 2634 90E0 ldi r25,hi8(16)
4065 2636 9093 0000 sts (DebugOut+34)+1,r25
4066 263a 8093 0000 sts DebugOut+34,r24
4067 263e 81E1 ldi r24,lo8(17)
4068 2640 90E0 ldi r25,hi8(17)
4069 2642 9093 0000 sts (DebugOut+36)+1,r25
4070 2646 8093 0000 sts DebugOut+36,r24
4071 264a 82E1 ldi r24,lo8(18)
4072 264c 90E0 ldi r25,hi8(18)
4073 264e 9093 0000 sts (DebugOut+38)+1,r25
4074 2652 8093 0000 sts DebugOut+38,r24
4075 2656 83E1 ldi r24,lo8(19)
4076 2658 90E0 ldi r25,hi8(19)
4077 265a 9093 0000 sts (DebugOut+40)+1,r25
4078 265e 8093 0000 sts DebugOut+40,r24
4079 2662 4091 0000 lds r20,vibrationOffsetNick
4080 2666 5091 0000 lds r21,(vibrationOffsetNick)+1
4081 266a 8091 0000 lds r24,savedVibrationOffsetNick
4082 266e 9091 0000 lds r25,(savedVibrationOffsetNick)+1
4083 2672 FA01 movw r30,r20
4084 2674 E81B sub r30,r24
4085 2676 F90B sbc r31,r25
4086 2678 F093 0000 sts (DebugOut+42)+1,r31
4087 267c E093 0000 sts DebugOut+42,r30
4088 2680 2091 0000 lds r18,vibrationOffsetRoll
4089 2684 3091 0000 lds r19,(vibrationOffsetRoll)+1
4090 2688 8091 0000 lds r24,savedVibrationOffsetRoll
4091 268c 9091 0000 lds r25,(savedVibrationOffsetRoll)+1
4092 2690 B901 movw r22,r18
4093 2692 681B sub r22,r24
4094 2694 790B sbc r23,r25
4095 2696 7093 0000 sts (DebugOut+44)+1,r23
4096 269a 6093 0000 sts DebugOut+44,r22
4097 269e 86E1 ldi r24,lo8(22)
4098 26a0 90E0 ldi r25,hi8(22)
4099 26a2 9093 0000 sts (DebugOut+46)+1,r25
4100 26a6 8093 0000 sts DebugOut+46,r24
4101 26aa 5092 0000 sts (DebugOut+48)+1,r5
4102 26ae 4092 0000 sts DebugOut+48,r4
4103 26b2 9092 0000 sts (DebugOut+50)+1,r9
4104 26b6 8092 0000 sts DebugOut+50,r8
4105 26ba 5093 0000 sts (DebugOut+52)+1,r21
4106 26be 4093 0000 sts DebugOut+52,r20
4107 26c2 3093 0000 sts (DebugOut+54)+1,r19
4108 26c6 2093 0000 sts DebugOut+54,r18
4109 26ca 8BE1 ldi r24,lo8(27)
4110 26cc 90E0 ldi r25,hi8(27)
4111 26ce 9093 0000 sts (DebugOut+56)+1,r25
4112 26d2 8093 0000 sts DebugOut+56,r24
4113 26d6 A090 0000 lds r10,StickNick
4114 26da B090 0000 lds r11,(StickNick)+1
4115 26de B092 0000 sts (DebugOut+58)+1,r11
4116 26e2 A092 0000 sts DebugOut+58,r10
4117 26e6 C090 0000 lds r12,StickRoll
4118 26ea D090 0000 lds r13,(StickRoll)+1
4119 26ee D092 0000 sts (DebugOut+60)+1,r13
4120 26f2 C092 0000 sts DebugOut+60,r12
4121 26f6 8091 0000 lds r24,debugNickNoisePeak
4122 26fa 9091 0000 lds r25,(debugNickNoisePeak)+1
4123 26fe 9093 0000 sts (DebugOut+62)+1,r25
4124 2702 8093 0000 sts DebugOut+62,r24
4125 2706 8091 0000 lds r24,debugRollNoisePeak
4126 270a 9091 0000 lds r25,(debugRollNoisePeak)+1
4127 270e 9093 0000 sts (DebugOut+64)+1,r25
4128 2712 8093 0000 sts DebugOut+64,r24
4129 2716 0C94 0000 jmp .L422
4130 .L474:
4131 271a 8091 0000 lds r24,CompassCalState
4132 271e 8823 tst r24
4133 2720 01F0 breq .+2
4134 2722 00C0 rjmp .L531
4135 .L405:
4136 2724 6091 0000 lds r22,IntegralGyroNick
4137 2728 7091 0000 lds r23,(IntegralGyroNick)+1
4138 272c 8091 0000 lds r24,(IntegralGyroNick)+2
4139 2730 9091 0000 lds r25,(IntegralGyroNick)+3
4140 2734 20E0 ldi r18,lo8(512)
4141 2736 32E0 ldi r19,hi8(512)
4142 2738 40E0 ldi r20,hlo8(512)
4143 273a 50E0 ldi r21,hhi8(512)
4144 273c 0E94 0000 call __divmodsi4
4145 2740 8901 movw r16,r18
4146 2742 37FD sbrc r19,7
4147 2744 00C0 rjmp .L532
4148 .L406:
4149 2746 6091 0000 lds r22,IntegralGyroRoll
4150 274a 7091 0000 lds r23,(IntegralGyroRoll)+1
4151 274e 8091 0000 lds r24,(IntegralGyroRoll)+2
4152 2752 9091 0000 lds r25,(IntegralGyroRoll)+3
4153 2756 20E0 ldi r18,lo8(512)
4154 2758 32E0 ldi r19,hi8(512)
4155 275a 40E0 ldi r20,hlo8(512)
4156 275c 50E0 ldi r21,hhi8(512)
4157 275e 0E94 0000 call __divmodsi4
4158 2762 5901 movw r10,r18
4159 2764 37FD sbrc r19,7
4160 2766 00C0 rjmp .L533
4161 .L407:
4162 2768 A016 cp r10,r16
4163 276a B106 cpc r11,r17
4164 276c 04F4 brge .L408
4165 276e 5801 movw r10,r16
4166 .L408:
4167 2770 C090 0000 lds r12,CompassHeading
4168 2774 D090 0000 lds r13,(CompassHeading)+1
4169 2778 D7FC sbrc r13,7
4170 277a 00C0 rjmp .L534
4171 277c 4CE1 ldi r20,lo8(540)
4172 277e 52E0 ldi r21,hi8(540)
4173 2780 C40E add r12,r20
4174 2782 D51E adc r13,r21
4175 2784 7601 movw r14,r12
4176 2786 0027 clr r16
4177 2788 F7FC sbrc r15,7
4178 278a 0095 com r16
4179 278c 102F mov r17,r16
4180 278e 64EE ldi r22,lo8(-540)
4181 2790 7DEF ldi r23,hi8(-540)
4182 2792 C60E add r12,r22
4183 2794 D71E adc r13,r23
4184 2796 6091 0000 lds r22,YawGyroHeading
4185 279a 7091 0000 lds r23,(YawGyroHeading)+1
4186 279e 8091 0000 lds r24,(YawGyroHeading)+2
4187 27a2 9091 0000 lds r25,(YawGyroHeading)+3
4188 27a6 20E0 ldi r18,lo8(512)
4189 27a8 32E0 ldi r19,hi8(512)
4190 27aa 40E0 ldi r20,hlo8(512)
4191 27ac 50E0 ldi r21,hhi8(512)
4192 27ae 0E94 0000 call __divmodsi4
4193 27b2 E21A sub r14,r18
4194 27b4 F30A sbc r15,r19
4195 27b6 040B sbc r16,r20
4196 27b8 150B sbc r17,r21
4197 27ba C801 movw r24,r16
4198 27bc B701 movw r22,r14
4199 27be 28E6 ldi r18,lo8(360)
4200 27c0 31E0 ldi r19,hi8(360)
4201 27c2 40E0 ldi r20,hlo8(360)
4202 27c4 50E0 ldi r21,hhi8(360)
4203 27c6 0E94 0000 call __divmodsi4
4204 27ca 645B subi r22,lo8(-(-180))
4205 27cc 7040 sbci r23,hi8(-(-180))
4206 .L410:
4207 27ce 8091 0000 lds r24,GyroYaw
4208 27d2 9091 0000 lds r25,(GyroYaw)+1
4209 27d6 9C87 std Y+12,r25
4210 27d8 8B87 std Y+11,r24
4211 27da 9C01 movw r18,r24
4212 27dc 97FD sbrc r25,7
4213 27de 00C0 rjmp .L535
4214 .L412:
4215 27e0 2138 cpi r18,129
4216 27e2 3105 cpc r19,__zero_reg__
4217 27e4 04F4 brge .+2
4218 27e6 00C0 rjmp .L411
4219 27e8 EE24 clr r14
4220 27ea FF24 clr r15
4221 .L413:
4222 27ec 0091 0000 lds r16,BadCompassHeading
4223 27f0 1091 0000 lds r17,(BadCompassHeading)+1
4224 27f4 0115 cp r16,__zero_reg__
4225 27f6 1105 cpc r17,__zero_reg__
4226 27f8 01F4 brne .L414
4227 27fa 99E1 ldi r25,lo8(25)
4228 27fc A916 cp r10,r25
4229 27fe B104 cpc r11,__zero_reg__
4230 2800 04F4 brge .+2
4231 2802 00C0 rjmp .L536
4232 .L414:
4233 2804 B501 movw r22,r10
4234 2806 7595 asr r23
4235 2808 6795 ror r22
4236 280a 7595 asr r23
4237 280c 6795 ror r22
4238 280e 7595 asr r23
4239 2810 6795 ror r22
4240 2812 6F5F subi r22,lo8(-(1))
4241 2814 7F4F sbci r23,hi8(-(1))
4242 2816 C701 movw r24,r14
4243 2818 0E94 0000 call __divmodhi4
4244 281c 9B01 movw r18,r22
4245 281e 4427 clr r20
4246 2820 37FD sbrc r19,7
4247 2822 4095 com r20
4248 2824 542F mov r21,r20
4249 2826 8091 0000 lds r24,YawGyroHeading
4250 282a 9091 0000 lds r25,(YawGyroHeading)+1
4251 282e A091 0000 lds r26,(YawGyroHeading)+2
4252 2832 B091 0000 lds r27,(YawGyroHeading)+3
4253 2836 280F add r18,r24
4254 2838 391F adc r19,r25
4255 283a 4A1F adc r20,r26
4256 283c 5B1F adc r21,r27
4257 283e 2093 0000 sts YawGyroHeading,r18
4258 2842 3093 0000 sts (YawGyroHeading)+1,r19
4259 2846 4093 0000 sts (YawGyroHeading)+2,r20
4260 284a 5093 0000 sts (YawGyroHeading)+3,r21
4261 284e 8091 0000 lds r24,FCParam+4
4262 2852 882E mov r8,r24
4263 2854 9924 clr r9
4264 2856 A89C mul r10,r8
4265 2858 C001 movw r24,r0
4266 285a A99C mul r10,r9
4267 285c 900D add r25,r0
4268 285e B89C mul r11,r8
4269 2860 900D add r25,r0
4270 2862 1124 clr r1
4271 2864 97FD sbrc r25,7
4272 2866 00C0 rjmp .L537
4273 .L415:
4274 2868 9595 asr r25
4275 286a 8795 ror r24
4276 286c 9595 asr r25
4277 286e 8795 ror r24
4278 2870 9595 asr r25
4279 2872 8795 ror r24
4280 2874 9595 asr r25
4281 2876 8795 ror r24
4282 2878 9595 asr r25
4283 287a 8795 ror r24
4284 287c 6401 movw r12,r8
4285 287e C81A sub r12,r24
4286 2880 D90A sbc r13,r25
4287 2882 D7FC sbrc r13,7
4288 2884 00C0 rjmp .L416
4289 2886 0115 cp r16,__zero_reg__
4290 2888 1105 cpc r17,__zero_reg__
4291 288a 01F0 breq .+2
4292 288c 00C0 rjmp .L417
4293 288e A090 0000 lds r10,MaxStickRoll
4294 2892 B090 0000 lds r11,(MaxStickRoll)+1
4295 2896 8091 0000 lds r24,MaxStickNick
4296 289a 9091 0000 lds r25,(MaxStickNick)+1
4297 289e A80E add r10,r24
4298 28a0 B91E adc r11,r25
4299 28a2 B7FC sbrc r11,7
4300 28a4 00C0 rjmp .L538
4301 .L418:
4302 28a6 B594 asr r11
4303 28a8 A794 ror r10
4304 28aa B594 asr r11
4305 28ac A794 ror r10
4306 28ae B594 asr r11
4307 28b0 A794 ror r10
4308 28b2 60E4 ldi r22,lo8(64)
4309 28b4 70E0 ldi r23,hi8(64)
4310 28b6 A60E add r10,r22
4311 28b8 B71E adc r11,r23
4312 28ba 8091 0000 lds r24,CompassCourse
4313 28be 9091 0000 lds r25,(CompassCourse)+1
4314 28c2 AA27 clr r26
4315 28c4 97FD sbrc r25,7
4316 28c6 A095 com r26
4317 28c8 BA2F mov r27,r26
4318 28ca 1CE1 ldi r17,lo8(540)
4319 28cc E12E mov r14,r17
4320 28ce 12E0 ldi r17,hi8(540)
4321 28d0 F12E mov r15,r17
4322 28d2 012D mov r16,__zero_reg__
4323 28d4 112D mov r17,__zero_reg__
4324 28d6 E81A sub r14,r24
4325 28d8 F90A sbc r15,r25
4326 28da 0A0B sbc r16,r26
4327 28dc 1B0B sbc r17,r27
4328 28de CA01 movw r24,r20
4329 28e0 B901 movw r22,r18
4330 28e2 20E0 ldi r18,lo8(512)
4331 28e4 32E0 ldi r19,hi8(512)
4332 28e6 40E0 ldi r20,hlo8(512)
4333 28e8 50E0 ldi r21,hhi8(512)
4334 28ea 0E94 0000 call __divmodsi4
4335 28ee E20E add r14,r18
4336 28f0 F31E adc r15,r19
4337 28f2 041F adc r16,r20
4338 28f4 151F adc r17,r21
4339 28f6 C801 movw r24,r16
4340 28f8 B701 movw r22,r14
4341 28fa 28E6 ldi r18,lo8(360)
4342 28fc 31E0 ldi r19,hi8(360)
4343 28fe 40E0 ldi r20,hlo8(360)
4344 2900 50E0 ldi r21,hhi8(360)
4345 2902 0E94 0000 call __divmodsi4
4346 2906 645B subi r22,lo8(-(-180))
4347 2908 7040 sbci r23,hi8(-(-180))
4348 290a 6C9D mul r22,r12
4349 290c C001 movw r24,r0
4350 290e 6D9D mul r22,r13
4351 2910 900D add r25,r0
4352 2912 7C9D mul r23,r12
4353 2914 900D add r25,r0
4354 2916 1124 clr r1
4355 2918 B501 movw r22,r10
4356 291a 0E94 0000 call __divmodhi4
4357 291e C401 movw r24,r8
4358 2920 880F lsl r24
4359 2922 991F rol r25
4360 2924 880D add r24,r8
4361 2926 991D adc r25,r9
4362 2928 8617 cp r24,r22
4363 292a 9707 cpc r25,r23
4364 292c 04F0 brlt .+2
4365 292e 00C0 rjmp .L419
4366 2930 BC01 movw r22,r24
4367 .L420:
4368 2932 9B01 movw r18,r22
4369 2934 4427 clr r20
4370 2936 37FD sbrc r19,7
4371 2938 4095 com r20
4372 293a 542F mov r21,r20
4373 293c 8091 0000 lds r24,ReadingIntegralGyroYaw
4374 2940 9091 0000 lds r25,(ReadingIntegralGyroYaw)+1
4375 2944 A091 0000 lds r26,(ReadingIntegralGyroYaw)+2
4376 2948 B091 0000 lds r27,(ReadingIntegralGyroYaw)+3
4377 294c 820F add r24,r18
4378 294e 931F adc r25,r19
4379 2950 A41F adc r26,r20
4380 2952 B51F adc r27,r21
4381 2954 8093 0000 sts ReadingIntegralGyroYaw,r24
4382 2958 9093 0000 sts (ReadingIntegralGyroYaw)+1,r25
4383 295c A093 0000 sts (ReadingIntegralGyroYaw)+2,r26
4384 2960 B093 0000 sts (ReadingIntegralGyroYaw)+3,r27
4385 2964 0C94 0000 jmp .L404
4386 .L402:
4387 2968 2155 subi r18,lo8(50001)
4388 296a 334C sbci r19,hi8(50001)
4389 296c 4040 sbci r20,hlo8(50001)
4390 296e 5040 sbci r21,hhi8(50001)
4391 2970 04F4 brge .+4
4392 2972 0C94 0000 jmp .L403
4393 2976 80E5 ldi r24,lo8(50000)
4394 2978 93EC ldi r25,hi8(50000)
4395 297a A0E0 ldi r26,hlo8(50000)
4396 297c B0E0 ldi r27,hhi8(50000)
4397 297e 8093 0000 sts ReadingIntegralGyroYaw,r24
4398 2982 9093 0000 sts (ReadingIntegralGyroYaw)+1,r25
4399 2986 A093 0000 sts (ReadingIntegralGyroYaw)+2,r26
4400 298a B093 0000 sts (ReadingIntegralGyroYaw)+3,r27
4401 298e 0C94 0000 jmp .L403
4402 .L279:
4403 2992 2998 cbi 37-0x20,1
4404 2994 0C94 0000 jmp .L280
4405 .L529:
4406 2998 9887 std Y+8,r25
4407 299a 8F83 std Y+7,r24
4408 299c 81E0 ldi r24,lo8(1)
4409 299e 8093 0000 sts FunnelCourse,r24
4410 29a2 0C94 0000 jmp .L387
4411 .L515:
4412 29a6 9B01 movw r18,r22
4413 29a8 77FD sbrc r23,7
4414 29aa 00C0 rjmp .L539
4415 .L366:
4416 29ac C901 movw r24,r18
4417 29ae 9595 asr r25
4418 29b0 8795 ror r24
4419 29b2 9595 asr r25
4420 29b4 8795 ror r24
4421 29b6 9093 0000 sts (MaxStickNick)+1,r25
4422 29ba 8093 0000 sts MaxStickNick,r24
4423 29be 8536 cpi r24,101
4424 29c0 9105 cpc r25,__zero_reg__
4425 29c2 04F4 brge .+2
4426 29c4 00C0 rjmp .L367
4427 29c6 84E6 ldi r24,lo8(100)
4428 29c8 90E0 ldi r25,hi8(100)
4429 29ca 9093 0000 sts (MaxStickNick)+1,r25
4430 29ce 8093 0000 sts MaxStickNick,r24
4431 29d2 00C0 rjmp .L367
4432 .L512:
4433 29d4 1092 0000 sts GyroIFactor,__zero_reg__
4434 29d8 00C0 rjmp .L362
4435 .L530:
4436 29da 80E5 ldi r24,lo8(80)
4437 29dc 90E0 ldi r25,hi8(80)
4438 29de 9C01 movw r18,r24
4439 29e0 5FEF ldi r21,hi8(-80)
4440 29e2 803B cpi r24,lo8(-80)
4441 29e4 9507 cpc r25,r21
4442 29e6 04F0 brlt .+4
4443 29e8 0C94 0000 jmp .L439
4444 29ec 00C0 rjmp .L540
4445 .L494:
4446 29ee C601 movw r24,r12
4447 29f0 0C94 0000 jmp .L458
4448 .L492:
4449 29f4 3C01 movw r6,r24
4450 29f6 0C94 0000 jmp .L443
4451 .L531:
4452 29fa 8091 0000 lds r24,MKFlags
4453 29fe 80FD sbrc r24,0
4454 2a00 00C0 rjmp .L405
4455 2a02 0E94 0000 call SetCompassCalState
4456 2a06 3090 0000 lds r3,LoopingNick
4457 2a0a 2091 0000 lds r18,GyroYaw
4458 2a0e 3091 0000 lds r19,(GyroYaw)+1
4459 2a12 3C87 std Y+12,r19
4460 2a14 2B87 std Y+11,r18
4461 2a16 0C94 0000 jmp .L404
4462 .L482:
4463 2a1a 0197 sbiw r24,1
4464 2a1c 9093 0000 sts (RcLostTimer.1981)+1,r25
4465 2a20 8093 0000 sts RcLostTimer.1981,r24
4466 2a24 8AE0 ldi r24,lo8(10)
4467 2a26 D816 cp r13,r24
4468 2a28 01F0 breq .+4
4469 2a2a 0C94 0000 jmp .L541
4470 .L286:
4471 2a2e 289A sbi 37-0x20,0
4472 2a30 0C94 0000 jmp .L288
4473 .L491:
4474 2a34 9201 movw r18,r4
4475 2a36 3595 asr r19
4476 2a38 2795 ror r18
4477 2a3a E216 cp r14,r18
4478 2a3c F306 cpc r15,r19
4479 2a3e 04F0 brlt .+4
4480 2a40 0C94 0000 jmp .L439
4481 2a44 9701 movw r18,r14
4482 2a46 0C94 0000 jmp .L439
4483 .L489:
4484 2a4a 8F81 ldd r24,Y+7
4485 2a4c 9885 ldd r25,Y+8
4486 2a4e 892B or r24,r25
4487 2a50 01F4 brne .+4
4488 2a52 0C94 0000 jmp .L433
4489 2a56 81E0 ldi r24,lo8(1)
4490 2a58 90E0 ldi r25,hi8(1)
4491 2a5a 9093 0000 sts (ModelIsFlying)+1,r25
4492 2a5e 8093 0000 sts ModelIsFlying,r24
4493 2a62 0C94 0000 jmp .L433
4494 .L487:
4495 2a66 81E0 ldi r24,lo8(1)
4496 2a68 90E0 ldi r25,hi8(1)
4497 2a6a 9093 0000 sts (UpdateCompassCourse.1986)+1,r25
4498 2a6e 8093 0000 sts UpdateCompassCourse.1986,r24
4499 2a72 0C94 0000 jmp .L399
4500 .L485:
4501 2a76 8091 0000 lds r24,FCParam+15
4502 2a7a 8823 tst r24
4503 2a7c 01F0 breq .+4
4504 2a7e 0C94 0000 jmp .L396
4505 2a82 8091 0000 lds r24,filteredHiResRollGyro
4506 2a86 9091 0000 lds r25,(filteredHiResRollGyro)+1
4507 2a8a AA27 clr r26
4508 2a8c 97FD sbrc r25,7
4509 2a8e A095 com r26
4510 2a90 BA2F mov r27,r26
4511 2a92 E816 cp r14,r24
4512 2a94 F906 cpc r15,r25
4513 2a96 0A07 cpc r16,r26
4514 2a98 1B07 cpc r17,r27
4515 2a9a 04F0 brlt .+2
4516 2a9c 00C0 rjmp .L398
4517 2a9e 2091 0000 lds r18,IntegralGyroRoll
4518 2aa2 3091 0000 lds r19,(IntegralGyroRoll)+1
4519 2aa6 4091 0000 lds r20,(IntegralGyroRoll)+2
4520 2aaa 5091 0000 lds r21,(IntegralGyroRoll)+3
4521 2aae 1216 cp __zero_reg__,r18
4522 2ab0 1306 cpc __zero_reg__,r19
4523 2ab2 1406 cpc __zero_reg__,r20
4524 2ab4 1506 cpc __zero_reg__,r21
4525 2ab6 04F0 brlt .+2
4526 2ab8 00C0 rjmp .L398
4527 2aba C090 0000 lds r12,StickRoll
4528 2abe D090 0000 lds r13,(StickRoll)+1
4529 2ac2 C814 cp r12,r8
4530 2ac4 D904 cpc r13,r9
4531 2ac6 04F4 brge .+2
4532 2ac8 00C0 rjmp .L398
4533 2aca 6C14 cp r6,r12
4534 2acc 7D04 cpc r7,r13
4535 2ace 04F4 brge .+2
4536 2ad0 00C0 rjmp .L398
4537 2ad2 8091 0000 lds r24,FCParam+17
4538 2ad6 8058 subi r24,lo8(-(-128))
4539 2ad8 9927 clr r25
4540 2ada 87FD sbrc r24,7
4541 2adc 9095 com r25
4542 2ade A92F mov r26,r25
4543 2ae0 B92F mov r27,r25
4544 2ae2 8E0D add r24,r14
4545 2ae4 9F1D adc r25,r15
4546 2ae6 A01F adc r26,r16
4547 2ae8 B11F adc r27,r17
4548 2aea 8093 0000 sts vibrationOffsetRoll,r24
4549 2aee 9093 0000 sts (vibrationOffsetRoll)+1,r25
4550 2af2 A093 0000 sts (vibrationOffsetRoll)+2,r26
4551 2af6 B093 0000 sts (vibrationOffsetRoll)+3,r27
4552 .L480:
4553 2afa 6091 0000 lds r22,FCParam+14
4554 2afe 70E0 ldi r23,lo8(0)
4555 2b00 80E0 ldi r24,lo8(0)
4556 2b02 90E0 ldi r25,hi8(0)
4557 2b04 0E94 0000 call __mulsi3
4558 2b08 24E6 ldi r18,lo8(100)
4559 2b0a 30E0 ldi r19,hi8(100)
4560 2b0c 40E0 ldi r20,hlo8(100)
4561 2b0e 50E0 ldi r21,hhi8(100)
4562 2b10 0E94 0000 call __divmodsi4
4563 2b14 2093 0000 sts IntegralGyroRoll,r18
4564 2b18 3093 0000 sts (IntegralGyroRoll)+1,r19
4565 2b1c 4093 0000 sts (IntegralGyroRoll)+2,r20
4566 2b20 5093 0000 sts (IntegralGyroRoll)+3,r21
4567 2b24 0C94 0000 jmp .L396
4568 .L368:
4569 2b28 4150 subi r20,lo8(-(-1))
4570 2b2a 5040 sbci r21,hi8(-(-1))
4571 2b2c 5093 0000 sts (MaxStickRoll)+1,r21
4572 2b30 4093 0000 sts MaxStickRoll,r20
4573 2b34 00C0 rjmp .L372
4574 .L348:
4575 2b36 1092 0000 sts (delay_startmotors.1983)+1,__zero_reg__
4576 2b3a 1092 0000 sts delay_startmotors.1983,__zero_reg__
4577 .L350:
4578 2b3e E091 0000 lds r30,ParamSet+3
4579 2b42 F0E0 ldi r31,lo8(0)
4580 2b44 EE0F lsl r30
4581 2b46 FF1F rol r31
4582 2b48 E050 subi r30,lo8(-(PPM_in))
4583 2b4a F040 sbci r31,hi8(-(PPM_in))
4584 2b4c 8081 ld r24,Z
4585 2b4e 9181 ldd r25,Z+1
4586 2b50 8C34 cpi r24,76
4587 2b52 9105 cpc r25,__zero_reg__
4588 2b54 04F4 brge .+2
4589 2b56 00C0 rjmp .L354
4590 2b58 8091 0000 lds r24,delay_stopmotors.1984
4591 2b5c 9091 0000 lds r25,(delay_stopmotors.1984)+1
4592 2b60 0196 adiw r24,1
4593 2b62 9093 0000 sts (delay_stopmotors.1984)+1,r25
4594 2b66 8093 0000 sts delay_stopmotors.1984,r24
4595 2b6a 893C cpi r24,201
4596 2b6c 9105 cpc r25,__zero_reg__
4597 2b6e 00F4 brsh .+4
4598 2b70 0C94 0000 jmp .L290
4599 2b74 88EC ldi r24,lo8(200)
4600 2b76 90E0 ldi r25,hi8(200)
4601 2b78 9093 0000 sts (delay_stopmotors.1984)+1,r25
4602 2b7c 8093 0000 sts delay_stopmotors.1984,r24
4603 2b80 1092 0000 sts (ModelIsFlying)+1,__zero_reg__
4604 2b84 1092 0000 sts ModelIsFlying,__zero_reg__
4605 .L478:
4606 2b88 8091 0000 lds r24,MKFlags
4607 2b8c 8E7F andi r24,lo8(-2)
4608 2b8e 8093 0000 sts MKFlags,r24
4609 2b92 0C94 0000 jmp .L290
4610 .L411:
4611 2b96 7B01 movw r14,r22
4612 2b98 EE0C lsl r14
4613 2b9a FF1C rol r15
4614 2b9c EE0C lsl r14
4615 2b9e FF1C rol r15
4616 2ba0 EE0C lsl r14
4617 2ba2 FF1C rol r15
4618 2ba4 00C0 rjmp .L413
4619 .L486:
4620 2ba6 8827 clr r24
4621 2ba8 9927 clr r25
4622 2baa 841B sub r24,r20
4623 2bac 950B sbc r25,r21
4624 2bae 0C94 0000 jmp .L400
4625 .L493:
4626 2bb2 2227 clr r18
4627 2bb4 3327 clr r19
4628 2bb6 2619 sub r18,r6
4629 2bb8 3709 sbc r19,r7
4630 2bba 0C94 0000 jmp .L453
4631 .L488:
4632 2bbe 2D5F subi r18,lo8(-(3))
4633 2bc0 3F4F sbci r19,hi8(-(3))
4634 2bc2 0C94 0000 jmp .L401
4635 .L511:
4636 2bc6 1092 0000 sts (StickGas)+1,__zero_reg__
4637 2bca 1092 0000 sts StickGas,__zero_reg__
4638 2bce 8091 0000 lds r24,ParamSet+8
4639 2bd2 82FF sbrs r24,2
4640 2bd4 00C0 rjmp .L362
4641 2bd6 00C0 rjmp .L512
4642 .L510:
4643 2bd8 2250 subi r18,lo8(-(-2))
4644 2bda 3040 sbci r19,hi8(-(-2))
4645 2bdc 3093 0000 sts (StickYaw)+1,r19
4646 2be0 2093 0000 sts StickYaw,r18
4647 2be4 00C0 rjmp .L357
4648 .L519:
4649 2be6 84E6 ldi r24,lo8(100)
4650 2be8 90E0 ldi r25,hi8(100)
4651 2bea 9093 0000 sts (MaxStickRoll)+1,r25
4652 2bee 8093 0000 sts MaxStickRoll,r24
4653 2bf2 00C0 rjmp .L372
4654 .L385:
4655 2bf4 1092 0000 sts LoopingNick,__zero_reg__
4656 2bf8 0C94 0000 jmp .L356
4657 .L382:
4658 2bfc 1092 0000 sts LoopingRoll,__zero_reg__
4659 2c00 00C0 rjmp .L383
4660 .L526:
4661 2c02 81E0 ldi r24,lo8(1)
4662 2c04 8093 0000 sts LoopingDown,r24
4663 2c08 00C0 rjmp .L380
4664 .L524:
4665 2c0a 81E0 ldi r24,lo8(1)
4666 2c0c 8093 0000 sts LoopingTop,r24
4667 2c10 8091 0000 lds r24,ParamSet+40
4668 2c14 482F mov r20,r24
4669 2c16 50E0 ldi r21,lo8(0)
4670 2c18 61E0 ldi r22,lo8(1)
4671 2c1a 00C0 rjmp .L378
4672 .L522:
4673 2c1c 81E0 ldi r24,lo8(1)
4674 2c1e 8093 0000 sts LoopingRight,r24
4675 2c22 8091 0000 lds r24,ParamSet+40
4676 2c26 482F mov r20,r24
4677 2c28 50E0 ldi r21,lo8(0)
4678 2c2a 00C0 rjmp .L376
4679 .L520:
4680 2c2c 81E0 ldi r24,lo8(1)
4681 2c2e 8093 0000 sts LoopingLeft,r24
4682 2c32 71E0 ldi r23,lo8(1)
4683 2c34 00C0 rjmp .L374
4684 .L417:
4685 2c36 0150 subi r16,lo8(-(-1))
4686 2c38 1040 sbci r17,hi8(-(-1))
4687 2c3a 1093 0000 sts (BadCompassHeading)+1,r17
4688 2c3e 0093 0000 sts BadCompassHeading,r16
4689 2c42 0C94 0000 jmp .L404
4690 .L521:
4691 2c46 1092 0000 sts LoopingLeft,__zero_reg__
4692 2c4a 8091 0000 lds r24,ParamSet+1
4693 2c4e A82F mov r26,r24
4694 2c50 B0E0 ldi r27,lo8(0)
4695 2c52 8091 0000 lds r24,ParamSet+40
4696 2c56 482F mov r20,r24
4697 2c58 50E0 ldi r21,lo8(0)
4698 2c5a AA0F lsl r26
4699 2c5c BB1F rol r27
4700 2c5e 70E0 ldi r23,lo8(0)
4701 2c60 00C0 rjmp .L374
4702 .L525:
4703 2c62 1092 0000 sts LoopingTop,__zero_reg__
4704 2c66 8091 0000 lds r24,ParamSet
4705 2c6a A82F mov r26,r24
4706 2c6c B0E0 ldi r27,lo8(0)
4707 2c6e 8091 0000 lds r24,ParamSet+40
4708 2c72 482F mov r20,r24
4709 2c74 50E0 ldi r21,lo8(0)
4710 2c76 AA0F lsl r26
4711 2c78 BB1F rol r27
4712 2c7a 60E0 ldi r22,lo8(0)
4713 2c7c 00C0 rjmp .L378
4714 .L523:
4715 2c7e 1092 0000 sts LoopingRight,__zero_reg__
4716 2c82 8091 0000 lds r24,ParamSet+40
4717 2c86 482F mov r20,r24
4718 2c88 50E0 ldi r21,lo8(0)
4719 2c8a 00C0 rjmp .L376
4720 .L527:
4721 2c8c 1092 0000 sts LoopingDown,__zero_reg__
4722 2c90 00C0 rjmp .L380
4723 .L292:
4724 2c92 8091 0000 lds r24,MKFlags
4725 2c96 8260 ori r24,lo8(2)
4726 2c98 8093 0000 sts MKFlags,r24
4727 2c9c 0C94 0000 jmp .L293
4728 .L536:
4729 2ca0 8091 0000 lds r24,UpdateCompassCourse.1986
4730 2ca4 9091 0000 lds r25,(UpdateCompassCourse.1986)+1
4731 2ca8 892B or r24,r25
4732 2caa 01F4 brne .+2
4733 2cac 00C0 rjmp .L414
4734 2cae 88EC ldi r24,lo8(200)
4735 2cb0 90E0 ldi r25,hi8(200)
4736 2cb2 9093 0000 sts (BeepTime)+1,r25
4737 2cb6 8093 0000 sts BeepTime,r24
4738 2cba B601 movw r22,r12
4739 2cbc 8827 clr r24
4740 2cbe 77FD sbrc r23,7
4741 2cc0 8095 com r24
4742 2cc2 982F mov r25,r24
4743 2cc4 F9E0 ldi r31,9
4744 2cc6 660F 1: lsl r22
4745 2cc8 771F rol r23
4746 2cca 881F rol r24
4747 2ccc 991F rol r25
4748 2cce FA95 dec r31
4749 2cd0 01F4 brne 1b
4750 2cd2 6093 0000 sts YawGyroHeading,r22
4751 2cd6 7093 0000 sts (YawGyroHeading)+1,r23
4752 2cda 8093 0000 sts (YawGyroHeading)+2,r24
4753 2cde 9093 0000 sts (YawGyroHeading)+3,r25
4754 2ce2 20E0 ldi r18,lo8(512)
4755 2ce4 32E0 ldi r19,hi8(512)
4756 2ce6 40E0 ldi r20,hlo8(512)
4757 2ce8 50E0 ldi r21,hhi8(512)
4758 2cea 0E94 0000 call __divmodsi4
4759 2cee 3093 0000 sts (CompassCourse)+1,r19
4760 2cf2 2093 0000 sts CompassCourse,r18
4761 2cf6 1092 0000 sts (UpdateCompassCourse.1986)+1,__zero_reg__
4762 2cfa 1092 0000 sts UpdateCompassCourse.1986,__zero_reg__
4763 2cfe 00C0 rjmp .L414
4764 .L490:
4765 2d00 0196 adiw r24,1
4766 2d02 0C94 0000 jmp .L438
4767 .L484:
4768 2d06 9095 com r25
4769 2d08 8195 neg r24
4770 2d0a 9F4F sbci r25,lo8(-1)
4771 2d0c C816 cp r12,r24
4772 2d0e D906 cpc r13,r25
4773 2d10 04F4 brge .+4
4774 2d12 0C94 0000 jmp .L396
4775 2d16 00C0 rjmp .L485
4776 .L483:
4777 2d18 9095 com r25
4778 2d1a 8195 neg r24
4779 2d1c 9F4F sbci r25,lo8(-1)
4780 2d1e 8C15 cp r24,r12
4781 2d20 9D05 cpc r25,r13
4782 2d22 04F0 brlt .+4
4783 2d24 0C94 0000 jmp .L393
4784 2d28 0C94 0000 jmp .L542
4785 .L481:
4786 2d2c 88E9 ldi r24,lo8(15000)
4787 2d2e 9AE3 ldi r25,hi8(15000)
4788 2d30 9093 0000 sts (BeepTime)+1,r25
4789 2d34 8093 0000 sts BeepTime,r24
4790 2d38 80E0 ldi r24,lo8(3072)
4791 2d3a 9CE0 ldi r25,hi8(3072)
4792 2d3c 9093 0000 sts (BeepModulation)+1,r25
4793 2d40 8093 0000 sts BeepModulation,r24
4794 2d44 0C94 0000 jmp .L283
4795 .L398:
4796 2d48 8091 0000 lds r24,filteredHiResRollGyro
4797 2d4c 9091 0000 lds r25,(filteredHiResRollGyro)+1
4798 2d50 AA27 clr r26
4799 2d52 97FD sbrc r25,7
4800 2d54 A095 com r26
4801 2d56 BA2F mov r27,r26
4802 2d58 8E15 cp r24,r14
4803 2d5a 9F05 cpc r25,r15
4804 2d5c A007 cpc r26,r16
4805 2d5e B107 cpc r27,r17
4806 2d60 04F0 brlt .+4
4807 2d62 0C94 0000 jmp .L396
4808 2d66 2091 0000 lds r18,IntegralGyroRoll
4809 2d6a 3091 0000 lds r19,(IntegralGyroRoll)+1
4810 2d6e 4091 0000 lds r20,(IntegralGyroRoll)+2
4811 2d72 5091 0000 lds r21,(IntegralGyroRoll)+3
4812 2d76 57FD sbrc r21,7
4813 2d78 00C0 rjmp .+4
4814 2d7a 0C94 0000 jmp .L396
4815 2d7e C090 0000 lds r12,StickRoll
4816 2d82 D090 0000 lds r13,(StickRoll)+1
4817 2d86 8827 clr r24
4818 2d88 9927 clr r25
4819 2d8a 8819 sub r24,r8
4820 2d8c 9909 sbc r25,r9
4821 2d8e 8C15 cp r24,r12
4822 2d90 9D05 cpc r25,r13
4823 2d92 04F4 brge .+4
4824 2d94 0C94 0000 jmp .L396
4825 2d98 8827 clr r24
4826 2d9a 9927 clr r25
4827 2d9c 8619 sub r24,r6
4828 2d9e 9709 sbc r25,r7
4829 2da0 C816 cp r12,r24
4830 2da2 D906 cpc r13,r25
4831 2da4 04F4 brge .+4
4832 2da6 0C94 0000 jmp .L396
4833 2daa 8091 0000 lds r24,FCParam+17
4834 2dae 8058 subi r24,lo8(-(-128))
4835 2db0 9927 clr r25
4836 2db2 87FD sbrc r24,7
4837 2db4 9095 com r25
4838 2db6 A92F mov r26,r25
4839 2db8 B92F mov r27,r25
4840 2dba E81A sub r14,r24
4841 2dbc F90A sbc r15,r25
4842 2dbe 0A0B sbc r16,r26
4843 2dc0 1B0B sbc r17,r27
4844 2dc2 E092 0000 sts vibrationOffsetRoll,r14
4845 2dc6 F092 0000 sts (vibrationOffsetRoll)+1,r15
4846 2dca 0093 0000 sts (vibrationOffsetRoll)+2,r16
4847 2dce 1093 0000 sts (vibrationOffsetRoll)+3,r17
4848 2dd2 00C0 rjmp .L480
4849 .L395:
4850 2dd4 8091 0000 lds r24,filteredHiResPitchGyro
4851 2dd8 9091 0000 lds r25,(filteredHiResPitchGyro)+1
4852 2ddc AA27 clr r26
4853 2dde 97FD sbrc r25,7
4854 2de0 A095 com r26
4855 2de2 BA2F mov r27,r26
4856 2de4 8E15 cp r24,r14
4857 2de6 9F05 cpc r25,r15
4858 2de8 A007 cpc r26,r16
4859 2dea B107 cpc r27,r17
4860 2dec 04F0 brlt .+4
4861 2dee 0C94 0000 jmp .L393
4862 2df2 2091 0000 lds r18,IntegralGyroNick
4863 2df6 3091 0000 lds r19,(IntegralGyroNick)+1
4864 2dfa 4091 0000 lds r20,(IntegralGyroNick)+2
4865 2dfe 5091 0000 lds r21,(IntegralGyroNick)+3
4866 2e02 57FD sbrc r21,7
4867 2e04 00C0 rjmp .+4
4868 2e06 0C94 0000 jmp .L393
4869 2e0a A090 0000 lds r10,StickNick
4870 2e0e B090 0000 lds r11,(StickNick)+1
4871 2e12 8827 clr r24
4872 2e14 9927 clr r25
4873 2e16 8819 sub r24,r8
4874 2e18 9909 sbc r25,r9
4875 2e1a 8A15 cp r24,r10
4876 2e1c 9B05 cpc r25,r11
4877 2e1e 04F4 brge .+4
4878 2e20 0C94 0000 jmp .L393
4879 2e24 8827 clr r24
4880 2e26 9927 clr r25
4881 2e28 8619 sub r24,r6
4882 2e2a 9709 sbc r25,r7
4883 2e2c A816 cp r10,r24
4884 2e2e B906 cpc r11,r25
4885 2e30 04F4 brge .+4
4886 2e32 0C94 0000 jmp .L393
4887 2e36 8091 0000 lds r24,FCParam+16
4888 2e3a 8058 subi r24,lo8(-(-128))
4889 2e3c 9927 clr r25
4890 2e3e 87FD sbrc r24,7
4891 2e40 9095 com r25
4892 2e42 A92F mov r26,r25
4893 2e44 B92F mov r27,r25
4894 2e46 E81A sub r14,r24
4895 2e48 F90A sbc r15,r25
4896 2e4a 0A0B sbc r16,r26
4897 2e4c 1B0B sbc r17,r27
4898 2e4e E092 0000 sts vibrationOffsetNick,r14
4899 2e52 F092 0000 sts (vibrationOffsetNick)+1,r15
4900 2e56 0093 0000 sts (vibrationOffsetNick)+2,r16
4901 2e5a 1093 0000 sts (vibrationOffsetNick)+3,r17
4902 2e5e 0C94 0000 jmp .L479
4903 .L517:
4904 2e62 3095 com r19
4905 2e64 2195 neg r18
4906 2e66 3F4F sbci r19,lo8(-1)
4907 2e68 00C0 rjmp .L370
4908 .L516:
4909 2e6a 2D5F subi r18,lo8(-(3))
4910 2e6c 3F4F sbci r19,hi8(-(3))
4911 2e6e 00C0 rjmp .L369
4912 .L513:
4913 2e70 2D5F subi r18,lo8(-(3))
4914 2e72 3F4F sbci r19,hi8(-(3))
4915 2e74 3595 asr r19
4916 2e76 2795 ror r18
4917 2e78 3595 asr r19
4918 2e7a 2795 ror r18
4919 2e7c 37FF sbrs r19,7
4920 2e7e 00C0 rjmp .L365
4921 .L514:
4922 2e80 3095 com r19
4923 2e82 2195 neg r18
4924 2e84 3F4F sbci r19,lo8(-1)
4925 2e86 00C0 rjmp .L365
4926 .L534:
4927 2e88 60E0 ldi r22,lo8(0)
4928 2e8a 70E0 ldi r23,hi8(0)
4929 2e8c 00C0 rjmp .L410
4930 .L416:
4931 2e8e 84EF ldi r24,lo8(500)
4932 2e90 91E0 ldi r25,hi8(500)
4933 2e92 9093 0000 sts (BadCompassHeading)+1,r25
4934 2e96 8093 0000 sts BadCompassHeading,r24
4935 2e9a 0C94 0000 jmp .L404
4936 .L298:
4937 2e9e 8081 ld r24,Z
4938 2ea0 9181 ldd r25,Z+1
4939 2ea2 8259 subi r24,lo8(-(110))
4940 2ea4 9F4F sbci r25,hi8(-(110))
4941 2ea6 8A15 cp r24,r10
4942 2ea8 9B05 cpc r25,r11
4943 2eaa 04F0 brlt .+4
4944 2eac 0C94 0000 jmp .L299
4945 2eb0 A114 cp r10,__zero_reg__
4946 2eb2 B104 cpc r11,__zero_reg__
4947 2eb4 01F4 brne .+4
4948 2eb6 0C94 0000 jmp .L299
4949 2eba C501 movw r24,r10
4950 2ebc 0197 sbiw r24,1
4951 2ebe 9093 0000 sts (Poti3)+1,r25
4952 2ec2 8093 0000 sts Poti3,r24
4953 2ec6 5C01 movw r10,r24
4954 2ec8 0C94 0000 jmp .L299
4955 .L296:
4956 2ecc 8081 ld r24,Z
4957 2ece 9181 ldd r25,Z+1
4958 2ed0 8259 subi r24,lo8(-(110))
4959 2ed2 9F4F sbci r25,hi8(-(110))
4960 2ed4 8017 cp r24,r16
4961 2ed6 9107 cpc r25,r17
4962 2ed8 04F0 brlt .+4
4963 2eda 0C94 0000 jmp .L297
4964 2ede 0115 cp r16,__zero_reg__
4965 2ee0 1105 cpc r17,__zero_reg__
4966 2ee2 01F4 brne .+4
4967 2ee4 0C94 0000 jmp .L297
4968 2ee8 C801 movw r24,r16
4969 2eea 0197 sbiw r24,1
4970 2eec 9093 0000 sts (Poti2)+1,r25
4971 2ef0 8093 0000 sts Poti2,r24
4972 2ef4 8C01 movw r16,r24
4973 2ef6 0C94 0000 jmp .L297
4974 .L294:
4975 2efa 8081 ld r24,Z
4976 2efc 9181 ldd r25,Z+1
4977 2efe 8259 subi r24,lo8(-(110))
4978 2f00 9F4F sbci r25,hi8(-(110))
4979 2f02 8E15 cp r24,r14
4980 2f04 9F05 cpc r25,r15
4981 2f06 04F0 brlt .+4
4982 2f08 0C94 0000 jmp .L295
4983 2f0c E114 cp r14,__zero_reg__
4984 2f0e F104 cpc r15,__zero_reg__
4985 2f10 01F4 brne .+4
4986 2f12 0C94 0000 jmp .L295
4987 2f16 C701 movw r24,r14
4988 2f18 0197 sbiw r24,1
4989 2f1a 9093 0000 sts (Poti1)+1,r25
4990 2f1e 8093 0000 sts Poti1,r24
4991 2f22 7C01 movw r14,r24
4992 2f24 0C94 0000 jmp .L295
4993 .L308:
4994 2f28 8091 0000 lds r24,PPM_in+24
4995 2f2c 9091 0000 lds r25,(PPM_in+24)+1
4996 2f30 8259 subi r24,lo8(-(110))
4997 2f32 9F4F sbci r25,hi8(-(110))
4998 2f34 8217 cp r24,r18
4999 2f36 9307 cpc r25,r19
5000 2f38 04F0 brlt .+4
5001 2f3a 0C94 0000 jmp .L309
5002 2f3e 2115 cp r18,__zero_reg__
5003 2f40 3105 cpc r19,__zero_reg__
5004 2f42 01F4 brne .+4
5005 2f44 0C94 0000 jmp .L309
5006 2f48 C901 movw r24,r18
5007 2f4a 0197 sbiw r24,1
5008 2f4c 9093 0000 sts (Poti8)+1,r25
5009 2f50 8093 0000 sts Poti8,r24
5010 2f54 9C01 movw r18,r24
5011 2f56 0C94 0000 jmp .L309
5012 .L306:
5013 2f5a 8091 0000 lds r24,PPM_in+22
5014 2f5e 9091 0000 lds r25,(PPM_in+22)+1
5015 2f62 8259 subi r24,lo8(-(110))
5016 2f64 9F4F sbci r25,hi8(-(110))
5017 2f66 8417 cp r24,r20
5018 2f68 9507 cpc r25,r21
5019 2f6a 04F0 brlt .+4
5020 2f6c 0C94 0000 jmp .L307
5021 2f70 4115 cp r20,__zero_reg__
5022 2f72 5105 cpc r21,__zero_reg__
5023 2f74 01F4 brne .+4
5024 2f76 0C94 0000 jmp .L307
5025 2f7a CA01 movw r24,r20
5026 2f7c 0197 sbiw r24,1
5027 2f7e 9093 0000 sts (Poti7)+1,r25
5028 2f82 8093 0000 sts Poti7,r24
5029 2f86 AC01 movw r20,r24
5030 2f88 0C94 0000 jmp .L307
5031 .L304:
5032 2f8c 8091 0000 lds r24,PPM_in+20
5033 2f90 9091 0000 lds r25,(PPM_in+20)+1
5034 2f94 8259 subi r24,lo8(-(110))
5035 2f96 9F4F sbci r25,hi8(-(110))
5036 2f98 8617 cp r24,r22
5037 2f9a 9707 cpc r25,r23
5038 2f9c 04F0 brlt .+4
5039 2f9e 0C94 0000 jmp .L305
5040 2fa2 6115 cp r22,__zero_reg__
5041 2fa4 7105 cpc r23,__zero_reg__
5042 2fa6 01F4 brne .+4
5043 2fa8 0C94 0000 jmp .L305
5044 2fac CB01 movw r24,r22
5045 2fae 0197 sbiw r24,1
5046 2fb0 9093 0000 sts (Poti6)+1,r25
5047 2fb4 8093 0000 sts Poti6,r24
5048 2fb8 BC01 movw r22,r24
5049 2fba 0C94 0000 jmp .L305
5050 .L302:
5051 2fbe 8091 0000 lds r24,PPM_in+18
5052 2fc2 9091 0000 lds r25,(PPM_in+18)+1
5053 2fc6 8259 subi r24,lo8(-(110))
5054 2fc8 9F4F sbci r25,hi8(-(110))
5055 2fca 8E17 cp r24,r30
5056 2fcc 9F07 cpc r25,r31
5057 2fce 04F0 brlt .+4
5058 2fd0 0C94 0000 jmp .L303
5059 2fd4 3097 sbiw r30,0
5060 2fd6 01F4 brne .+4
5061 2fd8 0C94 0000 jmp .L303
5062 2fdc CF01 movw r24,r30
5063 2fde 0197 sbiw r24,1
5064 2fe0 9093 0000 sts (Poti5)+1,r25
5065 2fe4 8093 0000 sts Poti5,r24
5066 2fe8 FC01 movw r30,r24
5067 2fea 0C94 0000 jmp .L303
5068 .L300:
5069 2fee 8081 ld r24,Z
5070 2ff0 9181 ldd r25,Z+1
5071 2ff2 8259 subi r24,lo8(-(110))
5072 2ff4 9F4F sbci r25,hi8(-(110))
5073 2ff6 8A17 cp r24,r26
5074 2ff8 9B07 cpc r25,r27
5075 2ffa 04F0 brlt .+4
5076 2ffc 0C94 0000 jmp .L301
5077 3000 1097 sbiw r26,0
5078 3002 01F4 brne .+4
5079 3004 0C94 0000 jmp .L301
5080 3008 CD01 movw r24,r26
5081 300a 0197 sbiw r24,1
5082 300c 9093 0000 sts (Poti4)+1,r25
5083 3010 8093 0000 sts Poti4,r24
5084 3014 DC01 movw r26,r24
5085 3016 0C94 0000 jmp .L301
5086 .L537:
5087 301a 4F96 adiw r24,31
5088 301c 00C0 rjmp .L415
5089 .L533:
5090 301e B094 com r11
5091 3020 A194 neg r10
5092 3022 B108 sbc r11,__zero_reg__
5093 3024 B394 inc r11
5094 3026 00C0 rjmp .L407
5095 .L532:
5096 3028 1095 com r17
5097 302a 0195 neg r16
5098 302c 1F4F sbci r17,lo8(-1)
5099 302e 00C0 rjmp .L406
5100 .L535:
5101 3030 3095 com r19
5102 3032 2195 neg r18
5103 3034 3F4F sbci r19,lo8(-1)
5104 3036 00C0 rjmp .L412
5105 .L359:
5106 3038 1092 0000 sts (StickYaw)+1,__zero_reg__
5107 303c 1092 0000 sts StickYaw,__zero_reg__
5108 3040 00C0 rjmp .L357
5109 .L507:
5110 3042 1092 0000 sts (Poti8)+1,__zero_reg__
5111 3046 1092 0000 sts Poti8,__zero_reg__
5112 304a 0C94 0000 jmp .L325
5113 .L500:
5114 304e 1092 0000 sts (Poti1)+1,__zero_reg__
5115 3052 1092 0000 sts Poti1,__zero_reg__
5116 3056 0C94 0000 jmp .L311
5117 .L506:
5118 305a 1092 0000 sts (Poti7)+1,__zero_reg__
5119 305e 1092 0000 sts Poti7,__zero_reg__
5120 3062 0C94 0000 jmp .L323
5121 .L505:
5122 3066 1092 0000 sts (Poti6)+1,__zero_reg__
5123 306a 1092 0000 sts Poti6,__zero_reg__
5124 306e 0C94 0000 jmp .L321
5125 .L504:
5126 3072 1092 0000 sts (Poti5)+1,__zero_reg__
5127 3076 1092 0000 sts Poti5,__zero_reg__
5128 307a 0C94 0000 jmp .L319
5129 .L503:
5130 307e 1092 0000 sts (Poti4)+1,__zero_reg__
5131 3082 1092 0000 sts Poti4,__zero_reg__
5132 3086 0C94 0000 jmp .L317
5133 .L502:
5134 308a 1092 0000 sts (Poti3)+1,__zero_reg__
5135 308e 1092 0000 sts Poti3,__zero_reg__
5136 3092 0C94 0000 jmp .L315
5137 .L501:
5138 3096 1092 0000 sts (Poti2)+1,__zero_reg__
5139 309a 1092 0000 sts Poti2,__zero_reg__
5140 309e 0C94 0000 jmp .L313
5141 .L419:
5142 30a2 4427 clr r20
5143 30a4 5527 clr r21
5144 30a6 481B sub r20,r24
5145 30a8 590B sbc r21,r25
5146 30aa 6417 cp r22,r20
5147 30ac 7507 cpc r23,r21
5148 30ae 04F0 brlt .+2
5149 30b0 00C0 rjmp .L420
5150 30b2 BA01 movw r22,r20
5151 30b4 00C0 rjmp .L420
5152 .L539:
5153 30b6 3095 com r19
5154 30b8 2195 neg r18
5155 30ba 3F4F sbci r19,lo8(-1)
5156 30bc 00C0 rjmp .L366
5157 .L518:
5158 30be 3095 com r19
5159 30c0 2195 neg r18
5160 30c2 3F4F sbci r19,lo8(-1)
5161 30c4 00C0 rjmp .L371
5162 .L354:
5163 30c6 1092 0000 sts (delay_stopmotors.1984)+1,__zero_reg__
5164 30ca 1092 0000 sts delay_stopmotors.1984,__zero_reg__
5165 30ce 0C94 0000 jmp .L290
5166 .L327:
5167 30d2 8081 ld r24,Z
5168 30d4 9181 ldd r25,Z+1
5169 30d6 855B subi r24,lo8(-75)
5170 30d8 9F4F sbci r25,hi8(-75)
5171 30da 04F4 brge .L344
5172 30dc 8091 0000 lds r24,delay_neutral.1982
5173 30e0 8F5F subi r24,lo8(-(1))
5174 30e2 8093 0000 sts delay_neutral.1982,r24
5175 30e6 893C cpi r24,lo8(-55)
5176 30e8 00F4 brsh .+4
5177 30ea 0C94 0000 jmp .L326
5178 30ee 1092 0000 sts delay_neutral.1982,__zero_reg__
5179 30f2 3BE0 ldi r19,lo8(11)
5180 30f4 3D15 cp r19,r13
5181 30f6 00F4 brsh .+2
5182 30f8 00C0 rjmp .L346
5183 30fa 2998 cbi 37-0x20,1
5184 .L347:
5185 30fc 1092 0000 sts (ModelIsFlying)+1,__zero_reg__
5186 3100 1092 0000 sts ModelIsFlying,__zero_reg__
5187 3104 0C94 0000 jmp .L476
5188 .L508:
5189 3108 0196 adiw r24,1
5190 310a 9093 0000 sts (delay_startmotors.1983)+1,r25
5191 310e 8093 0000 sts delay_startmotors.1983,r24
5192 3112 00C0 rjmp .L350
5193 .L499:
5194 3114 81E0 ldi r24,lo8(1)
5195 3116 90E0 ldi r25,hi8(1)
5196 3118 9093 0000 sts (UpdateCompassCourse.1986)+1,r25
5197 311c 8093 0000 sts UpdateCompassCourse.1986,r24
5198 3120 1092 0000 sts ReadingIntegralGyroYaw,__zero_reg__
5199 3124 1092 0000 sts (ReadingIntegralGyroYaw)+1,__zero_reg__
5200 3128 1092 0000 sts (ReadingIntegralGyroYaw)+2,__zero_reg__
5201 312c 1092 0000 sts (ReadingIntegralGyroYaw)+3,__zero_reg__
5202 3130 1092 0000 sts SetPointYaw.1980,__zero_reg__
5203 3134 1092 0000 sts (SetPointYaw.1980)+1,__zero_reg__
5204 3138 1092 0000 sts (SetPointYaw.1980)+2,__zero_reg__
5205 313c 1092 0000 sts (SetPointYaw.1980)+3,__zero_reg__
5206 3140 0C94 0000 jmp .L293
5207 .L538:
5208 3144 E7E0 ldi r30,lo8(7)
5209 3146 F0E0 ldi r31,hi8(7)
5210 3148 AE0E add r10,r30
5211 314a BF1E adc r11,r31
5212 314c 00C0 rjmp .L418
5213 .L344:
5214 314e 1092 0000 sts delay_neutral.1982,__zero_reg__
5215 3152 0C94 0000 jmp .L326
5216 .L352:
5217 3156 7901 movw r14,r18
5218 3158 0027 clr r16
5219 315a F7FC sbrc r15,7
5220 315c 0095 com r16
5221 315e 102F mov r17,r16
5222 3160 CB01 movw r24,r22
5223 3162 BA01 movw r22,r20
5224 3164 A801 movw r20,r16
5225 3166 9701 movw r18,r14
5226 3168 0E94 0000 call __divmodsi4
5227 316c 2093 0000 sts savedVibrationOffsetNick,r18
5228 3170 3093 0000 sts savedVibrationOffsetNick+1,r19
5229 3174 4093 0000 sts savedVibrationOffsetNick+2,r20
5230 3178 5093 0000 sts savedVibrationOffsetNick+3,r21
5231 317c 2093 0000 sts vibrationOffsetNick,r18
5232 3180 3093 0000 sts vibrationOffsetNick+1,r19
5233 3184 4093 0000 sts vibrationOffsetNick+2,r20
5234 3188 5093 0000 sts vibrationOffsetNick+3,r21
5235 318c C601 movw r24,r12
5236 318e B501 movw r22,r10
5237 3190 A801 movw r20,r16
5238 3192 9701 movw r18,r14
5239 3194 0E94 0000 call __divmodsi4
5240 3198 2093 0000 sts savedVibrationOffsetRoll,r18
5241 319c 3093 0000 sts savedVibrationOffsetRoll+1,r19
5242 31a0 4093 0000 sts savedVibrationOffsetRoll+2,r20
5243 31a4 5093 0000 sts savedVibrationOffsetRoll+3,r21
5244 31a8 2093 0000 sts vibrationOffsetRoll,r18
5245 31ac 3093 0000 sts vibrationOffsetRoll+1,r19
5246 31b0 4093 0000 sts vibrationOffsetRoll+2,r20
5247 31b4 5093 0000 sts vibrationOffsetRoll+3,r21
5248 31b8 C401 movw r24,r8
5249 31ba B301 movw r22,r6
5250 31bc A801 movw r20,r16
5251 31be 9701 movw r18,r14
5252 31c0 0E94 0000 call __divmodsi4
5253 31c4 2093 0000 sts vibrationOffsetYaw,r18
5254 31c8 3093 0000 sts (vibrationOffsetYaw)+1,r19
5255 31cc 4093 0000 sts (vibrationOffsetYaw)+2,r20
5256 31d0 5093 0000 sts (vibrationOffsetYaw)+3,r21
5257 31d4 0C94 0000 jmp .L353
5258 .L509:
5259 31d8 89EC ldi r24,lo8(201)
5260 31da 90E0 ldi r25,hi8(201)
5261 31dc 9093 0000 sts (delay_startmotors.1983)+1,r25
5262 31e0 8093 0000 sts delay_startmotors.1983,r24
5263 31e4 81E0 ldi r24,lo8(1)
5264 31e6 90E0 ldi r25,hi8(1)
5265 31e8 9093 0000 sts (ModelIsFlying)+1,r25
5266 31ec 8093 0000 sts ModelIsFlying,r24
5267 31f0 1092 0000 sts (vibrationCalCount.1977)+1,__zero_reg__
5268 31f4 1092 0000 sts vibrationCalCount.1977,__zero_reg__
5269 31f8 1092 0000 sts vibrationCalYaw.1976,__zero_reg__
5270 31fc 1092 0000 sts (vibrationCalYaw.1976)+1,__zero_reg__
5271 3200 1092 0000 sts (vibrationCalYaw.1976)+2,__zero_reg__
5272 3204 1092 0000 sts (vibrationCalYaw.1976)+3,__zero_reg__
5273 3208 1092 0000 sts vibrationCalRoll.1975,__zero_reg__
5274 320c 1092 0000 sts (vibrationCalRoll.1975)+1,__zero_reg__
5275 3210 1092 0000 sts (vibrationCalRoll.1975)+2,__zero_reg__
5276 3214 1092 0000 sts (vibrationCalRoll.1975)+3,__zero_reg__
5277 3218 1092 0000 sts vibrationCalNick.1974,__zero_reg__
5278 321c 1092 0000 sts (vibrationCalNick.1974)+1,__zero_reg__
5279 3220 1092 0000 sts (vibrationCalNick.1974)+2,__zero_reg__
5280 3224 1092 0000 sts (vibrationCalNick.1974)+3,__zero_reg__
5281 3228 88EE ldi r24,lo8(1000)
5282 322a 93E0 ldi r25,hi8(1000)
5283 322c A0E0 ldi r26,hlo8(1000)
5284 322e B0E0 ldi r27,hhi8(1000)
5285 3230 8093 0000 sts savedVibrationOffsetRoll,r24
5286 3234 9093 0000 sts (savedVibrationOffsetRoll)+1,r25
5287 3238 A093 0000 sts (savedVibrationOffsetRoll)+2,r26
5288 323c B093 0000 sts (savedVibrationOffsetRoll)+3,r27
5289 3240 8093 0000 sts savedVibrationOffsetNick,r24
5290 3244 9093 0000 sts (savedVibrationOffsetNick)+1,r25
5291 3248 A093 0000 sts (savedVibrationOffsetNick)+2,r26
5292 324c B093 0000 sts (savedVibrationOffsetNick)+3,r27
5293 3250 8091 0000 lds r24,MKFlags
5294 3254 8960 ori r24,lo8(9)
5295 3256 8093 0000 sts MKFlags,r24
5296 325a 00C0 rjmp .L350
5297 .L329:
5298 325c 299A sbi 37-0x20,1
5299 325e 0C94 0000 jmp .L330
5300 .L336:
5301 3262 21E0 ldi r18,lo8(1)
5302 3264 0C94 0000 jmp .L337
5303 .L331:
5304 3268 8091 0000 lds r24,ParamSet+1
5305 326c A82F mov r26,r24
5306 326e B0E0 ldi r27,lo8(0)
5307 3270 AA0F lsl r26
5308 3272 BB1F rol r27
5309 3274 FD01 movw r30,r26
5310 3276 E050 subi r30,lo8(-(PPM_in))
5311 3278 F040 sbci r31,hi8(-(PPM_in))
5312 327a 0190 ld __tmp_reg__,Z+
5313 327c F081 ld r31,Z
5314 327e E02D mov r30,__tmp_reg__
5315 3280 F7FD sbrc r31,7
5316 3282 00C0 rjmp .L543
5317 .L334:
5318 3284 E734 cpi r30,71
5319 3286 F105 cpc r31,__zero_reg__
5320 3288 04F0 brlt .+4
5321 328a 0C94 0000 jmp .L332
5322 328e 8091 0000 lds r24,ParamSet+8
5323 3292 8872 andi r24,lo8(40)
5324 3294 01F0 breq .L341
5325 3296 A050 subi r26,lo8(-(PPM_in))
5326 3298 B040 sbci r27,hi8(-(PPM_in))
5327 329a 8D91 ld r24,X+
5328 329c 9C91 ld r25,X
5329 329e 97FD sbrc r25,7
5330 32a0 00C0 rjmp .L544
5331 .L343:
5332 32a2 4E97 sbiw r24,30
5333 32a4 04F4 brge .L342
5334 32a6 4050 subi r20,lo8(-(PPM_in))
5335 32a8 5040 sbci r21,hi8(-(PPM_in))
5336 32aa FA01 movw r30,r20
5337 32ac 8081 ld r24,Z
5338 32ae 9181 ldd r25,Z+1
5339 32b0 8A5B subi r24,lo8(-70)
5340 32b2 9F4F sbci r25,hi8(-70)
5341 32b4 04F4 brge .L342
5342 32b6 81E0 ldi r24,lo8(1)
5343 32b8 8093 0000 sts CompassCalState,r24
5344 32bc 88EE ldi r24,lo8(1000)
5345 32be 93E0 ldi r25,hi8(1000)
5346 32c0 9093 0000 sts (BeepTime)+1,r25
5347 32c4 8093 0000 sts BeepTime,r24
5348 32c8 8091 0000 lds r24,ParamSet+2
5349 32cc A82F mov r26,r24
5350 32ce B0E0 ldi r27,lo8(0)
5351 32d0 AA0F lsl r26
5352 32d2 BB1F rol r27
5353 32d4 0C94 0000 jmp .L326
5354 .L346:
5355 32d8 299A sbi 37-0x20,1
5356 32da 00C0 rjmp .L347
5357 .L342:
5358 32dc 0E94 0000 call GetActiveParamSet
5359 32e0 0E94 0000 call ParamSet_ReadFromEEProm
5360 32e4 81E0 ldi r24,lo8(1)
5361 32e6 0C94 0000 jmp .L477
5362 .L341:
5363 32ea 0E94 0000 call GetActiveParamSet
5364 32ee 0E94 0000 call ParamSet_ReadFromEEProm
5365 32f2 0C94 0000 jmp .L476
5366 .L543:
5367 32f6 F095 com r31
5368 32f8 E195 neg r30
5369 32fa FF4F sbci r31,lo8(-1)
5370 32fc 00C0 rjmp .L334
5371 .L544:
5372 32fe 9095 com r25
5373 3300 8195 neg r24
5374 3302 9F4F sbci r25,lo8(-1)
5375 3304 00C0 rjmp .L343
5377 .global IntegralGyroNick
5378 .global IntegralGyroNick
5379 .section .bss
5382 IntegralGyroNick:
5383 0000 0000 0000 .skip 4,0
5384 .global IntegralGyroRoll
5385 .global IntegralGyroRoll
5388 IntegralGyroRoll:
5389 0004 0000 0000 .skip 4,0
5390 .global IntegralGyroYaw
5391 .global IntegralGyroYaw
5394 IntegralGyroYaw:
5395 0008 0000 0000 .skip 4,0
5396 .global ReadingIntegralGyroNick
5397 .global ReadingIntegralGyroNick
5400 ReadingIntegralGyroNick:
5401 000c 0000 0000 .skip 4,0
5402 .global ReadingIntegralGyroRoll
5403 .global ReadingIntegralGyroRoll
5406 ReadingIntegralGyroRoll:
5407 0010 0000 0000 .skip 4,0
5408 .global ReadingIntegralGyroYaw
5409 .global ReadingIntegralGyroYaw
5412 ReadingIntegralGyroYaw:
5413 0014 0000 0000 .skip 4,0
5414 .global CompassHeading
5415 .data
5418 CompassHeading:
5419 0000 FFFF .word -1
5420 .global CompassCourse
5423 CompassCourse:
5424 0002 FFFF .word -1
5425 .global CompassOffCourse
5426 .global CompassOffCourse
5427 .section .bss
5430 CompassOffCourse:
5431 0018 0000 .skip 2,0
5432 .global CompassCalState
5433 .global CompassCalState
5436 CompassCalState:
5437 001a 00 .skip 1,0
5438 .global FunnelCourse
5439 .global FunnelCourse
5442 FunnelCourse:
5443 001b 00 .skip 1,0
5444 .global BadCompassHeading
5445 .data
5448 BadCompassHeading:
5449 0004 F401 .word 500
5450 .global NaviAccNick
5451 .global NaviAccNick
5452 .section .bss
5455 NaviAccNick:
5456 001c 0000 .skip 2,0
5457 .global NaviAccRoll
5458 .global NaviAccRoll
5461 NaviAccRoll:
5462 001e 0000 .skip 2,0
5463 .global NaviCntAcc
5464 .global NaviCntAcc
5467 NaviCntAcc:
5468 0020 0000 .skip 2,0
5469 .global ModelIsFlying
5470 .global ModelIsFlying
5473 ModelIsFlying:
5474 0022 0000 .skip 2,0
5475 .global MKFlags
5476 .global MKFlags
5479 MKFlags:
5480 0024 00 .skip 1,0
5481 .global TurnOver180Nick
5482 .data
5485 TurnOver180Nick:
5486 0006 4C .byte 76
5487 0007 56 .byte 86
5488 0008 03 .byte 3
5489 0009 00 .byte 0
5490 .global TurnOver180Roll
5493 TurnOver180Roll:
5494 000a 4C .byte 76
5495 000b 56 .byte 86
5496 000c 03 .byte 3
5497 000d 00 .byte 0
5498 .global Ki
5501 Ki:
5502 000e 3801 .word 312
5503 .global Poti1
5504 .global Poti1
5505 .section .bss
5508 Poti1:
5509 0025 0000 .skip 2,0
5510 .global Poti2
5511 .global Poti2
5514 Poti2:
5515 0027 0000 .skip 2,0
5516 .global Poti3
5517 .global Poti3
5520 Poti3:
5521 0029 0000 .skip 2,0
5522 .global Poti4
5523 .global Poti4
5526 Poti4:
5527 002b 0000 .skip 2,0
5528 .global Poti5
5529 .global Poti5
5532 Poti5:
5533 002d 0000 .skip 2,0
5534 .global Poti6
5535 .global Poti6
5538 Poti6:
5539 002f 0000 .skip 2,0
5540 .global Poti7
5541 .global Poti7
5544 Poti7:
5545 0031 0000 .skip 2,0
5546 .global Poti8
5547 .global Poti8
5550 Poti8:
5551 0033 0000 .skip 2,0
5552 .global RequiredMotors
5553 .global RequiredMotors
5556 RequiredMotors:
5557 0035 00 .skip 1,0
5558 .global StickNick
5559 .global StickNick
5562 StickNick:
5563 0036 0000 .skip 2,0
5564 .global StickRoll
5565 .global StickRoll
5568 StickRoll:
5569 0038 0000 .skip 2,0
5570 .global StickYaw
5571 .global StickYaw
5574 StickYaw:
5575 003a 0000 .skip 2,0
5576 .global StickGas
5577 .global StickGas
5580 StickGas:
5581 003c 0000 .skip 2,0
5582 .global stickOffsetNick
5583 .global stickOffsetNick
5586 stickOffsetNick:
5587 003e 0000 .skip 2,0
5588 .global stickOffsetRoll
5589 .global stickOffsetRoll
5592 stickOffsetRoll:
5593 0040 0000 .skip 2,0
5594 .global vibrationOffsetNick
5595 .global vibrationOffsetNick
5598 vibrationOffsetNick:
5599 0042 0000 0000 .skip 4,0
5600 .global vibrationOffsetRoll
5601 .global vibrationOffsetRoll
5604 vibrationOffsetRoll:
5605 0046 0000 0000 .skip 4,0
5606 .global vibrationOffsetYaw
5607 .global vibrationOffsetYaw
5610 vibrationOffsetYaw:
5611 004a 0000 0000 .skip 4,0
5612 .global savedVibrationOffsetNick
5613 .global savedVibrationOffsetNick
5616 savedVibrationOffsetNick:
5617 004e 0000 0000 .skip 4,0
5618 .global savedVibrationOffsetRoll
5619 .global savedVibrationOffsetRoll
5622 savedVibrationOffsetRoll:
5623 0052 0000 0000 .skip 4,0
5624 .global GPSStickNick
5625 .global GPSStickNick
5628 GPSStickNick:
5629 0056 0000 .skip 2,0
5630 .global GPSStickRoll
5631 .global GPSStickRoll
5634 GPSStickRoll:
5635 0058 0000 .skip 2,0
5636 .global MaxStickNick
5637 .global MaxStickNick
5640 MaxStickNick:
5641 005a 0000 .skip 2,0
5642 .global MaxStickRoll
5643 .global MaxStickRoll
5646 MaxStickRoll:
5647 005c 0000 .skip 2,0
5648 .global ExternStickNick
5649 .global ExternStickNick
5652 ExternStickNick:
5653 005e 0000 .skip 2,0
5654 .global ExternStickRoll
5655 .global ExternStickRoll
5658 ExternStickRoll:
5659 0060 0000 .skip 2,0
5660 .global ExternStickYaw
5661 .global ExternStickYaw
5664 ExternStickYaw:
5665 0062 0000 .skip 2,0
5666 .global ExternHeightValue
5667 .data
5670 ExternHeightValue:
5671 0010 ECFF .word -20
5672 .global ReadingHeight
5673 .global ReadingHeight
5674 .section .bss
5677 ReadingHeight:
5678 0064 0000 .skip 2,0
5679 .global SetPointHeight
5680 .global SetPointHeight
5683 SetPointHeight:
5684 0066 0000 .skip 2,0
5685 .global AttitudeCorrectionRoll
5686 .global AttitudeCorrectionRoll
5689 AttitudeCorrectionRoll:
5690 0068 0000 .skip 2,0
5691 .global AttitudeCorrectionNick
5692 .global AttitudeCorrectionNick
5695 AttitudeCorrectionNick:
5696 006a 0000 .skip 2,0
5697 .global LoopingNick
5698 .global LoopingNick
5701 LoopingNick:
5702 006c 00 .skip 1,0
5703 .global LoopingRoll
5704 .global LoopingRoll
5707 LoopingRoll:
5708 006d 00 .skip 1,0
5709 .global LoopingLeft
5710 .global LoopingLeft
5713 LoopingLeft:
5714 006e 00 .skip 1,0
5715 .global LoopingRight
5716 .global LoopingRight
5719 LoopingRight:
5720 006f 00 .skip 1,0
5721 .global LoopingDown
5722 .global LoopingDown
5725 LoopingDown:
5726 0070 00 .skip 1,0
5727 .global LoopingTop
5728 .global LoopingTop
5731 LoopingTop:
5732 0071 00 .skip 1,0
5733 .global FCParam
5734 .data
5737 FCParam:
5738 0012 30 .byte 48
5739 0013 FB .byte -5
5740 0014 10 .byte 16
5741 0015 3A .byte 58
5742 0016 40 .byte 64
5743 0017 08 .byte 8
5744 0018 96 .byte -106
5745 0019 96 .byte -106
5746 001a 02 .byte 2
5747 001b 0A .byte 10
5748 001c 00 .byte 0
5749 001d 00 .byte 0
5750 001e 00 .byte 0
5751 001f 00 .byte 0
5752 0020 00 .byte 0
5753 0021 00 .byte 0
5754 0022 00 .byte 0
5755 0023 00 .byte 0
5756 0024 64 .byte 100
5757 0025 46 .byte 70
5758 0026 5A .byte 90
5759 0027 41 .byte 65
5760 0028 40 .byte 64
5761 0029 64 .byte 100
5762 002a 00 .byte 0
5763 002b 00 .byte 0
5764 002c 00 .byte 0
5765 002d 0000 00 .skip 3,0
5766 .lcomm stick_roll.1991,2
5767 .lcomm stick_nick.1990,2
5768 .lcomm MotorValue.1987,24
5769 .lcomm UpdateCompassCourse.1986,2
5770 .lcomm TimerDebugOut.1985,1
5771 .lcomm delay_stopmotors.1984,2
5772 .lcomm delay_startmotors.1983,2
5773 .lcomm delay_neutral.1982,1
5774 .lcomm RcLostTimer.1981,2
5775 .lcomm SetPointYaw.1980,4
5776 .lcomm vibrationCalCount.1977,2
5777 .lcomm vibrationCalYaw.1976,4
5778 .lcomm vibrationCalRoll.1975,4
5779 .lcomm vibrationCalNick.1974,4
5780 .lcomm IPartRoll.1973,4
5781 .lcomm IPartNick.1972,4
5784 stick.1951:
5785 0030 01 .byte 1
5786 .comm GyroNick,2,1
5787 .comm GyroRoll,2,1
5788 .comm GyroYaw,2,1
5789 .comm YawGyroHeading,4,1
5790 .comm GyroPFactor,1,1
5791 .comm GyroIFactor,1,1
5792 .comm GyroYawPFactor,1,1
5793 .comm GyroYawIFactor,1,1
5794 .comm NickNoisePeak,2,1
5795 .comm RollNoisePeak,2,1
5796 .comm debugNickNoisePeak,2,1
5797 .comm debugRollNoisePeak,2,1
DEFINED SYMBOLS
*ABS*:00000000 fc.c
/tmp/ccDP7nnW.s:2 *ABS*:0000003f __SREG__
/tmp/ccDP7nnW.s:3 *ABS*:0000003e __SP_H__
/tmp/ccDP7nnW.s:4 *ABS*:0000003d __SP_L__
/tmp/ccDP7nnW.s:5 *ABS*:00000000 __tmp_reg__
/tmp/ccDP7nnW.s:6 *ABS*:00000001 __zero_reg__
/tmp/ccDP7nnW.s:12 .text:00000000 MotorSmoothing
/tmp/ccDP7nnW.s:36 .text:00000020 Mean
/tmp/ccDP7nnW.s:5598 .bss:00000042 vibrationOffsetNick
*COM*:00000002 GyroNick
/tmp/ccDP7nnW.s:5604 .bss:00000046 vibrationOffsetRoll
*COM*:00000002 GyroRoll
*COM*:00000002 NickNoisePeak
*COM*:00000002 RollNoisePeak
/tmp/ccDP7nnW.s:5610 .bss:0000004a vibrationOffsetYaw
*COM*:00000002 GyroYaw
*COM*:00000004 YawGyroHeading
/tmp/ccDP7nnW.s:5412 .bss:00000014 ReadingIntegralGyroYaw
/tmp/ccDP7nnW.s:5400 .bss:0000000c ReadingIntegralGyroNick
/tmp/ccDP7nnW.s:5485 .data:00000006 TurnOver180Nick
/tmp/ccDP7nnW.s:5406 .bss:00000010 ReadingIntegralGyroRoll
/tmp/ccDP7nnW.s:5493 .data:0000000a TurnOver180Roll
/tmp/ccDP7nnW.s:5382 .bss:00000000 IntegralGyroNick
/tmp/ccDP7nnW.s:5388 .bss:00000004 IntegralGyroRoll
/tmp/ccDP7nnW.s:5394 .bss:00000008 IntegralGyroYaw
/tmp/ccDP7nnW.s:423 .text:00000414 ParameterMapping
/tmp/ccDP7nnW.s:5737 .data:00000012 FCParam
/tmp/ccDP7nnW.s:5526 .bss:0000002b Poti4
/tmp/ccDP7nnW.s:5501 .data:0000000e Ki
/tmp/ccDP7nnW.s:5508 .bss:00000025 Poti1
/tmp/ccDP7nnW.s:5514 .bss:00000027 Poti2
/tmp/ccDP7nnW.s:5520 .bss:00000029 Poti3
/tmp/ccDP7nnW.s:1347 .text:00000c54 SendMotorData
/tmp/ccDP7nnW.s:5479 .bss:00000024 MKFlags
/tmp/ccDP7nnW.s:1408 .text:00000cd6 SetNeutral
/tmp/ccDP7nnW.s:5418 .data:00000000 CompassHeading
/tmp/ccDP7nnW.s:5423 .data:00000002 CompassCourse
/tmp/ccDP7nnW.s:5628 .bss:00000056 GPSStickNick
/tmp/ccDP7nnW.s:5634 .bss:00000058 GPSStickRoll
/tmp/ccDP7nnW.s:5586 .bss:0000003e stickOffsetNick
/tmp/ccDP7nnW.s:5562 .bss:00000036 StickNick
/tmp/ccDP7nnW.s:5592 .bss:00000040 stickOffsetRoll
/tmp/ccDP7nnW.s:5568 .bss:00000038 StickRoll
/tmp/ccDP7nnW.s:1560 .text:00000e96 Beep
/tmp/ccDP7nnW.s:1599 .text:00000eda SetCompassCalState
/tmp/ccDP7nnW.s:5784 .data:00000030 stick.1951
/tmp/ccDP7nnW.s:5436 .bss:0000001a CompassCalState
/tmp/ccDP7nnW.s:1646 .text:00000f38 MotorControl
/tmp/ccDP7nnW.s:5580 .bss:0000003c StickGas
/tmp/ccDP7nnW.s:5773 .bss:00000096 RcLostTimer.1981
/tmp/ccDP7nnW.s:5473 .bss:00000022 ModelIsFlying
/tmp/ccDP7nnW.s:5707 .bss:0000006d LoopingRoll
/tmp/ccDP7nnW.s:5701 .bss:0000006c LoopingNick
/tmp/ccDP7nnW.s:5574 .bss:0000003a StickYaw
*COM*:00000001 GyroPFactor
*COM*:00000001 GyroIFactor
*COM*:00000001 GyroYawPFactor
*COM*:00000001 GyroYawIFactor
/tmp/ccDP7nnW.s:5640 .bss:0000005a MaxStickNick
/tmp/ccDP7nnW.s:5646 .bss:0000005c MaxStickRoll
/tmp/ccDP7nnW.s:5616 .bss:0000004e savedVibrationOffsetNick
/tmp/ccDP7nnW.s:5622 .bss:00000052 savedVibrationOffsetRoll
*COM*:00000002 debugNickNoisePeak
*COM*:00000002 debugRollNoisePeak
/tmp/ccDP7nnW.s:5448 .data:00000004 BadCompassHeading
/tmp/ccDP7nnW.s:5774 .bss:00000098 SetPointYaw.1980
/tmp/ccDP7nnW.s:5769 .bss:00000090 TimerDebugOut.1985
/tmp/ccDP7nnW.s:5442 .bss:0000001b FunnelCourse
/tmp/ccDP7nnW.s:5780 .bss:000000ae IPartNick.1972
/tmp/ccDP7nnW.s:5779 .bss:000000aa IPartRoll.1973
/tmp/ccDP7nnW.s:5767 .bss:00000076 MotorValue.1987
/tmp/ccDP7nnW.s:5532 .bss:0000002d Poti5
/tmp/ccDP7nnW.s:5538 .bss:0000002f Poti6
/tmp/ccDP7nnW.s:5544 .bss:00000031 Poti7
/tmp/ccDP7nnW.s:5550 .bss:00000033 Poti8
/tmp/ccDP7nnW.s:5772 .bss:00000095 delay_neutral.1982
/tmp/ccDP7nnW.s:5771 .bss:00000093 delay_startmotors.1983
/tmp/ccDP7nnW.s:5778 .bss:000000a6 vibrationCalNick.1974
/tmp/ccDP7nnW.s:5777 .bss:000000a2 vibrationCalRoll.1975
/tmp/ccDP7nnW.s:5776 .bss:0000009e vibrationCalYaw.1976
/tmp/ccDP7nnW.s:5775 .bss:0000009c vibrationCalCount.1977
/tmp/ccDP7nnW.s:5766 .bss:00000074 stick_nick.1990
.bss:00000072 stick_roll.1991
/tmp/ccDP7nnW.s:5670 .data:00000010 ExternHeightValue
/tmp/ccDP7nnW.s:5713 .bss:0000006e LoopingLeft
/tmp/ccDP7nnW.s:5719 .bss:0000006f LoopingRight
/tmp/ccDP7nnW.s:5731 .bss:00000071 LoopingTop
/tmp/ccDP7nnW.s:5725 .bss:00000070 LoopingDown
/tmp/ccDP7nnW.s:5677 .bss:00000064 ReadingHeight
/tmp/ccDP7nnW.s:5768 .bss:0000008e UpdateCompassCourse.1986
/tmp/ccDP7nnW.s:5770 .bss:00000091 delay_stopmotors.1984
/tmp/ccDP7nnW.s:5430 .bss:00000018 CompassOffCourse
/tmp/ccDP7nnW.s:5455 .bss:0000001c NaviAccNick
/tmp/ccDP7nnW.s:5461 .bss:0000001e NaviAccRoll
/tmp/ccDP7nnW.s:5467 .bss:00000020 NaviCntAcc
/tmp/ccDP7nnW.s:5556 .bss:00000035 RequiredMotors
/tmp/ccDP7nnW.s:5652 .bss:0000005e ExternStickNick
/tmp/ccDP7nnW.s:5658 .bss:00000060 ExternStickRoll
/tmp/ccDP7nnW.s:5664 .bss:00000062 ExternStickYaw
/tmp/ccDP7nnW.s:5683 .bss:00000066 SetPointHeight
/tmp/ccDP7nnW.s:5689 .bss:00000068 AttitudeCorrectionRoll
/tmp/ccDP7nnW.s:5695 .bss:0000006a AttitudeCorrectionNick
 
UNDEFINED SYMBOLS
__do_copy_data
__do_clear_bss
hiResPitchGyro
__divmodsi4
filteredHiResPitchGyro
hiResRollGyro
filteredHiResRollGyro
rawYawGyroSum
ADReady
RC_Quality
ParamSet
__divmodhi4
MotorTest_Active
Motor
MotorTest
DebugOut
I2C_Start
Servo_Off
analog_calibrate
BeepTime
PPM_in
Servo_On
Delay_ms
BoardRelease
PcAccess
BeepModulation
PPM_diff
NewPpmData
ADCycleCount
__mulsi3
MissingMotor
Mixer
SetActiveParamSet
GetActiveParamSet
ParamSet_ReadFromEEProm
ExternControl
UBat
/branches/dongfang_FC_rewrite/flexcontrol.h
0,0 → 1,8
typedef struct {
int16_t(*getPitch)(void);
int16_t(*getRoll)(void);
int16_t(*getYaw)(void);
uint16_t(*getThrottle)(void);
uint8_t isSignalGood(void);
uint8_t isSignalUnreadable(void);
} t_control;
/branches/dongfang_FC_rewrite/flight.c
0,0 → 1,585
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 "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();
 
// TODO: Move off.
// RC_Quality = 100;
}
 
/************************************************************************/
/* 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[12] = Motor[0].SetPoint; // Front
DebugOut.Analog[13] = Motor[1].SetPoint; // Rear
DebugOut.Analog[14] = Motor[3].SetPoint; // Left
DebugOut.Analog[15] = 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 ? dynamicParams.GyroI : 0,
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, pitchTerm, rollTerm;
 
// PID controller variables
int16_t PDPartPitch, PDPartRoll, PDPartYaw, PPartPitch, PPartRoll;
static int32_t IPartPitch = 0, IPartRoll = 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;
 
// No support for altitude control right now.
// static uint8_t HeightControlActive = 0;
// static int16_t HeightControlGas = 0;
 
static int8_t debugDataTimer = 1;
 
// High resolution motor values for smoothing of PID motor outputs
static int16_t motorFilters[MAX_MOTORS];
 
uint8_t i;
 
// 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.GasMin + 10) throttleTerm = staticParams.GasMin + 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.
* TODO: What was the value of IPartPitch? At 1st run of this, it's 0 already.
*/
if(isFlying < 256) {
IPartPitch = 0;
IPartRoll = 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
/************************************************************************/
/* 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).
if(looping & LOOPING_PITCH_AXIS) {
PPartPitch = 0;
} else { // TODO: Where do the 44000 come from???
PPartPitch = pitchAngle * gyroIFactor / (44000 / STICK_GAIN); // P-Part - Proportional to Integral
}
// Now blend in the D-part - proportional to the Differential of the integral = the rate.
PDPartPitch = PPartPitch + (int32_t)((int32_t)pitchRate_PID * gyroPFactor / (256L / STICK_GAIN))
+ (pitchDifferential * (int16_t)dynamicParams.GyroD) / 16;
// The P-part is actually the I-part...
if(looping & LOOPING_ROLL_AXIS) {
PPartRoll = 0;
} else { // TODO: Where do the 44000 come from???
PPartRoll = (rollAngle * gyroIFactor) / (44000 / STICK_GAIN); // P-Part - Proportional to Integral
}
// Now blend in the P-part - proportional to the Differential of the integral = the rate
PDPartRoll = PPartRoll + (int32_t)((int32_t)rollRate_PID * gyroPFactor / (256L / STICK_GAIN))
+ (rollDifferential * (int16_t)dynamicParams.GyroD) / 16;
PDPartYaw = (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / STICK_GAIN)
+ (int32_t)(yawAngle * yawIFactor) / (2 * (44000 / STICK_GAIN));
// limit control feedback
#define SENSOR_LIMIT (4096 * 4)
CHECK_MIN_MAX(PDPartPitch, -SENSOR_LIMIT, SENSOR_LIMIT);
CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT);
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.GasMin; // reduce gas to min to avoid lift of
}
 
/*
* Height control was here.
*/
if(throttleTerm > staticParams.GasMax - 20) throttleTerm = (staticParams.GasMax - 20);
throttleTerm *= STICK_GAIN;
 
/*
* Compose yaw term.
*/
#define MIN_YAWGAS (40 * STICK_GAIN) // yaw also below this gas value
yawTerm = PDPartYaw - setPointYaw * STICK_GAIN;
// limit yawTerm
if(throttleTerm > MIN_YAWGAS) {
/*
* -throttle/2 < -20 <= yawTerm <= 20 < throttle/2
*/
CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2));
} else {
/*
* -20 <= yawTerm <= 20
*/
CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
}
tmp_int = staticParams.GasMax * STICK_GAIN;
 
/*
* throttle-MaxThrottle <= yawTerm <= MaxThrottle-throttle
*/
CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
/*
* 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.
IPartPitch += PPartPitch - controlPitch; // Integrate difference between P part (the angle) and the stick pos.
IPartRoll += PPartRoll - controlRoll; // I-part for attitude control OK
} else {
// "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.
IPartPitch += PDPartPitch - controlPitch; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
IPartRoll += PDPartRoll - controlRoll; // 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(IPartPitch, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
 
// Add (P, D) parts minus stick pos. to the scaled-down I part.
pitchTerm = PDPartPitch - controlPitch + IPartPitch / Ki; // PID-controller for pitch
 
CHECK_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
rollTerm = PDPartRoll - controlRoll + IPartRoll / Ki; // PID-controller for roll
 
/*
* 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?
*/
tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64;
CHECK_MIN_MAX(pitchTerm, -tmp_int, tmp_int);
CHECK_MIN_MAX(rollTerm, -tmp_int, tmp_int);
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Universal Mixer
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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)pitchTerm * Mixer.Motor[i][MIX_PITCH]) / 64L;
tmp += ((int32_t)rollTerm * 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] / STICK_GAIN;
CHECK_MIN_MAX(tmp, staticParams.GasMin, staticParams.GasMax);
Motor[i].SetPoint = tmp;
}
else Motor[i].SetPoint = 0;
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!(--debugDataTimer)) {
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
DebugOut.Analog[0] = (10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
DebugOut.Analog[1] = (10 * rollAngle) / 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] = ZAxisAcc;
 
DebugOut.Analog[19] = throttleTerm;
DebugOut.Analog[20] = pitchTerm;
DebugOut.Analog[21] = rollTerm;
DebugOut.Analog[22] = yawTerm;
DebugOut.Analog[23] = PPartPitch; //
DebugOut.Analog[24] = IPartPitch /Ki; // meget meget lille.
DebugOut.Analog[25] = PDPartPitch; // omtrent lig ppart.
 
DebugOut.Analog[26] = pitchAccNoisePeak;
DebugOut.Analog[27] = rollAccNoisePeak;
 
DebugOut.Analog[30] = pitchGyroNoisePeak;
DebugOut.Analog[31] = rollGyroNoisePeak;
}
}
/branches/dongfang_FC_rewrite/flight.h
0,0 → 1,28
/*#######################################################################################
Flight Control
#######################################################################################*/
 
#ifndef _FLIGHT_H
#define _FLIGHT_H
 
#include <inttypes.h>
#include "analog.h"
#include "configuration.h"
 
extern uint8_t RequiredMotors;
 
// looping params
extern long TurnOver180Nick, TurnOver180Roll;
 
// external control
extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw;
 
extern int16_t naviAccPitch, naviAccRoll, naviCntAcc;
 
extern volatile uint8_t MKFlags;
 
void flight_control(void);
void sendMotorData(void);
void flight_setNeutral(void);
 
#endif //_FLIGHT_H
/branches/dongfang_FC_rewrite/flight2.c
0,0 → 1,607
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 "eeprom.h"
#include "flight.h"
 
// Only for debug. Remove.
#include "analog.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;}
 
// TODO: 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 yawRatePFactor, yawRateIFactor; // 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
// 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();
 
// TODO: Move off.
// RC_Quality = 100;
}
 
/************************************************************************/
/* 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[12] = Motor[0].SetPoint; // Front
DebugOut.Analog[13] = Motor[1].SetPoint; // Rear
DebugOut.Analog[14] = Motor[3].SetPoint; // Left
DebugOut.Analog[15] = 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;
yawRatePFactor = _yawPFactor;
yawRateIFactor = _yawIFactor;
}
 
void setNormalFlightParameters(void) {
if(staticParams.GlobalConfig & CFG_HEADING_HOLD) gyroIFactor = 0;
else gyroIFactor = dynamicParams.GyroI;
 
setFlightParameters(dynamicParams.IFactor + 1,
dynamicParams.GyroP + 10,
staticParams.GlobalConfig & CFG_HEADING_HOLD ? dynamicParams.GyroI : 0,
dynamicParams.GyroP + 10,
dynamicParams.UserParams[6]
);
}
 
void setStableFlightParameters(void) {
setFlightParameters(33, 90, 120, 90, 120);
}
 
void handleCommands(uint8_t command, 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
uint8_t setting = controlMixer_getArgument();
if ((setting > 0 && setting < 6) || setting == 9) {
// Gyro calinbration, with or without selecting a new parameter-set.
if(setting > 0 && setting < 6) {
// A valid parameter-set (1..5) was chosen - use it.
setActiveParamSet(setting);
}
ParamSet_ReadFromEEProm(getActiveParamSet());
attitude_setNeutral();
flight_setNeutral();
// Right stick is centered; calibrate it to zero (hmm strictly does not belong here).
controlMixer_setNeutral(setting == 9); // Calibrate right stick neutral position.
beepNumber(getActiveParamSet());
} else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && setting == 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, pitchTerm, rollTerm;
 
// PID controller variables
int16_t PDPartPitch, PDPartRoll, PDPartYaw, PPartPitch, PPartRoll;
static int32_t IPartPitch = 0, IPartRoll = 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;
 
// No support for altitude control right now.
// static uint8_t HeightControlActive = 0;
// static int16_t HeightControlGas = 0;
 
static int8_t debugDataTimer = 0;
 
// High resolution motor values for smoothing of PID motor outputs
static int16_t motorFilters[MAX_MOTORS];
 
uint8_t i;
 
// 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.GasMin + 10) throttleTerm = staticParams.GasMin + 10;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// RC-signal is bad
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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.
* TODO: What was the value of IPartPitch? At 1st run of this, it's 0 already.
*/
if(isFlying < 256) {
IPartPitch = 0;
IPartRoll = 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.
*/
handleCommands(controlMixer_getCommand(), controlMixer_isCommandRepeated());
 
// if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
setNormalFlightParameters();
// }
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// in case of emergency landing
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// set all inputs to save values
/*
* 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 latter.
*/
if(looping) {
if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit;
}
/*
* Here is a dynamic calibration experiment: Adjust integrals and gyro offsets if the pilot appears to be always
* pushing of pulling on the pitch or roll stick.
*/
/*
if(ADCycleCount >= dynamicParams.UserParam2 * 10) {
// This algo works OK on the desk but it is a little sluggish and it oscillates.
// It does not very effectively cancel drift because of dynamics.
minStickForAutoCal = dynamicParams.UserParam3 * 10;
maxStickForAutoCal = dynamicParams.UserParam4 * 10;
// If not already corrected to the limit, and dynamic calibration is enabled:
if (abs(dynamicOffsetPitch - savedDynamicOffsetPitch) < dynamicParams.UserParam1 && !dynamicParams.UserParam6) {
// The pilot pushes on the stick, the integral is > 0, and the gyro val is > 0. Looks like a value-too-high case, so increase the offset.
if (filteredHiResPitchGyro > dynamicOffsetPitch && pitchAngle > 0 && maxStickPitch >= minStickForAutoCal && maxStickPitch <= maxStickForAutoCal) {
dynamicOffsetPitch += (int8_t)(dynamicParams.UserParam7 - 128); // (adding something seems right...)
pitchAngle = (pitchAngle * (int32_t)dynamicParams.UserParam5) / 100L;
} else if (filteredHiResPitchGyro < dynamicOffsetPitch && pitchAngle < 0 && maxStickPitch <= -minStickForAutoCal && maxStickPitch >= -maxStickForAutoCal) {
dynamicOffsetPitch -= (int8_t)(dynamicParams.UserParam7 - 128); // (subtracting something seems right...)
pitchAngle = (pitchAngle * (int32_t)dynamicParams.UserParam5) / 100L;
}
}
// If not already corrected to the limit, and dynamic calibration is enabled:
if (abs(dynamicOffsetRoll - savedDynamicOffsetRoll) <= dynamicParams.UserParam1 && !dynamicParams.UserParam6) {
if (filteredHiResRollGyro > dynamicOffsetRoll && rollAngle > 0 && maxStickRoll >= minStickForAutoCal && maxStickRoll <= maxStickForAutoCal) {
dynamicOffsetRoll += (int8_t)(dynamicParams.UserParam8 - 128);
rollAngle = (rollAngle * (int32_t)dynamicParams.UserParam5) / 100L;
} else if (filteredHiResRollGyro < dynamicOffsetRoll && rollAngle < 0 && maxStickRoll <= -minStickForAutoCal && maxStickRoll >= -maxStickForAutoCal) {
dynamicOffsetRoll -= (int8_t)(dynamicParams.UserParam8 - 128);
rollAngle = (rollAngle * (int32_t)dynamicParams.UserParam5) / 100L;
}
}
ADCycleCount = 0;
}
*/
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Yawing
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated
badCompassHeading = 1000;
if(!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) {
updateCompassCourse = 1;
}
}
setPointYaw = controlYaw;
 
// trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw)
// TODO: We want NO feedback of control related stuff to the attitude related stuff.
yawAngle -= tmp_int;
 
// limit the effect
CHECK_MIN_MAX(yawAngle, -50000, 50000)
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Compass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// compass code is used if Compass option is selected
 
/*
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) {
updateCompass();
}
*/
 
#if defined (USE_MK3MAG)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// GPS
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) {
GPS_Main();
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
}
else {
// GPSStickPitch = 0;
// GPSStickRoll = 0;
}
#endif
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// calculate control feedback from angle (gyro integral) and agular velocity (gyro signal)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// The P-part is actually the I-part...
if(looping & LOOPING_PITCH_AXIS) {
PPartPitch = 0;
} else {
// TODO: Where do the 44000 come from???
PPartPitch = (pitchAngle * gyroIFactor) / (44000 / STICK_GAIN); // P-Part - Proportional to Integral
// PPartPitch = IntegralNickMalFaktor in H&I code.
}
 
// Now blend in the P-part - proportional to the Differential of the integral = the gyro value.
PDPartPitch = PPartPitch + (int32_t)((int32_t)pitchRate_PID * gyroPFactor) / (256L / STICK_GAIN)
+ (pitchDifferential * dynamicParams.GyroD) / 16;
// = MesswertNick in H&I code
// The P-part is actually the I-part...
if(looping & LOOPING_ROLL_AXIS) {
PPartRoll = 0;
} else {
PPartRoll = (rollAngle * gyroIFactor) / (44000 / STICK_GAIN); // P-Part - Proportional to Integral
}
// Now blend in the P-part - proportional to the Differential of the integral = the gyro value.
PDPartRoll = PPartRoll + (int32_t)((int32_t)rollRate_PID * gyroPFactor) / (256L / STICK_GAIN)
+ (rollDifferential * dynamicParams.GyroD) / 16;
PDPartYaw = (int32_t)(yawRate * 2 * (int32_t)yawRatePFactor) / (256L / STICK_GAIN) + (int32_t)(yawAngle * yawRateIFactor) / (2 * (44000 / STICK_GAIN));
// limit control feedback
#define SENSOR_LIMIT (4096 * 4)
CHECK_MIN_MAX(PDPartPitch, -SENSOR_LIMIT, SENSOR_LIMIT);
CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT);
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.GasMin; // reduce gas to min to avoid lift of
}
 
/*
* Height control was here.
*/
 
if(throttleTerm > staticParams.GasMax - 20) throttleTerm = (staticParams.GasMax - 20);
throttleTerm *= STICK_GAIN;
 
/*
* Compose yaw term.
*/
#define MIN_YAWGAS (40 * STICK_GAIN) // yaw also below this gas value
yawTerm = PDPartYaw - setPointYaw * STICK_GAIN; // yaw controller
// 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.GasMax * STICK_GAIN;
CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
 
/*
* 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.
IPartPitch += PPartPitch - controlPitch; // Integrate difference between P part (the angle) and the stick pos.
IPartRoll += PPartRoll - controlRoll; // I-part for attitude control OK
} else {
// "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.
IPartPitch += PDPartPitch - controlPitch; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
IPartRoll += PDPartRoll - controlRoll; // 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(IPartPitch, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
 
// Add (P, D) parts minus stick pos. to the scaled-down I part.
pitchTerm = PDPartPitch - controlPitch + IPartPitch / Ki; // PID-controller for pitch
 
CHECK_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L));
rollTerm = PDPartRoll - controlRoll + IPartRoll / Ki; // PID-controller for roll
 
/*
* 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?
*/
tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64;
CHECK_MIN_MAX(pitchTerm, -tmp_int, tmp_int);
CHECK_MIN_MAX(rollTerm, -tmp_int, tmp_int);
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Universal Mixer
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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)pitchTerm * Mixer.Motor[i][MIX_PITCH]) / 64L;
tmp += ((int32_t)rollTerm * 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] / STICK_GAIN;
CHECK_MIN_MAX(tmp, staticParams.GasMin, staticParams.GasMax);
Motor[i].SetPoint = tmp;
}
else Motor[i].SetPoint = 0;
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!debugDataTimer--) {
debugDataTimer = 24; // update debug outputs every 25*2ms = 50 ms (20Hz)
DebugOut.Analog[0] = (10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
DebugOut.Analog[1] = (10 * rollAngle) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
 
/*
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = RC_Quality;
DebugOut.Analog[11] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
*/
 
// 12..15 are the 4 first motors.
 
DebugOut.Analog[16] = pitchAxisAcc;
DebugOut.Analog[17] = rollAxisAcc;
// DebugOut.Analog[18] = ZAxisAcc;
DebugOut.Analog[19] = throttleTerm;
DebugOut.Analog[20] = pitchTerm;
DebugOut.Analog[21] = rollTerm;
DebugOut.Analog[22] = yawTerm;
DebugOut.Analog[23] = PPartPitch; //
DebugOut.Analog[24] = IPartPitch /Ki; // meget meget lille.
DebugOut.Analog[25] = PDPartPitch; // omtrent lig ppart.
 
DebugOut.Analog[26] = pitchAccNoisePeak;
DebugOut.Analog[27] = rollAccNoisePeak;
 
DebugOut.Analog[30] = pitchGyroNoisePeak;
DebugOut.Analog[31] = rollGyroNoisePeak;
}
}
/branches/dongfang_FC_rewrite/gps.c
0,0 → 1,465
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <inttypes.h>
#include <stdlib.h>
#include "ubx.h"
#include "mymath.h"
#include "timer0.h"
#include "uart0.h"
#include "rc.h"
#include "eeprom.h"
 
typedef enum
{
GPS_FLIGHT_MODE_UNDEF,
GPS_FLIGHT_MODE_FREE,
GPS_FLIGHT_MODE_AID,
GPS_FLIGHT_MODE_HOME,
} FlightMode_t;
 
#define GPS_POSINTEGRAL_LIMIT 32000
#define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes
#define GPS_P_LIMIT 25
 
 
typedef struct
{
int32_t Longitude;
int32_t Latitude;
int32_t Altitude;
Status_t Status;
} GPS_Pos_t;
 
// GPS coordinates for hold position
GPS_Pos_t HoldPosition = {0,0,0,INVALID};
// GPS coordinates for home position
GPS_Pos_t HomePosition = {0,0,0,INVALID};
// the current flight mode
FlightMode_t FlightMode = GPS_FLIGHT_MODE_UNDEF;
 
 
// ---------------------------------------------------------------------------------
void GPS_UpdateParameter(void)
{
static FlightMode_t FlightModeOld = GPS_FLIGHT_MODE_UNDEF;
 
if((RC_Quality < 100) || (MKFlags & MKFLAG_EMERGENCY_LANDING))
{
FlightMode = GPS_FLIGHT_MODE_FREE;
}
else
{
if (dynamicParams.NaviGpsModeControl < 50) FlightMode = GPS_FLIGHT_MODE_AID;
else if(dynamicParams.NaviGpsModeControl < 180) FlightMode = GPS_FLIGHT_MODE_FREE;
else FlightMode = GPS_FLIGHT_MODE_HOME;
}
if (FlightMode != FlightModeOld)
{
BeepTime = 100;
}
FlightModeOld = FlightMode;
}
 
 
 
// ---------------------------------------------------------------------------------
// This function defines a good GPS signal condition
uint8_t GPS_IsSignalOK(void)
{
static uint8_t GPSFix = 0;
if( (GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D) && (GPSInfo.flags & FLAG_GPSFIXOK) && ((GPSInfo.satnum >= staticParams.NaviGpsMinSat) || GPSFix))
{
GPSFix = 1;
return(1);
 
}
else return (0);
 
}
// ---------------------------------------------------------------------------------
// rescale xy-vector length to limit
uint8_t GPS_LimitXY(int32_t *x, int32_t *y, int32_t limit)
{
uint8_t retval = 0;
int32_t len;
len = (int32_t)c_sqrt(*x * *x + *y * *y);
if (len > limit)
{
// normalize control vector components to the limit
*x = (*x * limit) / len;
*y = (*y * limit) / len;
retval = 1;
}
return(retval);
}
 
// checks nick and roll sticks for manual control
uint8_t GPS_IsManualControlled(void)
{
if ( (abs(PPM_in[staticParams.ChannelAssignment[CH_NICK]]) < staticParams.NaviStickThreshold) && (abs(PPM_in[staticParams.ChannelAssignment[CH_ROLL]]) < staticParams.NaviStickThreshold)) return 0;
else return 1;
}
 
// set given position to current gps position
uint8_t GPS_SetCurrPosition(GPS_Pos_t * pGPSPos)
{
uint8_t retval = 0;
if(pGPSPos == NULL) return(retval); // bad pointer
 
if(GPS_IsSignalOK())
{ // is GPS signal condition is fine
pGPSPos->Longitude = GPSInfo.longitude;
pGPSPos->Latitude = GPSInfo.latitude;
pGPSPos->Altitude = GPSInfo.altitude;
pGPSPos->Status = NEWDATA;
retval = 1;
}
else
{ // bad GPS signal condition
pGPSPos->Status = INVALID;
retval = 0;
}
return(retval);
}
 
// clear position
uint8_t GPS_ClearPosition(GPS_Pos_t * pGPSPos)
{
uint8_t retval = 0;
if(pGPSPos == NULL) return(retval); // bad pointer
else
{
pGPSPos->Longitude = 0;
pGPSPos->Latitude = 0;
pGPSPos->Altitude = 0;
pGPSPos->Status = INVALID;
retval = 1;
}
return (retval);
}
 
// disable GPS control sticks
void GPS_Neutral(void)
{
GPSStickNick = 0;
GPSStickRoll = 0;
}
 
// calculates the GPS control stick values from the deviation to target position
// if the pointer to the target positin is NULL or is the target position invalid
// then the P part of the controller is deactivated.
void GPS_PIDController(GPS_Pos_t *pTargetPos)
{
static int32_t PID_Nick, PID_Roll;
int32_t coscompass, sincompass;
int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
int32_t PID_North = 0, PID_East = 0;
static int32_t cos_target_latitude = 1;
static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0;
static GPS_Pos_t *pLastTargetPos = 0;
 
// if GPS data and Compass are ok
if( GPS_IsSignalOK() && (CompassHeading >= 0) )
{
 
if(pTargetPos != NULL) // if there is a target position
{
if(pTargetPos->Status != INVALID) // and the position data are valid
{
// if the target data are updated or the target pointer has changed
if ((pTargetPos->Status != PROCESSED) || (pTargetPos != pLastTargetPos) )
{
// reset error integral
GPSPosDevIntegral_North = 0;
GPSPosDevIntegral_East = 0;
// recalculate latitude projection
cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L));
// remember last target pointer
pLastTargetPos = pTargetPos;
// mark data as processed
pTargetPos->Status = PROCESSED;
}
// calculate position deviation from latitude and longitude differences
GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally
GPSPosDev_East = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally
// calculate latitude projection
GPSPosDev_East *= cos_target_latitude;
GPSPosDev_East /= 8192;
}
else // no valid target position available
{
// reset error
GPSPosDev_North = 0;
GPSPosDev_East = 0;
// reset error integral
GPSPosDevIntegral_North = 0;
GPSPosDevIntegral_East = 0;
}
}
else // no target position available
{
// reset error
GPSPosDev_North = 0;
GPSPosDev_East = 0;
// reset error integral
GPSPosDevIntegral_North = 0;
GPSPosDevIntegral_East = 0;
}
 
//Calculate PID-components of the controller
 
// D-Part
D_North = ((int32_t)dynamicParams.NaviGpsD * GPSInfo.velnorth)/512;
D_East = ((int32_t)dynamicParams.NaviGpsD * GPSInfo.veleast)/512;
 
// P-Part
P_North = ((int32_t)dynamicParams.NaviGpsP * GPSPosDev_North)/2048;
P_East = ((int32_t)dynamicParams.NaviGpsP * GPSPosDev_East)/2048;
 
// I-Part
I_North = ((int32_t)dynamicParams.NaviGpsI * GPSPosDevIntegral_North)/8192;
I_East = ((int32_t)dynamicParams.NaviGpsI * GPSPosDevIntegral_East)/8192;
 
 
// combine P & I
PID_North = P_North + I_North;
PID_East = P_East + I_East;
if(!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT))
{
GPSPosDevIntegral_North += GPSPosDev_North/16;
GPSPosDevIntegral_East += GPSPosDev_East/16;
GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, GPS_POSINTEGRAL_LIMIT);
}
 
// combine PI- and D-Part
PID_North += D_North;
PID_East += D_East;
 
 
// scale combination with gain.
PID_North = (PID_North * (int32_t)dynamicParams.NaviGpsGain) / 100;
PID_East = (PID_East * (int32_t)dynamicParams.NaviGpsGain) / 100;
 
// GPS to nick and roll settings
 
// A positive nick angle moves head downwards (flying forward).
// A positive roll angle tilts left side downwards (flying left).
// If compass heading is 0 the head of the copter is in north direction.
// A positive nick angle will fly to north and a positive roll angle will fly to west.
// In case of a positive north deviation/velocity the
// copter should fly to south (negative nick).
// In case of a positive east position deviation and a positive east velocity the
// copter should fly to west (positive roll).
// The influence of the GPSStickNick and GPSStickRoll variable is contrarily to the stick values
// in the flight.c. Therefore a positive north deviation/velocity should result in a positive
// GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll.
 
coscompass = (int32_t)c_cos_8192(YawGyroHeading / GYRO_DEG_FACTOR);
sincompass = (int32_t)c_sin_8192(YawGyroHeading / GYRO_DEG_FACTOR);
PID_Nick = (coscompass * PID_North + sincompass * PID_East) / 8192;
PID_Roll = (sincompass * PID_North - coscompass * PID_East) / 8192;
 
 
// limit resulting GPS control vector
GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
 
GPSStickNick = (int16_t)PID_Nick;
GPSStickRoll = (int16_t)PID_Roll;
}
else // invalid GPS data or bad compass reading
{
GPS_Neutral(); // do nothing
// reset error integral
GPSPosDevIntegral_North = 0;
GPSPosDevIntegral_East = 0;
}
}
 
 
 
 
void GPS_Main(void)
{
static uint8_t GPS_P_Delay = 0;
static uint16_t beep_rythm = 0;
 
GPS_UpdateParameter();
 
// store home position if start of flight flag is set
if(MKFlags & MKFLAG_CALIBRATE)
{
if(GPS_SetCurrPosition(&HomePosition)) BeepTime = 700;
}
 
switch(GPSInfo.status)
{
case INVALID: // invalid gps data
GPS_Neutral();
if(FlightMode != GPS_FLIGHT_MODE_FREE)
{
BeepTime = 100; // beep if signal is neccesary
}
break;
case PROCESSED: // if gps data are already processed do nothing
// downcount timeout
if(GPSTimeout) GPSTimeout--;
// if no new data arrived within timeout set current data invalid
// and therefore disable GPS
else
{
GPS_Neutral();
GPSInfo.status = INVALID;
}
break;
case NEWDATA: // new valid data from gps device
// if the gps data quality is good
beep_rythm++;
 
if (GPS_IsSignalOK())
{
switch(FlightMode) // check what's to do
{
case GPS_FLIGHT_MODE_FREE:
// update hold position to current gps position
GPS_SetCurrPosition(&HoldPosition); // can get invalid if gps signal is bad
// disable gps control
GPS_Neutral();
break;
 
case GPS_FLIGHT_MODE_AID:
if(HoldPosition.Status != INVALID)
{
if( GPS_IsManualControlled() ) // MK controlled by user
{
// update hold point to current gps position
GPS_SetCurrPosition(&HoldPosition);
// disable gps control
GPS_Neutral();
GPS_P_Delay = 0;
}
else // GPS control active
{
if(GPS_P_Delay < 7)
{ // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
GPS_P_Delay++;
GPS_SetCurrPosition(&HoldPosition); // update hold point to current gps position
GPS_PIDController(NULL); // activates only the D-Part
}
else GPS_PIDController(&HoldPosition);// activates the P&D-Part
}
}
else // invalid Hold Position
{ // try to catch a valid hold position from gps data input
GPS_SetCurrPosition(&HoldPosition);
GPS_Neutral();
}
break;
 
case GPS_FLIGHT_MODE_HOME:
if(HomePosition.Status != INVALID)
{
// update hold point to current gps position
// to avoid a flight back if home comming is deactivated
GPS_SetCurrPosition(&HoldPosition);
if( GPS_IsManualControlled() ) // MK controlled by user
{
GPS_Neutral();
}
else // GPS control active
{
GPS_PIDController(&HomePosition);
}
}
else // bad home position
{
BeepTime = 50; // signal invalid home position
// try to hold at least the position as a fallback option
 
if (HoldPosition.Status != INVALID)
{
if( GPS_IsManualControlled() ) // MK controlled by user
{
GPS_Neutral();
}
else // GPS control active
{
GPS_PIDController(&HoldPosition);
}
}
else
{ // try to catch a valid hold position
GPS_SetCurrPosition(&HoldPosition);
GPS_Neutral();
}
}
break; // eof TSK_HOME
default: // unhandled task
GPS_Neutral();
break; // eof default
} // eof switch GPS_Task
} // eof gps data quality is good
else // gps data quality is bad
{ // disable gps control
GPS_Neutral();
if(FlightMode != GPS_FLIGHT_MODE_FREE)
{
// beep if signal is not sufficient
if(!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100;
else if (GPSInfo.satnum < staticParams.NaviGpsMinSat && !(beep_rythm % 5)) BeepTime = 10;
}
}
// set current data as processed to avoid further calculations on the same gps data
GPSInfo.status = PROCESSED;
break;
} // eof GPSInfo.status
}
 
/branches/dongfang_FC_rewrite/gps.h
0,0 → 1,9
#ifndef _GPS_H
#define _GPS_H
 
#include <inttypes.h>
 
extern void GPS_Main(void);
 
#endif //_GPS_H
 
/branches/dongfang_FC_rewrite/gyro.h
0,0 → 1,10
/*
* Common procedures for all gyro types: One to initialize them and one to calibrate them.
* Calibration would be something like:
* FC 1.3 hardware: Searching the DAC values that return neutral readings.
* FC 2.0 hardware: Nothing to do.
* InvenSense hardware: Output a pulse on the AUTO_ZERO line.
*/
 
void gyro_init(void);
void gyro_calibrate();
/branches/dongfang_FC_rewrite/invenSense.c
0,0 → 1,35
#include "invenSense.h"
#include "timer0.h"
#include "configuration.h"
 
#include <avr/io.h>
 
#define AUTOZERO_PORT PORTD
#define AUTOZERO_DDR DDRD
#define AUTOZERO_BIT 5
 
void gyro_calibrate() {
// If port not already set to output and high, do it.
if (!(AUTOZERO_DDR & (1<<AUTOZERO_BIT)) || !(AUTOZERO_PORT & (1<<AUTOZERO_BIT))) {
AUTOZERO_PORT |= (1<<AUTOZERO_BIT);
AUTOZERO_DDR |= (1<<AUTOZERO_BIT);
Delay_ms(100);
}
// Make a pulse on the auto-zero output line.
AUTOZERO_PORT &= ~(1<<AUTOZERO_BIT);
Delay_ms(1);
AUTOZERO_PORT |= (1<<AUTOZERO_BIT);
// Delay_ms(10);
Delay_ms_Mess(100);
}
 
void gyro_setDefaults(void) {
staticParams.GyroD = 3;
staticParams.GyroAccFactor = 1;
staticParams.DriftComp = 1;
 
// Not used.
staticParams.AngleTurnOverPitch = 85;
staticParams.AngleTurnOverRoll = 85;
}
/branches/dongfang_FC_rewrite/invenSense.h
0,0 → 1,41
#ifndef _INVENSENSE_H
#define _INVENSENSE_H
 
#include "sensors.h"
 
#define GYRO_HW_NAME "ISens"
 
/*
* Configuration for my prototype board with InvenSense gyros.
* The FC 1.3 board is installed upside down, therefore 2 acc. meters are reversed.
*/
#define ACC_REVERSE_ROLLAXIS yes
#define ACC_REVERSE_ZAXIS yes
 
/*
* The InvenSense gyros have a lower sensitivity than for example the ADXRS610s on the FC 2.0 ME,
* but they have a wider range too.
* 2mV/deg/s gyros and no amplifiers:
* H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
*/
#define GYRO_HW_FACTOR 0.6827f
 
/*
* Correction factor - determined experimentally: Hold the copter in the hand, and turn it 90 degrees.
* If AnglePitch or AngleRoll in debug in MK-Tool changes by x degrees, multiply the value here by x/90.
* If the hardware related contants are set correctly, flight should be OK without bothering to
* make any adjustments here. It is only for luxury.
*/
#define GYRO_PITCHROLL_CORRECTION 0.93f
 
/*
* Same for yaw.
*/
#define GYRO_YAW_CORRECTION 0.97f
 
/*
* Yaw axis is reverse.
*/
#define GYRO_REVERSE_YAW yes
 
#endif
/branches/dongfang_FC_rewrite/main.c
0,0 → 1,295
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <avr/boot.h>
 
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
 
#include "timer0.h"
#include "timer2.h"
#include "uart0.h"
#include "uart1.h"
#include "output.h"
#include "menu.h"
#include "attitude.h"
#include "flight.h"
#include "controlMixer.h"
#include "rc.h"
#include "analog.h"
#include "configuration.h"
#include "printf_P.h"
#include "twimaster.h"
#ifdef USE_NAVICTRL
#include "spi.h"
#endif
#ifdef USE_MK3MAG
#include "mk3mag.h"
#endif
#include "eeprom.h"
 
int16_t main (void) {
uint16_t timer;
 
// disable interrupts global
cli();
 
// analyze hardware environment
CPUType = getCPUType();
BoardRelease = getBoardRelease();
 
// disable watchdog
MCUSR &=~(1<<WDRF);
WDTCSR |= (1<<WDCE)|(1<<WDE);
WDTCSR = 0;
 
// PPM_in[CH_THROTTLE] = 0;
// Why??? They are already initialized to 0.
// stickPitch = stickRoll = stickYaw = 0;
 
RED_OFF;
 
// initalize modules
output_init();
timer0_init();
timer2_init();
USART0_Init();
if(CPUType == ATMEGA644P) USART1_Init();
RC_Init();
analog_init();
I2C_init();
#ifdef USE_NAVICTRL
SPI_MasterInit();
#endif
#ifdef USE_MK3MAG
MK3MAG_Init();
#endif
// enable interrupts global
sei();
 
printf("\n\r===================================");
printf("\n\rFlightControl");
printf("\n\rHardware: Custom");
printf("\r\n CPU: Atmega644");
if(CPUType == ATMEGA644P)
printf("p");
printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
printf("\n\r===================================");
 
// Parameter Set handling
ParamSet_Init();
 
// wait for a short time (otherwise the RC channel check won't work below)
// timer = SetDelay(500);
// while(!CheckDelay(timer));
// Instead, while away the time by flashing the 2 outputs:
// First J16, then J17. Makes it easier to see which is which.
 
timer = SetDelay(200);
J16_ON;
GRN_OFF;
RED_ON;
while(!CheckDelay(timer));
 
timer = SetDelay(200);
J16_OFF;
RED_OFF;
GRN_ON;
J17_ON;
while(!CheckDelay(timer));
 
timer = SetDelay(200);
while(!CheckDelay(timer));
J17_OFF;
 
twi_diagnostics();
printf("\n\r===================================");
 
/*
if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL)
{
printf("\n\rCalibrating air pressure sensor..");
timer = SetDelay(1000);
SearchAirPressureOffset();
while (!CheckDelay(timer));
printf("OK\n\r");
}
*/
 
#ifdef USE_NAVICTRL
printf("\n\rSupport for NaviCtrl");
#ifdef USE_RC_DSL
printf("\r\nSupport for DSL RC at 2nd UART");
#endif
#ifdef USE_RC_SPECTRUM
printf("\r\nSupport for SPECTRUM RC at 2nd UART");
#endif
#endif
 
#ifdef USE_MK3MAG
printf("\n\rSupport for MK3MAG Compass");
#endif
 
#if (defined (USE_MK3MAG))
if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART");
else printf("\n\rSupport for GPS at 1st UART");
#endif
 
controlMixer_setNeutral(0);
 
// Cal. attitude sensors and reset integrals.
attitude_setNeutral();
 
Servo_On();
 
// Init flight parameters
flight_setNeutral();
 
RED_OFF;
 
beep(2000);
printf("\n\rControl: ");
if (staticParams.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");
else printf("Neutral (ACC-Mode)");
 
printf("\n\n\r");
 
LCD_Clear();
 
I2CTimeout = 5000;
 
while (1) {
if(runFlightControl && analogDataReady) { // control interval
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
J4HIGH;
flight_control();
J4LOW;
sendMotorData(); // the flight control code
RED_OFF;
/*
Does not belong here. Instead, external control should be ignored in
controlMixer if there was no new data from there for some time.
if(externalControlActive) externalControlActive--;
else {
externalControl.config = 0;
externalStickPitch = 0;
externalStickRoll = 0;
externalStickYaw = 0;
}
*/
/*
Does not belong here.
if(RC_Quality) RC_Quality--;
*/
/* Does not belong here. Well since we are not supporting navi right now anyway, leave out.
#ifdef USE_NAVICTRL
if(NCDataOkay) {
if(--NCDataOkay == 0) // no data from NC
{ // set gps control sticks neutral
GPSStickPitch = 0;
GPSStickRoll = 0;
NCSerialDataOkay = 0;
}
}
#endif
*/
if(!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout
RED_ON;
if(!I2CTimeout) {
I2C_Reset();
I2CTimeout = 5;
}
} else {
RED_OFF;
}
// allow Serial Data Transmit if motors must not updated or motors are not running
// Why only when that???
if( !runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {
USART0_TransmitTxData();
}
USART0_ProcessRxData();
if(CheckDelay(timer)) {
if(UBat < staticParams.LowVoltageWarning) {
beepBatteryAlarm();
}
#ifdef USE_NAVICTRL
SPI_StartTransmitPacket();
SendSPI = 4;
#endif
timer = SetDelay(20); // every 20 ms
}
output_update();
}
#ifdef USE_NAVICTRL
if(!SendSPI) {
// SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
// within the SPI_TransmitByte() routine the value is set to 4.
// I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
// and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
SPI_TransmitByte();
}
#endif
}
return (1);
}
/branches/dongfang_FC_rewrite/main.h
0,0 → 1,3
#ifndef _MAIN_H
#define _MAIN_H
#endif //_MAIN_H
/branches/dongfang_FC_rewrite/makefile
0,0 → 1,502
#--------------------------------------------------------------------
# MCU name
MCU = atmega644p
F_CPU = 20000000
#-------------------------------------------------------------------
VERSION_MAJOR = 0
VERSION_MINOR = 74
VERSION_PATCH = 3
 
VERSION_SERIAL_MAJOR = 10 # Serial Protocol Major Version
VERSION_SERIAL_MINOR = 1 # Serial Protocol Minor Version
NC_SPI_COMPATIBLE = 6 # SPI Protocol Version
 
#-------------------------------------------------------------------
#OPTIONS
 
# Use one of the extensions for a gps solution
EXT = NAVICTRL
#EXT = MK3MAG
 
# Use optional one the RCs if EXT = NAVICTRL has been used
RC = DSL
#RC = SPECTRUM
 
GYRO=ENC-03_FC1.3
#GYRO=ADXRS610_FC2.0
#GYRO=invenSense
 
#-------------------------------------------------------------------
# get SVN revision
REV=0001
#$(shell sh -c "cat .svn/entries | sed -n '4p'")
 
ifeq ($(MCU), atmega644)
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m
HEX_NAME = MEGA644_$(EXT)_$(RC)_$(GYRO)
endif
 
ifeq ($(MCU), atmega644p)
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m
HEX_NAME = MEGA644p_$(EXT)_$(RC)_$(GYRO)
endif
 
 
ifeq ($(F_CPU), 16000000)
QUARZ = 16MHZ
endif
 
ifeq ($(F_CPU), 20000000)
QUARZ = 20MHZ
endif
 
 
# Output format. (can be srec, ihex, binary)
FORMAT = ihex
 
# Target file name (without extension).
 
ifeq ($(VERSION_PATCH), 0)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)a_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 1)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)b_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 2)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)c_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 3)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)d_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 4)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)e_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 5)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)f_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 6)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)g_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 7)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)h_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 8)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)i_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 9)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)j_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 10)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)k_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 11)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)l_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 12)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)m_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 13)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)n_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 14)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)o_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 15)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)p_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 16)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)q_SVN$(REV)
endif
ifeq ($(VERSION_PATCH), 17)
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)r_SVN$(REV)
endif
 
# Optimization level, can be [0, 1, 2, 3, s]. 0 turns off optimization.
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
OPT = 2
#OPT = s
 
##########################################################################################################
# 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 externalControl.c GPSControl.c
SRC += dongfangMath.c twimaster.c rc.c attitude.c flight.c eeprom.c uart1.c configuration.c $(GYRO).c
 
ifeq ($(EXT), MK3MAG)
SRC += mk3mag.c mymath.c gps.c ubx.c
endif
ifeq ($(EXT), NAVICTRL)
SRC += spi.c
ifeq ($(RC), DSL)
SRC += dsl.c
endif
ifeq ($(RC), SPECTRUM)
SRC += spectrum.c
endif
endif
##########################################################################################################
 
# List Assembler source files here.
# Make them always end in a capital .S. Files ending in a lowercase .s
# will not be considered source files but generated files (assembler
# output from the compiler), and will be deleted upon "make clean"!
# Even though the DOS/Win* filesystem matches both .s and .S the same,
# it will preserve the spelling of the filenames, and gcc itself does
# care about how the name is spelled on its command-line.
ASRC =
 
# List any extra directories to look for include files here.
# Each directory must be seperated by a space.
EXTRAINCDIRS =
 
 
# Optional compiler flags.
# -g: generate debugging information (for GDB, or for COFF conversion)
# -O*: optimization level
# -f...: tuning, see gcc manual and avr-libc documentation
# -Wall...: warning level
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlms: create assembler listing
CFLAGS = -O$(OPT) \
-funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums \
-Wall -Wstrict-prototypes \
-DGYRO=$(GYRO) \
-Wa,-adhlns=$(<:.c=.lst) \
$(patsubst %,-I%,$(EXTRAINCDIRS))
 
 
# Set a "language standard" compiler flag.
# Unremark just one line below to set the language standard to use.
# gnu99 = C99 + GNU extensions. See GCC manual for more information.
#CFLAGS += -std=c89
#CFLAGS += -std=gnu89
#CFLAGS += -std=c99
CFLAGS += -std=gnu99
 
CFLAGS += -DF_CPU=$(F_CPU) -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_PATCH=$(VERSION_PATCH) -DVERSION_SERIAL_MAJOR=$(VERSION_SERIAL_MAJOR) -DVERSION_SERIAL_MINOR=$(VERSION_SERIAL_MINOR) -DNC_SPI_COMPATIBLE=$(NC_SPI_COMPATIBLE)
 
ifeq ($(EXT), MK3MAG)
CFLAGS += -DUSE_MK3MAG
endif
ifeq ($(EXT), NAVICTRL)
CFLAGS += -DUSE_NAVICTRL
ifeq ($(RC), DSL)
CFLAGS += -DUSE_RC_DSL
endif
ifeq ($(RC), SPECTRUM)
CFLAGS += -DUSE_RC_SPECTRUM
endif
endif
 
ifeq ($(SETUP), QUADRO)
CFLAGS += -DUSE_QUADRO
endif
ifeq ($(SETUP), OCTO)
CFLAGS += -DUSE_OCTO
endif
ifeq ($(SETUP), OCTO2)
CFLAGS += -DUSE_OCTO2
endif
ifeq ($(SETUP), OCTO3)
CFLAGS += -DUSE_OCTO3
endif
 
 
# Optional assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlms: create listing
# -gstabs: have the assembler create line number information; note that
# for use in COFF files, additional information about filenames
# and function names needs to be present in the assembler source
# files -- see avr-libc docs [FIXME: not yet described there]
ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs
 
 
 
# Optional linker flags.
# -Wl,...: tell GCC to pass this to linker.
# -Map: create map file
# --cref: add cross reference to map file
LDFLAGS = -Wl,-Map=$(TARGET).map,--cref
 
# Additional libraries
 
# Minimalistic printf version
#LDFLAGS += -Wl,-u,vfprintf -lprintf_min
 
# Floating point printf version (requires -lm below)
#LDFLAGS += -Wl,-u,vfprintf -lprintf_flt
 
# -lm = math library
LDFLAGS += -lm
 
 
##LDFLAGS += -T./linkerfile/avr5.x
 
 
 
# Programming support using avrdude. Settings and variables.
 
# Programming hardware: alf avr910 avrisp bascom bsd
# dt006 pavr picoweb pony-stk200 sp12 stk200 stk500
#
# Type: avrdude -c ?
# to get a full listing.
#
#AVRDUDE_PROGRAMMER = dt006
#AVRDUDE_PROGRAMMER = stk200
#AVRDUDE_PROGRAMMER = ponyser
AVRDUDE_PROGRAMMER = avrispv2
#falls Ponyser ausgewählt wird, muss sich unsere avrdude-Configdatei im Bin-Verzeichnis des Compilers befinden
 
#AVRDUDE_PORT = com1 # programmer connected to serial device
#AVRDUDE_PORT = lpt1 # programmer connected to parallel port
AVRDUDE_PORT = usb # programmer connected to USB
 
#AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex
AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex $(FUSE_SETTINGS)
#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep
 
#avrdude -c avrispv2 -P usb -p m32 -U flash:w:blink.hex
AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER)
 
# Uncomment the following if you want avrdude's erase cycle counter.
# Note that this counter needs to be initialized first using -Yn,
# see avrdude manual.
#AVRDUDE_ERASE += -y
 
# Uncomment the following if you do /not/ wish a verification to be
# performed after programming the device.
AVRDUDE_FLAGS += -V
 
# Increase verbosity level. Please use this when submitting bug
# reports about avrdude. See <http://savannah.nongnu.org/projects/avrdude>
# to submit bug reports.
#AVRDUDE_FLAGS += -v -v
 
# ---------------------------------------------------------------------------
# Define directories, if needed.
DIRAVR = c:/winavr
DIRAVRBIN = $(DIRAVR)/bin
DIRAVRUTILS = $(DIRAVR)/utils/bin
DIRINC = .
DIRLIB = $(DIRAVR)/avr/lib
 
 
# Define programs and commands.
SHELL = sh
 
CC = avr-gcc
 
OBJCOPY = avr-objcopy
OBJDUMP = avr-objdump
SIZE = avr-size
 
# Programming support using avrdude.
AVRDUDE = avrdude
 
REMOVE = rm -f
COPY = cp
 
HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex
ELFSIZE = $(SIZE) -A $(TARGET).elf
 
# Define Messages
# English
MSG_ERRORS_NONE = Errors: none
MSG_BEGIN = -------- begin --------
MSG_END = -------- end --------
MSG_SIZE_BEFORE = Size before:
MSG_SIZE_AFTER = Size after:
MSG_COFF = Converting to AVR COFF:
MSG_EXTENDED_COFF = Converting to AVR Extended COFF:
MSG_FLASH = Creating load file for Flash:
MSG_EEPROM = Creating load file for EEPROM:
MSG_EXTENDED_LISTING = Creating Extended Listing:
MSG_SYMBOL_TABLE = Creating Symbol Table:
MSG_LINKING = Linking:
MSG_COMPILING = Compiling:
MSG_ASSEMBLING = Assembling:
MSG_CLEANING = Cleaning project:
 
 
# Define all object files.
OBJ = $(SRC:.c=.o) $(ASRC:.S=.o)
 
# Define all listing files.
LST = $(ASRC:.S=.lst) $(SRC:.c=.lst)
 
# Combine all necessary flags and optional flags.
# Add target processor to flags.
#ALL_CFLAGS = -mmcu=$(MCU) -DF_CPU=$(F_CPU) -I. $(CFLAGS)
ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS)
ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS)
 
 
# Default target.
all: begin gccversion sizebefore $(TARGET).elf $(TARGET).hex $(TARGET).eep \
$(TARGET).lss $(TARGET).sym sizeafter finished end
 
 
# Eye candy.
# AVR Studio 3.x does not check make's exit code but relies on
# the following magic strings to be generated by the compile job.
begin:
@echo
@echo $(MSG_BEGIN)
 
finished:
@echo $(MSG_ERRORS_NONE)
 
end:
@echo $(MSG_END)
@echo
 
 
# Display size of file.
sizebefore:
@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi
 
sizeafter:
@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
 
 
 
# Display compiler version information.
gccversion :
@$(CC) --version
 
 
# Convert ELF to COFF for use in debugging / simulating in
# AVR Studio or VMLAB.
COFFCONVERT=$(OBJCOPY) --debugging \
--change-section-address .data-0x800000 \
--change-section-address .bss-0x800000 \
--change-section-address .noinit-0x800000 \
--change-section-address .eeprom-0x810000
 
 
coff: $(TARGET).elf
@echo
@echo $(MSG_COFF) $(TARGET).cof
$(COFFCONVERT) -O coff-avr $< $(TARGET).cof
 
 
extcoff: $(TARGET).elf
@echo
@echo $(MSG_EXTENDED_COFF) $(TARGET).cof
$(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof
 
 
 
 
# Program the device.
program: $(TARGET).hex $(TARGET).eep
$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM)
 
 
 
 
# Create final output files (.hex, .eep) from ELF output file.
%.hex: %.elf
@echo
@echo $(MSG_FLASH) $@
$(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@
 
%.eep: %.elf
@echo
@echo $(MSG_EEPROM) $@
-$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \
--change-section-lma .eeprom=0 -O $(FORMAT) $< $@
 
# Create extended listing file from ELF output file.
%.lss: %.elf
@echo
@echo $(MSG_EXTENDED_LISTING) $@
$(OBJDUMP) -h -S $< > $@
 
# Create a symbol table from ELF output file.
%.sym: %.elf
@echo
@echo $(MSG_SYMBOL_TABLE) $@
avr-nm -n $< > $@
 
 
 
# Link: create ELF output file from object files.
.SECONDARY : $(TARGET).elf
.PRECIOUS : $(OBJ)
%.elf: $(OBJ)
@echo
@echo $(MSG_LINKING) $@
$(CC) $(ALL_CFLAGS) $(OBJ) --output $@ $(LDFLAGS)
 
 
# Compile: create object files from C source files.
%.o : %.c
@echo
@echo $(MSG_COMPILING) $<
$(CC) -c $(ALL_CFLAGS) $< -o $@
 
 
# Compile: create assembler files from C source files.
%.s : %.c
$(CC) -S $(ALL_CFLAGS) $< -o $@
 
 
# Assemble: create object files from assembler source files.
%.o : %.S
@echo
@echo $(MSG_ASSEMBLING) $<
$(CC) -c $(ALL_ASFLAGS) $< -o $@
 
 
 
 
 
 
# Target: clean project.
clean: begin clean_list finished end
 
clean_list :
@echo
@echo $(MSG_CLEANING)
# $(REMOVE) $(TARGET).hex
$(REMOVE) $(TARGET).eep
$(REMOVE) $(TARGET).obj
$(REMOVE) $(TARGET).cof
$(REMOVE) $(TARGET).elf
$(REMOVE) $(TARGET).map
$(REMOVE) $(TARGET).obj
$(REMOVE) $(TARGET).a90
$(REMOVE) $(TARGET).sym
$(REMOVE) $(TARGET).lnk
$(REMOVE) $(TARGET).lss
$(REMOVE) $(OBJ)
$(REMOVE) $(LST)
$(REMOVE) $(SRC:.c=.s)
$(REMOVE) $(SRC:.c=.d)
 
 
# Automatically generate C source code dependencies.
# (Code originally taken from the GNU make user manual and modified
# (See README.txt Credits).)
#
# Note that this will work with sh (bash) and sed that is shipped with WinAVR
# (see the SHELL variable defined above).
# This may not work with other shells or other seds.
#
%.d: %.c
set -e; $(CC) -MM $(ALL_CFLAGS) $< \
| sed 's,\(.*\)\.o[ :]*,\1.o \1.d : ,g' > $@; \
[ -s $@ ] || rm -f $@
 
 
# Remove the '-' if you want to see the dependency files generated.
-include $(SRC:.c=.d)
 
 
 
# Listing of phony targets.
.PHONY : all begin finish end sizebefore sizeafter gccversion coff extcoff \
clean clean_list program
 
/branches/dongfang_FC_rewrite/menu.c
0,0 → 1,291
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <inttypes.h>
#include "eeprom.h"
#include "timer2.h"
#include "rc.h"
#include "externalControl.h"
#include "uart0.h"
#include "printf_P.h"
#include "analog.h"
#include "twimaster.h"
#include "attitude.h"
 
#if (defined (USE_MK3MAG))
#include "ubx.h"
#endif
 
#if (!defined (USE_MK3MAG))
uint8_t MaxMenuItem = 13;
#else
#ifdef USE_MK3MAG
uint8_t MaxMenuItem = 14;
#endif
#endif
uint8_t MenuItem = 0;
uint8_t RemoteKeys = 0;
 
#define KEY1 0x01
#define KEY2 0x02
#define KEY3 0x04
#define KEY4 0x08
#define KEY5 0x10
 
#define DISPLAYBUFFSIZE 80
int8_t DisplayBuff[DISPLAYBUFFSIZE] = "Hello World";
uint8_t DispPtr = 0;
 
 
/************************************/
/* Clear LCD Buffer */
/************************************/
void LCD_Clear(void)
{
uint8_t i;
for( i = 0; i < DISPLAYBUFFSIZE; i++) DisplayBuff[i] = ' ';
}
 
 
/************************************/
/* Update Menu on LCD */
/************************************/
// Display with 20 characters in 4 lines
void LCD_PrintMenu(void) {
if(RemoteKeys & KEY1) {
if(MenuItem) MenuItem--;
else MenuItem = MaxMenuItem;
}
 
if(RemoteKeys & KEY2) {
if(MenuItem == MaxMenuItem) MenuItem = 0;
else MenuItem++;
}
if((RemoteKeys & KEY1) && (RemoteKeys & KEY2)) MenuItem = 0;
LCD_Clear();
if(MenuItem > MaxMenuItem) MenuItem = MaxMenuItem;
// print menu item number in the upper right corner
if(MenuItem < 10) {
LCD_printfxy(17,0,"[%i]",MenuItem);
} else {
LCD_printfxy(16,0,"[%i]",MenuItem);
}
switch(MenuItem) {
case 0:// Version Info Menu Item
LCD_printfxy(0,0,"+ MikroKopter +");
LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d%c",BoardRelease/10,BoardRelease%10,VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH+'a');
LCD_printfxy(0,2,"Setting: %d %s", getActiveParamSet(), Mixer.Name);
if(I2CTimeout < 6) {
LCD_printfxy(0,3,"I2C Error!!!");
} else if (missingMotor) {
LCD_printfxy(0,3,"Missing BL-Ctrl:%d", missingMotor);
}
else LCD_printfxy(0,3,"(c) Holger Buss");
break;
/*
case 1:// Height Control Menu Item
if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) {
LCD_printfxy(0,0,"Height: %5i",ReadingHeight);
LCD_printfxy(0,1,"Set Point: %5i",SetPointHeight);
LCD_printfxy(0,2,"Air Press.:%5i",0);
LCD_printfxy(0,3,"Offset :%5i",0);
}
else
{
LCD_printfxy(0,1,"No ");
LCD_printfxy(0,2,"Height Control");
}
break;
*/
case 2:// Attitude Menu Item
LCD_printfxy(0,0,"Attitude");
LCD_printfxy(0,1,"Nick: %5i", pitchAngle / GYRO_DEG_FACTOR_PITCHROLL);
LCD_printfxy(0,2,"Roll: %5i", pitchAngle / GYRO_DEG_FACTOR_PITCHROLL);
LCD_printfxy(0,3,"Heading: %5i", compassHeading);
break;
case 3:// Remote Control Channel Menu Item
LCD_printfxy(0,0,"C1:%4i C2:%4i ",PPM_in[1],PPM_in[2]);
LCD_printfxy(0,1,"C3:%4i C4:%4i ",PPM_in[3],PPM_in[4]);
LCD_printfxy(0,2,"C5:%4i C6:%4i ",PPM_in[5],PPM_in[6]);
LCD_printfxy(0,3,"C7:%4i C8:%4i ",PPM_in[7],PPM_in[8]);
break;
case 4:// Remote Control Mapping Menu Item
LCD_printfxy(0,0,"Ni:%4i Ro:%4i ",PPM_in[staticParams.ChannelAssignment[CH_PITCH]],PPM_in[staticParams.ChannelAssignment[CH_ROLL]]);
LCD_printfxy(0,1,"Gs:%4i Ya:%4i ",PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]],PPM_in[staticParams.ChannelAssignment[CH_YAW]]);
LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[staticParams.ChannelAssignment[CH_POTS]],PPM_in[staticParams.ChannelAssignment[CH_POTS+1]]);
LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[staticParams.ChannelAssignment[CH_POTS+2]],PPM_in[staticParams.ChannelAssignment[CH_POTS+3]]);
break;
/*
case 5:// Gyro Sensor Menu Item
LCD_printfxy(0,0,"Gyro - Sensor");
switch(BoardRelease) {
case 10:
LCD_printfxy(0,1,"Nick %4i (%3i.%i)", AdValueGyroNick - HiResNickOffset / HIRES_GYRO_AMPLIFY, HiResNickOffset / HIRES_GYRO_AMPLIFY, HiResNickOffset % HIRES_GYRO_AMPLIFY);
LCD_printfxy(0,2,"Roll %4i (%3i.%i)", AdValueGyroRoll - HiResRollOffset / HIRES_GYRO_AMPLIFY, HiResRollOffset / HIRES_GYRO_AMPLIFY, HiResRollOffset % HIRES_GYRO_AMPLIFY);
LCD_printfxy(0,3,"Yaw %4i (%3i)", AdValueGyroYaw , YawOffset);
break;
case 11:
case 12:
case 20: // divice Offests by 2 becuse 2 samples are added in adc isr
LCD_printfxy(0,1,"Nick %4i (%3i.%i)",0, HiResNickOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResNickOffset % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7)
LCD_printfxy(0,2,"Roll %4i (%3i.%i)",0, HiResRollOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResRollOffset % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7)
LCD_printfxy(0,3,"Yaw %4i (%3i)",YawOffset - AdValueGyroYaw , YawOffset/2);
break;
case 13:
default: // divice Offests by 2 becuse 2 samples are added in adc isr
LCD_printfxy(0,1,"Nick %4i (%3i.%i)(%3i)",0, HiResNickOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResNickOffset % (HIRES_GYRO_AMPLIFY * 2))/2, 0); // division by 2 to push the reminder below 10 (15/2 = 7)
LCD_printfxy(0,2,"Roll %4i (%3i.%i)(%3i)",0, HiResRollOffset / (HIRES_GYRO_AMPLIFY * 2), (HiResRollOffset % (HIRES_GYRO_AMPLIFY * 2))/2, 0); // division by 2 to push the reminder below 10 (15/2 = 7)
LCD_printfxy(0,3,"Yaw %4i (%3i)(%3i)",YawOffset - AdValueGyroYaw , YawOffset/2, 0);
break;
}
break;
case 6:// Acceleration Sensor Menu Item
LCD_printfxy(0,0,"ACC - Sensor");
LCD_printfxy(0,1,"Nick %4i (%3i)",0,0); // factor 2 because of adding 2 samples in ADC ISR
LCD_printfxy(0,2,"Roll %4i (%3i)",0,0); // factor 2 because of adding 2 samples in ADC ISR
LCD_printfxy(0,3,"Height %4i (%3i)",0,0);
break;
*/
case 7:// Battery Voltage / Remote Control Level
LCD_printfxy(0,1,"Voltage: %3i.%1iV",UBat/10, UBat%10);
LCD_printfxy(0,2,"RC-Level: %5i",RC_Quality);
break;
case 8:// Compass Menu Item
LCD_printfxy(0,0,"Compass ");
LCD_printfxy(0,1,"Course: %5i", compassCourse);
LCD_printfxy(0,2,"Heading: %5i", compassHeading);
LCD_printfxy(0,3,"OffCourse: %5i", compassOffCourse);
break;
case 9:// Poti Menu Item
LCD_printfxy(0,0,"Po1: %3i Po5: %3i" ,variables[0], variables[4]); //PPM24-Extesion
LCD_printfxy(0,1,"Po2: %3i Po6: %3i" ,variables[1], variables[5]); //PPM24-Extesion
LCD_printfxy(0,2,"Po3: %3i Po7: %3i" ,variables[2], variables[6]); //PPM24-Extesion
LCD_printfxy(0,3,"Po4: %3i Po8: %3i" ,variables[3], variables[7]); //PPM24-Extesion
break;
/*
case 10:// Servo Menu Item
LCD_printfxy(0,0,"Servo " );
LCD_printfxy(0,1,"Setpoint %3i",dynamicParams.ServoNickControl);
LCD_printfxy(0,2,"Position: %3i",ServoNickValue);
LCD_printfxy(0,3,"Range:%3i-%3i",staticParams.ServoNickMin, staticParams.ServoNickMax);
break;
*/
case 11://Extern Control
LCD_printfxy(0,0,"ExternControl " );
LCD_printfxy(0,1,"Ni:%4i Ro:%4i ", externalControl.pitch, externalControl.roll);
LCD_printfxy(0,2,"Gs:%4i Ya:%4i ", externalControl.throttle, externalControl.yaw);
LCD_printfxy(0,3,"Hi:%4i Cf:%4i ", externalControl.height, externalControl.config);
break;
case 12://BL Communication errors
LCD_printfxy(0,0,"BL-Ctrl Errors " );
LCD_printfxy(0,1," %3d %3d %3d %3d ",Motor[0].Error,Motor[1].Error,Motor[2].Error,Motor[3].Error);
LCD_printfxy(0,2," %3d %3d %3d %3d ",Motor[4].Error,Motor[5].Error,Motor[6].Error,Motor[7].Error);
LCD_printfxy(0,3," %3d %3d %3d %3d ",Motor[8].Error,Motor[9].Error,Motor[10].Error,Motor[11].Error);
break;
case 13://BL Overview
LCD_printfxy(0,0,"BL-Ctrl found " );
LCD_printfxy(0,1," %c %c %c %c ",Motor[0].Present + '-',Motor[1].Present + '-',Motor[2].Present + '-',Motor[3].Present + '-');
LCD_printfxy(0,2," %c %c %c %c ",Motor[4].Present + '-',Motor[5].Present + '-',Motor[6].Present + '-',Motor[7].Present + '-');
LCD_printfxy(0,3," %c - - - ",Motor[8].Present + '-');
if(Motor[9].Present) LCD_printfxy(4,3,"10");
if(Motor[10].Present) LCD_printfxy(8,3,"11");
if(Motor[11].Present) LCD_printfxy(12,3,"12");
break;
#if (defined (USE_MK3MAG))
case 14://GPS Lat/Lon coords
if (GPSInfo.status == INVALID) {
LCD_printfxy(0,0,"No GPS data!");
} else {
switch (GPSInfo.satfix)
{
case SATFIX_NONE:
LCD_printfxy(0,0,"Sats: %d Fix: No", GPSInfo.satnum);
break;
case SATFIX_2D:
LCD_printfxy(0,0,"Sats: %d Fix: 2D", GPSInfo.satnum);
break;
case SATFIX_3D:
LCD_printfxy(0,0,"Sats: %d Fix: 3D", GPSInfo.satnum);
break;
default:
LCD_printfxy(0,0,"Sats: %d Fix: ??", GPSInfo.satnum);
break;
}
int16_t i1,i2,i3;
i1 = (int16_t)(GPSInfo.longitude/10000000L);
i2 = abs((int16_t)((GPSInfo.longitude%10000000L)/10000L));
i3 = abs((int16_t)(((GPSInfo.longitude%10000000L)%10000L)/10L));
LCD_printfxy(0,1,"Lon: %d.%03d%03d deg",i1, i2, i3);
i1 = (int16_t)(GPSInfo.latitude/10000000L);
i2 = abs((int16_t)((GPSInfo.latitude%10000000L)/10000L));
i3 = abs((int16_t)(((GPSInfo.latitude%10000000L)%10000L)/10L));
LCD_printfxy(0,2,"Lat: %d.%03d%03d deg",i1, i2, i3);
i1 = (int16_t)(GPSInfo.altitude/1000L);
i2 = abs((int16_t)(GPSInfo.altitude%1000L));
LCD_printfxy(0,3,"Alt: %d.%03d m",i1, i2);
}
break;
#endif
default:
MaxMenuItem = MenuItem - 1;
MenuItem = 0;
break;
}
RemoteKeys = 0;
}
/branches/dongfang_FC_rewrite/menu.h
0,0 → 1,17
#ifndef _MENU_H
#define _MENU_H
 
#include <inttypes.h>
 
#define DISPLAYBUFFSIZE 80
 
extern void LCD_PrintMenu(void);
extern void LCD_Clear(void);
extern int8_t DisplayBuff[DISPLAYBUFFSIZE];
extern uint8_t DispPtr;
extern uint8_t MenuItem;
extern uint8_t MaxMenuItem;
extern uint8_t RemoteKeys;
#endif //_MENU_H
 
 
/branches/dongfang_FC_rewrite/mk3mag.c
0,0 → 1,130
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <avr/io.h>
#include <stdlib.h>
#include <inttypes.h>
#include "timer0.h"
#include "rc.h"
#include "eeprom.h"
#include "mk3mag.h"
 
uint8_t PWMTimeout = 12;
ToMk3Mag_t ToMk3Mag;
 
 
/*********************************************/
/* Initialize Interface to MK3MAG Compass */
/*********************************************/
void MK3MAG_Init(void)
{
// Port PC4 connected to PWM output from compass module
DDRC &= ~(1<<DDC4); // set as input
PORTC |= (1<<PORTC4); // pull up to increase PWM counter also if nothing is connected
 
PWMTimeout = 0;
 
ToMk3Mag.CalState = 0;
ToMk3Mag.Orientation = 1;
}
 
 
/*********************************************/
/* Get PWM from MK3MAG */
/*********************************************/
void MK3MAG_Update(void) // called every 102.4 us by timer 0 ISR
{
static uint16_t PWMCount = 0;
static uint16_t BeepDelay = 0;
// The pulse width varies from 1ms (0°) to 36.99ms (359.9°)
// in other words 100us/° with a +1ms offset.
// The signal goes low for 65ms between pulses,
// so the cycle time is 65mS + the pulse width.
 
// pwm is high
 
if(PINC & (1<<PINC4))
{ // If PWM signal is high increment PWM high counter
// This counter is incremented by a periode of 102.4us,
// i.e. the resoluton of pwm coded heading is approx. 1 deg.
PWMCount++;
// pwm overflow?
if (PWMCount > 400)
{
if(PWMTimeout) PWMTimeout--; // decrement timeout
CompassHeading = -1; // unknown heading
PWMCount = 0; // reset PWM Counter
}
 
}
else // pwm is low
{ // ignore pwm values values of 0 and higher than 37 ms;
if((PWMCount) && (PWMCount < 362)) // 362 * 102.4us = 37.0688 ms
{
if(PWMCount <10) CompassHeading = 0;
else CompassHeading = ((uint32_t)(PWMCount - 10) * 1049L)/1024; // correct timebase and offset
CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid
// 12 * 362 counts * 102.4 us
}
PWMCount = 0; // reset pwm counter
}
if(!PWMTimeout)
{
if(CheckDelay(BeepDelay))
{
if(!BeepTime) BeepTime = 100; // make noise with 10Hz to signal the compass problem
BeepDelay = SetDelay(100);
}
}
}
 
 
 
/branches/dongfang_FC_rewrite/mk3mag.h
0,0 → 1,20
#ifndef _MK3MAG_H
#define _MK3MAG_H
 
typedef struct {
int16_t Attitude[2];
uint8_t UserParam[2];
uint8_t CalState;
uint8_t Orientation;
} ToMk3Mag_t;
 
extern ToMk3Mag_t ToMk3Mag;
 
// Initialization
void MK3MAG_Init(void);
 
// should be called cyclic to get actual compass heading
void MK3MAG_Update(void);
 
#endif //_MK3MAG_H
 
/branches/dongfang_FC_rewrite/mm3.c
0,0 → 1,532
/*
 
Copyright 2008, by Killagreg
 
This program (files mm3.c and mm3.h) is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation;
either version 3 of the License, or (at your option) any later version.
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
 
Please note: The original implementation was done by Niklas Nold.
All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de
*/
#include <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <inttypes.h>
 
#include "mm3.h"
#include "mymath.h"
#include "timer0.h"
#include "rc.h"
#include "eeprom.h"
#include "printf_P.h"
 
 
// for compatibility reasons gcc3.x <-> gcc4.x
#ifndef SPCR
#define SPCR SPCR0
#endif
#ifndef SPIE
#define SPIE SPIE0
#endif
#ifndef SPE
#define SPE SPE0
#endif
#ifndef DORD
#define DORD DORD0
#endif
#ifndef MSTR
#define MSTR MSTR0
#endif
#ifndef CPOL
#define CPOL CPOL0
#endif
#ifndef CPHA
#define CPHA CPHA0
#endif
#ifndef SPR1
#define SPR1 SPR01
#endif
#ifndef SPR0
#define SPR0 SPR00
#endif
 
#ifndef SPDR
#define SPDR SPDR0
#endif
 
#ifndef SPSR
#define SPSR SPSR0
#endif
#ifndef SPIF
#define SPIF SPIF0
#endif
#ifndef WCOL
#define WCOL WCOL0
#endif
#ifndef SPI2X
#define SPI2X SPI2X0
#endif
// -------------------------
 
 
#define MAX_AXIS_VALUE 500
 
 
typedef struct
{
uint8_t STATE;
uint16_t DRDY;
uint8_t AXIS;
int16_t x_axis;
int16_t y_axis;
int16_t z_axis;
} MM3_working_t;
 
 
// MM3 State Machine
#define MM3_STATE_RESET 0
#define MM3_STATE_START_TRANSFER 1
#define MM3_STATE_WAIT_DRDY 2
#define MM3_STATE_DRDY 3
#define MM3_STATE_BYTE2 4
 
#define MM3_X_AXIS 0x01
#define MM3_Y_AXIS 0x02
#define MM3_Z_AXIS 0x03
 
 
#define MM3_PERIOD_32 0x00
#define MM3_PERIOD_64 0x10
#define MM3_PERIOD_128 0x20
#define MM3_PERIOD_256 0x30
#define MM3_PERIOD_512 0x40
#define MM3_PERIOD_1024 0x50
#define MM3_PERIOD_2048 0x60
#define MM3_PERIOD_4096 0x70
 
#if defined(USE_WALTER_EXT) // walthers board
// Output Pins (J9)PC6->MM3_SS ,(J8)PB2->MM3_RESET
#define MM3_SS_PORT PORTC //J9->MM3_SS
#define MM3_SS_DDR DDRC
#define MM3_SS_PIN PC6
#define MM3_RESET_PORT PORTB //J8->MM3_RESET
#define MM3_RESET_DDR DDRB
#define MM3_RESET_PIN PB2
#elif defined(USE_NICK666) // nick666 version 0.67g
#define MM3_SS_PORT PORTD //J5->MM3_SS
#define MM3_SS_DDR DDRD
#define MM3_SS_PIN PD3
#define MM3_RESET_PORT PORTB //J8->MM3_RESET
#define MM3_RESET_DDR DDRB
#define MM3_RESET_PIN PB2
#else // killagregs board
// Output Pins PC4->MM3_SS ,PC5->MM3_RESET
#define MM3_SS_PORT PORTC
#define MM3_SS_DDR DDRC
#define MM3_SS_PIN PC4
#define MM3_RESET_PORT PORTC
#define MM3_RESET_DDR DDRC
#define MM3_RESET_PIN PC5
#endif
 
#define MM3_SS_ON MM3_SS_PORT &= ~(1<<MM3_SS_PIN);
#define MM3_SS_OFF MM3_SS_PORT |= (1<<MM3_SS_PIN);
#define MM3_RESET_ON MM3_RESET_PORT |= (1<<MM3_RESET_PIN);
#define MM3_RESET_OFF MM3_RESET_PORT &= ~(1<<MM3_RESET_PIN);
 
 
 
MM3_calib_t MM3_calib;
volatile MM3_working_t MM3;
volatile uint8_t MM3_Timeout = 0;
 
 
 
/*********************************************/
/* Initialize Interface to MM3 Compass */
/*********************************************/
void MM3_Init(void)
{
uint8_t sreg = SREG;
 
cli();
 
// Configure Pins for SPI
// set SCK (PB7), MOSI (PB5) as output
DDRB |= (1<<DDB7)|(1<<DDB5);
// set MISO (PB6) as input
DDRB &= ~(1<<DDB6);
 
 
// Output Pins MM3_SS ,MM3_RESET
MM3_SS_DDR |= (1<<MM3_SS_PIN);
MM3_RESET_DDR |= (1<<MM3_RESET_PIN);
// set pins permanent to low
MM3_SS_PORT &= ~((1<<MM3_SS_PIN));
MM3_RESET_PORT &= ~((1<<MM3_RESET_PIN));
 
// Initialize SPI-Interface
// Enable interrupt (SPIE=1)
// Enable SPI bus (SPE=1)
// MSB transmitted first (DORD = 0)
// Master SPI Mode (MSTR=1)
// Clock polarity low when idle (CPOL=0)
// Clock phase sample at leading edge (CPHA=0)
// Clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz
SPCR = (1<<SPIE)|(1<<SPE)|(0<<DORD)|(1<<MSTR)|(0<<CPOL)|(0<<CPHA)|(1<<SPR1)|(1<<SPR0);
SPSR &= ~(1<<SPI2X);
 
// Init Statemachine
MM3.AXIS = MM3_X_AXIS;
MM3.STATE = MM3_STATE_RESET;
 
// Read calibration from EEprom
MM3_calib.X_off = (int8_t)GetParamByte(PID_MM3_X_OFF);
MM3_calib.Y_off = (int8_t)GetParamByte(PID_MM3_Y_OFF);
MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF);
MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE);
MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE);
MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE);
 
MM3_Timeout = 0;
 
SREG = sreg;
}
 
 
/*********************************************/
/* Get Data from MM3 */
/*********************************************/
void MM3_Update(void) // called every 102.4 µs by timer 0 ISR
{
switch (MM3.STATE)
{
case MM3_STATE_RESET:
MM3_SS_ON // select slave
MM3_RESET_ON // RESET to High, MM3 Reset
MM3.STATE = MM3_STATE_START_TRANSFER;
return;
 
case MM3_STATE_START_TRANSFER:
MM3_RESET_OFF // RESET auf Low (was 102.4 µs at high level)
// write to SPDR triggers automatically the transfer MOSI MISO
// MM3 Period, + AXIS code
switch(MM3.AXIS)
{
case MM3_X_AXIS:
SPDR = MM3_PERIOD_256 + MM3_X_AXIS;
break;
case MM3_Y_AXIS:
SPDR = MM3_PERIOD_256 + MM3_Y_AXIS;
break;
case MM3_Z_AXIS:
SPDR = MM3_PERIOD_256 + MM3_Z_AXIS;
break;
default:
MM3.AXIS = MM3_X_AXIS;
MM3.STATE = MM3_STATE_RESET;
return;
}
 
// DRDY line is not connected, therefore
// wait before reading data back
MM3.DRDY = SetDelay(8); // wait 8ms for data ready
MM3.STATE = MM3_STATE_WAIT_DRDY;
return;
 
case MM3_STATE_WAIT_DRDY:
if (CheckDelay(MM3.DRDY))
{
// write something into SPDR to trigger data reading
SPDR = 0x00;
MM3.STATE = MM3_STATE_DRDY;
}
return;
}
}
 
 
/*********************************************/
/* Interrupt SPI transfer complete */
/*********************************************/
ISR(SPI_STC_vect)
{
static int8_t tmp;
int16_t value;
 
switch (MM3.STATE)
{
// 1st byte received
case MM3_STATE_DRDY:
tmp = SPDR; // store 1st byte
SPDR = 0x00; // trigger transfer of 2nd byte
MM3.STATE = MM3_STATE_BYTE2;
return;
 
case MM3_STATE_BYTE2: // 2nd byte received
value = (int16_t)tmp; // combine the 1st and 2nd byte to a word
value <<= 8; // shift 1st byte to MSB-Position
value |= (int16_t)SPDR; // add 2nd byte
 
if(abs(value) < MAX_AXIS_VALUE) // ignore spikes
{
switch (MM3.AXIS)
{
case MM3_X_AXIS:
MM3.x_axis = value;
MM3.AXIS = MM3_Y_AXIS;
break;
case MM3_Y_AXIS:
MM3.y_axis = value;
MM3.AXIS = MM3_Z_AXIS;
break;
case MM3_Z_AXIS:
MM3.z_axis = value;
MM3.AXIS = MM3_X_AXIS;
break;
default:
MM3.AXIS = MM3_X_AXIS;
break;
}
}
MM3_SS_OFF // deselect slave
MM3.STATE = MM3_STATE_RESET;
// Update timeout is called every 102.4 µs.
// It takes 2 cycles to write a measurement data request for one axis and
// at at least 8 ms / 102.4 µs = 79 cycles to read the requested data back.
// I.e. 81 cycles * 102.4 µs = 8.3ms per axis.
// The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() -
// decremtent the MM3_Timeout every 100 ms.
// incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout.
if ((MM3.x_axis != MM3.y_axis) || (MM3.x_axis != MM3.z_axis) || (MM3.y_axis != MM3.z_axis))
{ // if all axis measurements give diffrent readings the data should be valid
if(MM3_Timeout < 20) MM3_Timeout++;
}
else // something is very strange here
{
if(MM3_Timeout ) MM3_Timeout--;
}
return;
 
default:
return;
}
}
 
 
/*********************************************/
/* Calibrate Compass */
/*********************************************/
void MM3_Calibrate(void)
{
static int16_t x_min, x_max, y_min, y_max, z_min, z_max;
 
switch(CompassCalState)
{
case 1: // change to x-y axis
x_min = 10000;
x_max = -10000;
y_min = 10000;
y_max = -10000;
z_min = 10000;
z_max = -10000;
break;
case 2:
// find Min and Max of the X- and Y-Axis
if(MM3.x_axis < x_min) x_min = MM3.x_axis;
if(MM3.x_axis > x_max) x_max = MM3.x_axis;
if(MM3.y_axis < y_min) y_min = MM3.y_axis;
if(MM3.y_axis > y_max) y_max = MM3.y_axis;
break;
case 3:
// change to z-Axis
break;
case 4:
RED_ON; // find Min and Max of the Z-axis
if(MM3.z_axis < z_min) z_min = MM3.z_axis;
if(MM3.z_axis > z_max) z_max = MM3.z_axis;
break;
case 5:
// calc range of all axis
MM3_calib.X_range = (x_max - x_min);
MM3_calib.Y_range = (y_max - y_min);
MM3_calib.Z_range = (z_max - z_min);
 
// calc offset of all axis
MM3_calib.X_off = (x_max + x_min) / 2;
MM3_calib.Y_off = (y_max + y_min) / 2;
MM3_calib.Z_off = (z_max + z_min) / 2;
 
// save to EEProm
SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off);
SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off);
SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off);
SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range);
SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range);
SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range);
 
CompassCalState = 0;
break;
default:
CompassCalState = 0;
break;
}
}
 
 
/*
void MM3_Calibrate(void)
{
static uint8_t debugcounter = 0;
int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0;
uint8_t measurement = 50, beeper = 0;
uint16_t timer;
 
GRN_ON;
RED_OFF;
 
// get maximum and minimum reading of all axis
while (measurement)
{
// reset range markers if yawstick ist leftmost
if(PPM_in[staticParams.ChannelAssignment[CH_YAW]] > 100)
{
x_min = 0;
x_max = 0;
y_min = 0;
y_max = 0;
z_min = 0;
z_max = 0;
}
 
if (MM3.x_axis > x_max) x_max = MM3.x_axis;
else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
 
if (MM3.y_axis > y_max) y_max = MM3.y_axis;
else if (MM3.y_axis < y_min) y_min = MM3.y_axis;
 
if (MM3.z_axis > z_max) z_max = MM3.z_axis;
else if (MM3.z_axis < z_min) z_min = MM3.z_axis;
 
if (!beeper)
{
RED_FLASH;
GRN_FLASH;
BeepTime = 50;
beeper = 50;
}
beeper--;
// loop with period of 10 ms / 100 Hz
timer = SetDelay(10);
while(!CheckDelay(timer));
 
if(debugcounter++ > 30)
{
printf("\n\rXMin:%4d, XMax:%4d, YMin:%4d, YMax:%4d, ZMin:%4d, ZMax:%4d",x_min,x_max,y_min,y_max,z_min,z_max);
debugcounter = 0;
}
 
// If gas is less than 100, stop calibration with a delay of 0.5 seconds
if (PPM_in[staticParams.ChannelAssignment[CH_GAS]] < 100) measurement--;
}
// Rage of all axis
MM3_calib.X_range = (x_max - x_min);
MM3_calib.Y_range = (y_max - y_min);
MM3_calib.Z_range = (z_max - z_min);
 
// Offset of all axis
MM3_calib.X_off = (x_max + x_min) / 2;
MM3_calib.Y_off = (y_max + y_min) / 2;
MM3_calib.Z_off = (z_max + z_min) / 2;
 
// save to EEProm
SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off);
SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off);
SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off);
SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range);
SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range);
SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range);
 
}
*/
 
/*********************************************/
/* Calculate north direction (heading) */
/*********************************************/
void MM3_Heading(void)
{
int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw;
int32_t Hx, Hy, Hz, Hx_corr, Hy_corr;
int16_t angle;
int16_t heading;
 
if (MM3_Timeout)
{
// Offset correction and normalization (values of H are +/- 512)
Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range;
Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range;
Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range;
 
// Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation
// assuming the MM3 board is mounted parallel to the frame.
// User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading
// in a top view counter clockwise direction.
// North is in opposite direction of the small arrow on the MM3 board.
// Therefore 180 deg must be added to that angle.
angle = ((int16_t)staticParams.UserParams2[0] + 180);
// wrap angle to interval of 0°- 359°
angle += 360;
angle %= 360;
sin_yaw = (int32_t)(c_sin_8192(angle));
cos_yaw = (int32_t)(c_cos_8192(angle));
 
Hx_corr = Hx;
Hy_corr = Hy;
 
// rotate
Hx = (Hx_corr * cos_yaw - Hy_corr * sin_yaw) / 8192;
Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192;
 
 
// tilt compensation
 
// calculate sinus cosinus of nick and tilt angle
angle = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR);
sin_nick = (int32_t)(c_sin_8192(angle));
cos_nick = (int32_t)(c_cos_8192(angle));
 
angle = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR);
sin_roll = (int32_t)(c_sin_8192(angle));
cos_roll = (int32_t)(c_cos_8192(angle));
 
Hx_corr = Hx * cos_nick;
Hx_corr -= Hz * sin_nick;
Hx_corr /= 8192;
 
Hy_corr = Hy * cos_roll;
Hy_corr += Hz * sin_roll;
Hy_corr /= 8192;
 
// calculate Heading
heading = c_atan2(Hy_corr, Hx_corr);
 
// atan returns angular range from -180 deg to 180 deg in counter clockwise notation
// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
if (heading < 0) heading = -heading;
else heading = 360 - heading;
}
else // MM3_Timeout = 0 i.e now new data from external board
{
if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem
heading = -1;
}
// update compass values in fc variables
CompassHeading = heading;
if (CompassHeading < 0) CompassOffCourse = 0;
else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
}
/branches/dongfang_FC_rewrite/mm3.h
0,0 → 1,29
#ifndef _MM3_H
#define _MM3_H
 
typedef struct
{
int8_t X_off;
int8_t Y_off;
int8_t Z_off;
int16_t X_range;
int16_t Y_range;
int16_t Z_range;
} MM3_calib_t;
 
extern MM3_calib_t MM3_calib;
 
// Initialization of the MM3 communication
void MM3_Init(void);
 
// should be called cyclic to get actual compass axis readings
void MM3_Update(void);
// this function calibrates the MM3
// and returns immediately if the communication to the MM3-Board is broken.
void MM3_Calibrate(void);
 
// update compass heading
void MM3_Heading(void);
 
#endif //_MM3_H
 
/branches/dongfang_FC_rewrite/old_macros.h
0,0 → 1,47
/*
For backwards compatibility only.
Ingo Busker ingo@mikrocontroller.com
*/
 
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
 
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
 
#ifndef inb
#define inb(sfr) _SFR_BYTE(sfr)
#endif
 
#ifndef outb
#define outb(sfr, val) (_SFR_BYTE(sfr) = (val))
#endif
 
#ifndef inw
#define inw(sfr) _SFR_WORD(sfr)
#endif
 
#ifndef outw
#define outw(sfr, val) (_SFR_WORD(sfr) = (val))
#endif
 
#ifndef outp
#define outp(val, sfr) outb(sfr, val)
#endif
 
#ifndef inp
#define inp(sfr) inb(sfr)
#endif
 
#ifndef BV
#define BV(bit) _BV(bit)
#endif
 
 
#ifndef PRG_RDB
#define PRG_RDB pgm_read_byte
#endif
 
/branches/dongfang_FC_rewrite/output.c
0,0 → 1,106
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <inttypes.h>
#include "output.h"
#include "eeprom.h"
 
// To access the DebugOut struct.
#include "uart0.h"
 
uint8_t J16Blinkcount = 0, J16Mask = 1;
uint8_t J17Blinkcount = 0, J17Mask = 1;
 
// initializes the LED control outputs J16, J17
void output_init(void) {
// set PC2 & PC3 as output (control of J16 & J17)
DDRC |= (1<<DDC2)|(1<<DDC3);
J16_OFF;
J17_OFF;
J16Blinkcount = 0; J16Mask = 128;
J17Blinkcount = 0; J17Mask = 128;
}
 
void flashingLights(void) {
static int8_t delay = 0;
if(!delay--) { // 10 ms intervall
delay = 4;
if ((staticParams.J16Timing > 250) && (dynamicParams.J16Timing > 230)) {
if(staticParams.J16Bitmask & 128) J16_ON;
else J16_OFF;
} else if ((staticParams.J16Timing > 250) && (dynamicParams.J16Timing < 10)) {
if(staticParams.J16Bitmask & 128) J16_OFF;
else J16_ON;
} else if(!J16Blinkcount--) {
J16Blinkcount = dynamicParams.J16Timing - 1;
if(J16Mask == 1) J16Mask = 128; else J16Mask = J16Mask >> 1;
if(J16Mask & staticParams.J16Bitmask) J16_ON; else J16_OFF;
}
if ((staticParams.J17Timing > 250) && (dynamicParams.J17Timing > 230)) {
if(staticParams.J17Bitmask & 128) J17_ON;
else J17_OFF;
} else if ((staticParams.J17Timing > 250) && (dynamicParams.J17Timing < 10)) {
if(staticParams.J17Bitmask & 128) J17_OFF;
else J17_ON;
} else if(!J17Blinkcount--) {
J17Blinkcount = dynamicParams.J17Timing - 1;
if(J17Mask == 1) J17Mask = 128; else J17Mask = J17Mask >> 1;
if(J17Mask & staticParams.J17Bitmask) J17_ON; else J17_OFF;
}
}
}
 
void output_update(void) {
// flashingLights();
if (DebugOut.Digital[0]) J16_ON; else J16_OFF;
if (DebugOut.Digital[1]) J17_ON; else J17_OFF;
}
/branches/dongfang_FC_rewrite/output.h
0,0 → 1,22
#ifndef _OUTPUT_H
#define _OUTPUT_H
 
#include <avr/io.h>
 
// This is for LEDs connected directly between +5V and the AVR port, without transistors.
// PORTbit = 0 --> LED on.
// To use the normal transistor set-up where 1 --> transistor conductive, reverse the
// ON and OFF statements.
#define J16_ON PORTC &= ~(1<<PORTC2)
#define J16_OFF PORTC |= (1<<PORTC2)
#define J16_TOGGLE PORTC ^= (1<<PORTC2)
 
#define J17_ON PORTC &= ~(1<<PORTC3)
#define J17_OFF PORTC |= (1<<PORTC3)
#define J17_TOGGLE PORTC ^= (1<<PORTC3)
 
void output_init(void);
void output_update(void);
 
#endif //_output_H
 
/branches/dongfang_FC_rewrite/ports.txt
0,0 → 1,21
This is a list of the ATMega pins that are used for various ad-hoc I/O on the FC.
The A/D converter inputs, the ISP interface, I2C and the air pressure circuit and
red and green LED outputs are not included.
 
ATMEGA pin FC1.3 name FC 2.0 name Used for
-------------------------------------------------------------------------------------------
PC7 SPEAKER SPEAKER Beeper.
PC6 J6 SERVO_R FC1.3: J9. FC2.0: Reset on IC5/4017.
PC5 PC5 PC5 Expansion port pin 6. "SPI slave select" in spi.c.
PC4 PC4 PC4 Expansion port pin 5. PWM in from MK3Mag compass.
PC3 PC3 PC3 SV2 pin 1 via transistor. Also known as J17.
PC2 PC2 PC2 SV2 pin 5 via transistor. Also known as J16.
 
PD7 SERVO SERVO_C FC1.3: J7. FC2.0: Clock on IC5/4017.
PD6 ICP(PPM) ICP(PPM) R/C PPM signal in.
PD5 J3 J3 R/C channel 5 forward (rc.c). Auto-zero feature on InvenSense gyros (InvenSense.c, dongfang only).
PD4 J4 J4 R/C channel 6 forward (rc.c). Execution time measurement of main loop (main.c).
PD3 J5 J5 TXD1 with ATMega644p. R/C channel 7 forward with ATMega644.
PD2 RXD1 with ATMega644p
 
TDX1 and RXD1 are the 2nd UART in ATMEGA644p. It can be used for direct connection of a UBlox GPS module (no Navi-Ctrl), or for connection of a Spectrum or DSL serial R/C satellite.
/branches/dongfang_FC_rewrite/printf_P.c
0,0 → 1,483
// Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist nicht von der Lizenz für den MikroKopter-Teil unterstellt
 
/*
Copyright (C) 1993 Free Software Foundation
 
This file is part of the GNU IO Library. This library is free
software; you can redistribute it and/or modify it under the
terms of the GNU General Public License as published by the
Free Software Foundation; either version 2, or (at your option)
any later version.
 
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
 
You should have received a copy of the GNU General Public License
along with this library; see the file COPYING. If not, write to the Free
Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 
As a special exception, if you link this library with files
compiled with a GNU compiler to produce an executable, this does not cause
the resulting executable to be covered by the GNU General Public License.
This exception does not however invalidate any other reasons why
the executable file might be covered by the GNU General Public License. */
 
/*
* Copyright (c) 1990 Regents of the University of California.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. [rescinded 22 July 1999]
* 4. Neither the name of the University nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE REGENTS 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 REGENTS 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.
*/
 
/******************************************************************************
This file is a patched version of printf called _printf_P
It is made to work with avr-gcc for Atmel AVR MCUs.
There are some differences from standard printf:
1. There is no floating point support (with fp the code is about 8K!)
2. Return type is void
3. Format string must be in program memory (by using macro printf this is
done automaticaly)
4. %n is not implemented (just remove the comment around it if you need it)
5. If LIGHTPRINTF is defined, the code is about 550 bytes smaller and the
folowing specifiers are disabled :
space # * . - + p s o O
6. A function void uart_sendchar(char c) is used for output. The UART must
be initialized before using printf.
 
Alexander Popov
sasho@vip.orbitel.bg
******************************************************************************/
 
/*
* Actual printf innards.
*
* This code is large and complicated...
*/
 
#include <string.h>
#ifdef __STDC__
#include <stdarg.h>
#else
#include <varargs.h>
#endif
 
#include "old_macros.h"
#include "printf_P.h"
#include "menu.h"
#include "uart0.h"
 
 
//#define LIGHTPRINTF
char PrintZiel;
 
 
char Putchar(char zeichen)
{
if(PrintZiel == OUT_LCD) { DisplayBuff[DispPtr++] = zeichen; return(1);}
else return(uart_putchar(zeichen));
}
 
 
void PRINT(const char * ptr, unsigned int len)
{
for(;len;len--) Putchar(*ptr++);
}
 
void PRINTP(const char * ptr, unsigned int len)
{
for(;len;len--) Putchar(pgm_read_byte(ptr++));
}
 
void PAD_SP(signed char howmany)
{
for(;howmany>0;howmany--) Putchar(' ');
}
 
void PAD_0(signed char howmany)
{
for(;howmany>0;howmany--) Putchar('0');
}
 
#define BUF 40
 
/*
* Macros for converting digits to letters and vice versa
*/
#define to_digit(c) ((c) - '0')
#define is_digit(c) ((c)<='9' && (c)>='0')
#define to_char(n) ((n) + '0')
 
/*
* Flags used during conversion.
*/
#define LONGINT 0x01 /* long integer */
#define LONGDBL 0x02 /* long double; unimplemented */
#define SHORTINT 0x04 /* short integer */
#define ALT 0x08 /* alternate form */
#define LADJUST 0x10 /* left adjustment */
#define ZEROPAD 0x20 /* zero (as opposed to blank) pad */
#define HEXPREFIX 0x40 /* add 0x or 0X prefix */
 
void _printf_P (char ziel,char const *fmt0, ...) /* Works with string from FLASH */
{
va_list ap;
register const char *fmt; /* format string */
register char ch; /* character from fmt */
register int n; /* handy integer (short term usage) */
register char *cp; /* handy char pointer (short term usage) */
const char *fmark; /* for remembering a place in fmt */
register unsigned char flags; /* flags as above */
signed char width; /* width from format (%8d), or 0 */
signed char prec; /* precision from format (%.3d), or -1 */
char sign; /* sign prefix (' ', '+', '-', or \0) */
unsigned long _ulong=0; /* integer arguments %[diouxX] */
#define OCT 8
#define DEC 10
#define HEX 16
unsigned char base; /* base for [diouxX] conversion */
signed char dprec; /* a copy of prec if [diouxX], 0 otherwise */
signed char dpad; /* extra 0 padding needed for integers */
signed char fieldsz; /* field size expanded by sign, dpad etc */
/* The initialization of 'size' is to suppress a warning that
'size' might be used unitialized. It seems gcc can't
quite grok this spaghetti code ... */
signed char size = 0; /* size of converted field or string */
char buf[BUF]; /* space for %c, %[diouxX], %[eEfgG] */
char ox[2]; /* space for 0x hex-prefix */
 
PrintZiel = ziel; // bestimmt, LCD oder UART
va_start(ap, fmt0);
 
fmt = fmt0;
 
/*
* Scan the format for conversions (`%' character).
*/
for (;;) {
for (fmark = fmt; (ch = pgm_read_byte(fmt)) != '\0' && ch != '%'; fmt++)
/* void */;
if ((n = fmt - fmark) != 0) {
PRINTP(fmark, n);
}
if (ch == '\0')
goto done;
fmt++; /* skip over '%' */
 
flags = 0;
dprec = 0;
width = 0;
prec = -1;
sign = '\0';
 
rflag: ch = PRG_RDB(fmt++);
reswitch:
#ifdef LIGHTPRINTF
if (ch=='o' || ch=='u' || (ch|0x20)=='x') {
#else
if (ch=='u' || (ch|0x20)=='x') {
#endif
if (flags&LONGINT) {
_ulong=va_arg(ap, unsigned long);
} else {
register unsigned int _d;
_d=va_arg(ap, unsigned int);
_ulong = flags&SHORTINT ? (unsigned long)(unsigned short)_d : (unsigned long)_d;
}
}
 
#ifndef LIGHTPRINTF
if(ch==' ') {
/*
* ``If the space and + flags both appear, the space
* flag will be ignored.''
* -- ANSI X3J11
*/
if (!sign)
sign = ' ';
goto rflag;
} else if (ch=='#') {
flags |= ALT;
goto rflag;
} else if (ch=='*'||ch=='-') {
if (ch=='*') {
/*
* ``A negative field width argument is taken as a
* - flag followed by a positive field width.''
* -- ANSI X3J11
* They don't exclude field widths read from args.
*/
if ((width = va_arg(ap, int)) >= 0)
goto rflag;
width = -width;
}
flags |= LADJUST;
flags &= ~ZEROPAD; /* '-' disables '0' */
goto rflag;
} else if (ch=='+') {
sign = '+';
goto rflag;
} else if (ch=='.') {
if ((ch = PRG_RDB(fmt++)) == '*') {
n = va_arg(ap, int);
prec = n < 0 ? -1 : n;
goto rflag;
}
n = 0;
while (is_digit(ch)) {
n = n*10 + to_digit(ch);
ch = PRG_RDB(fmt++);
}
prec = n < 0 ? -1 : n;
goto reswitch;
} else
#endif /* LIGHTPRINTF */
if (ch=='0') {
/*
* ``Note that 0 is taken as a flag, not as the
* beginning of a field width.''
* -- ANSI X3J11
*/
if (!(flags & LADJUST))
flags |= ZEROPAD; /* '-' disables '0' */
goto rflag;
} else if (ch>='1' && ch<='9') {
n = 0;
do {
n = 10 * n + to_digit(ch);
ch = PRG_RDB(fmt++);
} while (is_digit(ch));
width = n;
goto reswitch;
} else if (ch=='h') {
flags |= SHORTINT;
goto rflag;
} else if (ch=='l') {
flags |= LONGINT;
goto rflag;
} else if (ch=='c') {
*(cp = buf) = va_arg(ap, int);
size = 1;
sign = '\0';
} else if (ch=='D'||ch=='d'||ch=='i') {
if(ch=='D')
flags |= LONGINT;
if (flags&LONGINT) {
_ulong=va_arg(ap, long);
} else {
register int _d;
_d=va_arg(ap, int);
_ulong = flags&SHORTINT ? (long)(short)_d : (long)_d;
}
 
if ((long)_ulong < 0) {
_ulong = -_ulong;
sign = '-';
}
base = DEC;
goto number;
} else
/*
if (ch=='n') {
if (flags & LONGINT)
*va_arg(ap, long *) = ret;
else if (flags & SHORTINT)
*va_arg(ap, short *) = ret;
else
*va_arg(ap, int *) = ret;
continue; // no output
} else
*/
#ifndef LIGHTPRINTF
if (ch=='O'||ch=='o') {
if (ch=='O')
flags |= LONGINT;
base = OCT;
goto nosign;
} else if (ch=='p') {
/*
* ``The argument shall be a pointer to void. The
* value of the pointer is converted to a sequence
* of printable characters, in an implementation-
* defined manner.''
* -- ANSI X3J11
*/
/* NOSTRICT */
_ulong = (unsigned int)va_arg(ap, void *);
base = HEX;
flags |= HEXPREFIX;
ch = 'x';
goto nosign;
} else if (ch=='s') { // print a string from RAM
if ((cp = va_arg(ap, char *)) == NULL) {
cp=buf;
cp[0] = '(';
cp[1] = 'n';
cp[2] = 'u';
cp[4] = cp[3] = 'l';
cp[5] = ')';
cp[6] = '\0';
}
if (prec >= 0) {
/*
* can't use strlen; can only look for the
* NUL in the first `prec' characters, and
* strlen() will go further.
*/
char *p = (char*)memchr(cp, 0, prec);
 
if (p != NULL) {
size = p - cp;
if (size > prec)
size = prec;
} else
size = prec;
} else
size = strlen(cp);
sign = '\0';
} else
#endif /* LIGHTPRINTF */
if(ch=='U'||ch=='u') {
if (ch=='U')
flags |= LONGINT;
base = DEC;
goto nosign;
} else if (ch=='X'||ch=='x') {
base = HEX;
/* leading 0x/X only if non-zero */
if (flags & ALT && _ulong != 0)
flags |= HEXPREFIX;
 
/* unsigned conversions */
nosign: sign = '\0';
/*
* ``... diouXx conversions ... if a precision is
* specified, the 0 flag will be ignored.''
* -- ANSI X3J11
*/
number: if ((dprec = prec) >= 0)
flags &= ~ZEROPAD;
 
/*
* ``The result of converting a zero value with an
* explicit precision of zero is no characters.''
* -- ANSI X3J11
*/
cp = buf + BUF;
if (_ulong != 0 || prec != 0) {
register unsigned char _d,notlastdigit;
do {
notlastdigit=(_ulong>=base);
_d = _ulong % base;
 
if (_d<10) {
_d+='0';
} else {
_d+='a'-10;
if (ch=='X') _d&=~0x20;
}
*--cp=_d;
_ulong /= base;
} while (notlastdigit);
#ifndef LIGHTPRINTF
// handle octal leading 0
if (base==OCT && flags & ALT && *cp != '0')
*--cp = '0';
#endif
}
 
size = buf + BUF - cp;
} else { //default
/* "%?" prints ?, unless ? is NUL */
if (ch == '\0')
goto done;
/* pretend it was %c with argument ch */
cp = buf;
*cp = ch;
size = 1;
sign = '\0';
}
 
/*
* All reasonable formats wind up here. At this point,
* `cp' points to a string which (if not flags&LADJUST)
* should be padded out to `width' places. If
* flags&ZEROPAD, it should first be prefixed by any
* sign or other prefix; otherwise, it should be blank
* padded before the prefix is emitted. After any
* left-hand padding and prefixing, emit zeroes
* required by a decimal [diouxX] precision, then print
* the string proper, then emit zeroes required by any
* leftover floating precision; finally, if LADJUST,
* pad with blanks.
*/
 
/*
* compute actual size, so we know how much to pad.
*/
fieldsz = size;
 
dpad = dprec - size;
if (dpad < 0)
dpad = 0;
 
if (sign)
fieldsz++;
else if (flags & HEXPREFIX)
fieldsz += 2;
fieldsz += dpad;
 
/* right-adjusting blank padding */
if ((flags & (LADJUST|ZEROPAD)) == 0)
PAD_SP(width - fieldsz);
 
/* prefix */
if (sign) {
PRINT(&sign, 1);
} else if (flags & HEXPREFIX) {
ox[0] = '0';
ox[1] = ch;
PRINT(ox, 2);
}
 
/* right-adjusting zero padding */
if ((flags & (LADJUST|ZEROPAD)) == ZEROPAD)
PAD_0(width - fieldsz);
 
/* leading zeroes from decimal precision */
PAD_0(dpad);
 
/* the string or number proper */
PRINT(cp, size);
 
/* left-adjusting padding (always blank) */
if (flags & LADJUST)
PAD_SP(width - fieldsz);
}
done:
va_end(ap);
}
/branches/dongfang_FC_rewrite/printf_P.h
0,0 → 1,19
#ifndef _PRINTF_P_H_
#define _PRINTF_P_H_
 
#include <avr/pgmspace.h>
 
#define OUT_V24 0
#define OUT_LCD 1
 
 
void _printf_P (char, char const *fmt0, ...);
extern char PrintZiel;
 
 
#define printf_P(format, args...) _printf_P(OUT_V24,format , ## args)
#define printf(format, args...) _printf_P(OUT_V24,PSTR(format) , ## args)
#define LCD_printfxy(x,y,format, args...) { DispPtr = y * 20 + x; _printf_P(OUT_LCD,PSTR(format) , ## args);}
#define LCD_printf(format, args...) { _printf_P(OUT_LCD,PSTR(format) , ## args);}
 
#endif
/branches/dongfang_FC_rewrite/rate-notes
0,0 → 1,5
integral: 180 grader er filtergyroroll summeret til 250000.
filtergyroroll er 16 gange ADV.
Summation sker: Hver 2 ms.
 
DVS: sample ADV / grad / sek er: 250000 / 180 grader / 500 s^-1 / 16 = 0.17 [ s / grad ]
/branches/dongfang_FC_rewrite/rc.c
0,0 → 1,385
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <avr/interrupt.h>
 
#include "rc.h"
#include "controlMixer.h"
#include "configuration.h"
 
// The channel array is 1-based. The 0th entry is not used.
volatile int16_t PPM_in[MAX_CHANNELS];
volatile int16_t PPM_diff[MAX_CHANNELS];
volatile uint8_t NewPpmData = 1;
volatile int16_t RC_Quality = 0;
int16_t RC_PRTY[4];
 
int16_t stickOffsetPitch = 0, stickOffsetRoll = 0;
 
/***************************************************************
* 16bit timer 1 is used to decode the PPM-Signal
***************************************************************/
void RC_Init (void) {
uint8_t sreg = SREG;
 
// disable all interrupts before reconfiguration
cli();
 
// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
DDRD &= ~(1<<DDD6);
PORTD |= (1<<PORTD6);
 
// Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
// set as output
DDRD |= /*(1<<DDD5)|*/(1<<DDD4);
// low level
PORTD &= ~(/*(1<<PORTD5)|*/(1<<PORTD4));
 
// PD3 can't be used if 2nd UART is activated
// because TXD1 is at that port
if(CPUType != ATMEGA644P) {
DDRD |= (1<<PORTD3);
PORTD &= ~(1<<PORTD3);
}
 
// Timer/Counter1 Control Register A, B, C
 
// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
// Enable input capture noise cancler (bit: ICNC1=1)
// Trigger on positive edge of the input capture pin (bit: ICES1=1),
// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs
// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
TCCR1A &= ~((1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM11)|(1<<WGM10));
TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12));
TCCR1B |= (1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);
TCCR1C &= ~((1<<FOC1A)|(1<<FOC1B));
 
// Timer/Counter1 Interrupt Mask Register
 
// Enable Input Capture Interrupt (bit: ICIE1=1)
// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
// Enable Overflow Interrupt (bit: TOIE1=0)
TIMSK1 &= ~((1<<OCIE1B)|(1<<OCIE1A)|(1<<TOIE1));
TIMSK1 |= (1<<ICIE1);
 
RC_Quality = 0;
 
SREG = sreg;
}
 
 
/********************************************************************/
/* Every time a positive edge is detected at PD6 */
/********************************************************************/
/* t-Frame
<----------------------------------------------------------------------->
____ ______ _____ ________ ______ sync gap ____
| | | | | | | | | | |
| | | | | | | | | | |
___| |_| |_| |_| |_.............| |________________|
<-----><-------><------><--------> <------> <---
t0 t1 t2 t4 tn t0
 
The PPM-Frame length is 22.5 ms.
Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.
The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms.
The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms.
The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is
the syncronization gap.
*/
ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms
int16_t signal = 0, tmp;
static int16_t index;
static uint16_t oldICR1 = 0;
// 16bit Input Capture Register ICR1 contains the timer value TCNT1
// at the time the edge was detected
// calculate the time delay to the previous event time which is stored in oldICR1
// calculatiing the difference of the two uint16_t and converting the result to an int16_t
// implicit handles a timer overflow 65535 -> 0 the right way.
signal = (uint16_t) ICR1 - oldICR1;
oldICR1 = ICR1;
//sync gap? (3.52 ms < signal < 25.6 ms)
if((signal > 1100) && (signal < 8000)) {
// if a sync gap happens and there where at least 4 channels decoded before
// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
if(index >= 4) {
NewPpmData = 0; // Null means NewData for the first 4 channels
}
// synchronize channel index
index = 1;
} else { // within the PPM frame
if(index < MAX_CHANNELS-1) // PPM24 supports 12 channels
{
// check for valid signal length (0.8 ms < signal < 2.1984 ms)
// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
if((signal > 250) && (signal < 687))
{
// shift signal to zero symmetric range -154 to 159
signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
// check for stable signal
if(abs(signal - PPM_in[index]) < 6) {
if(RC_Quality < 200) RC_Quality +=10;
else RC_Quality = 200;
}
// calculate exponential history for signal
tmp = (3 * PPM_in[index] + signal) / 4;
if(tmp > signal + 1) tmp--; else
if(tmp < signal-1) tmp++;
// calculate signal difference on good signal level
if(RC_Quality >= 195)
PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
else PPM_diff[index] = 0;
PPM_in[index] = tmp; // update channel value
}
index++; // next channel
// demux sum signal for channels 5 to 7 to J3, J4, J5
// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
// channels are usually available at the receiver anyway.
// if(index == 5) J3HIGH; else J3LOW;
// if(index == 6) J4HIGH; else J4LOW;
// if(CPUType != ATMEGA644P) // not used as TXD1
// {
// if(index == 7) J5HIGH; else J5LOW;
// }
}
}
}
 
#define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]]
#define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]]
 
/*
* This must be called (as the only thing) for each control loop cycle (488 Hz).
*/
void RC_update() {
int16_t tmp1, tmp2;
if(RC_Quality) {
RC_Quality--;
if (NewPpmData-- == 0) {
RC_PRTY[CONTROL_PITCH] = (RCChannel(CH_PITCH) - stickOffsetPitch) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD;
RC_PRTY[CONTROL_ROLL] = (RCChannel(CH_ROLL) - stickOffsetRoll) * staticParams.StickP + RCDiff(CH_ROLL) * staticParams.StickD;
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[6] + 120;
if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
// exponential stick sensitivity in yawring rate
tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2
tmp2 += (staticParams.StickYawP * tmp1) / 4;
RC_PRTY[CONTROL_YAW] = tmp2;
}
} else { // Bad signal
RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0;
}
}
 
/*
* Get Pitch, Roll, Throttle, Yaw values
*/
int16_t* RC_getPRTY(void) {
return RC_PRTY;
}
 
/*
* Get other channel value
*/
int16_t RC_getVariable(uint8_t varNum) {
if (varNum < 4)
// 0th variable is 5th channel (1-based) etc.
return RCChannel(varNum + 5);
/*
* Let's just say:
* The RC variable 4 is hardwired to channel 5
* The RC variable 5 is hardwired to channel 6
* The RC variable 6 is hardwired to channel 7
* The RC variable 7 is hardwired to channel 8
* Alternatively, one could bind them to channel (4 + varNum) - or whatever...
*/
return PPM_in[varNum + 1];
}
 
uint8_t RC_getSignalQuality(void) {
if (RC_Quality >= 160)
return SIGNAL_GOOD;
if (RC_Quality >= 140)
return SIGNAL_OK;
if (RC_Quality >= 120)
return SIGNAL_BAD;
return SIGNAL_LOST;
}
 
/*
* To should fired only when the right stick is in the center position.
* This will cause the value of pitch and roll stick to be adjusted
* to zero (not just to near zero, as per the assumption in rc.c
* about the rc signal. I had values about 50..70 with a Futaba
* R617 receiver.) This calibration is not strictly necessary, but
* for control logic that depends on the exact (non)center position
* of a stick, it may be useful.
*/
void RC_calibrate(void) {
if (staticParams.GlobalConfig & CFG_HEADING_HOLD) {
// In HH, it s OK to trim the R/C. The effect should not be conteracted here.
stickOffsetPitch = stickOffsetRoll = 0;
} else {
stickOffsetPitch += RCChannel(CH_PITCH);
stickOffsetRoll += RCChannel(CH_ROLL);
}
}
 
#define COMMAND_THRESHOLD 85
uint8_t RC_getCommand(void) {
if(RCChannel(CH_THROTTLE) > COMMAND_THRESHOLD) {
// throttle is up
if(RCChannel(CH_YAW) > COMMAND_THRESHOLD)
return COMMAND_GYROCAL;
if(RCChannel(CH_YAW) < -COMMAND_THRESHOLD)
return COMMAND_ACCCAL;
return COMMAND_NONE;
} else if(RCChannel(CH_THROTTLE) < -COMMAND_THRESHOLD) {
// pitch is down
if(RCChannel(CH_YAW) > COMMAND_THRESHOLD)
return COMMAND_STOP;
if(RCChannel(CH_YAW) < -COMMAND_THRESHOLD)
return COMMAND_START;
return COMMAND_NONE;
} else {
// pitch is around center
return COMMAND_NONE;
}
}
 
/*
* Command arguments on R/C:
* 2--3--4
* | | +
* 1 0 5 ^ 0
* | | |
* 8--7--6
*
* + <--
* 0
*
* Not in any of these positions: 0
*/
#define ARGUMENT_THRESHOLD 70
uint8_t RC_getArgument(void) {
if(RCChannel(CH_PITCH) > ARGUMENT_THRESHOLD) {
// pitch is up
if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD)
return 2;
if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD)
return 4;
return 3;
} else if(RCChannel(CH_PITCH) < -ARGUMENT_THRESHOLD) {
// pitch is down
if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD)
return 8;
if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD)
return 6;
return 7;
} else {
// pitch is around center
if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD)
return 1;
if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD)
return 5;
return 0;
}
}
 
uint8_t RC_getLooping(uint8_t looping) {
// static uint8_t looping = 0;
 
if(RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_LEFT) {
looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT);
} else if((looping & LOOPING_LEFT) && RCChannel(CH_ROLL) < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT));
}
if(RCChannel(CH_ROLL) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_RIGHT) {
looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT);
} else if((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT));
}
if(RCChannel(CH_PITCH) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_UP) {
looping |= (LOOPING_PITCH_AXIS | LOOPING_UP);
} else if((looping & LOOPING_UP) && RCChannel(CH_PITCH) < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP));
}
if(RCChannel(CH_PITCH) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_DOWN) {
looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN);
} else if((looping & LOOPING_DOWN) && RCChannel(CH_PITCH) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN));
}
 
return looping;
}
 
/*
* Abstract controls are not used at the moment.
t_control rc_control = {
RC_getPitch,
RC_getRoll,
RC_getYaw,
RC_getThrottle,
RC_getSignalQuality,
RC_calibrate
};
*/
/branches/dongfang_FC_rewrite/rc.h
0,0 → 1,52
#ifndef _RC_H
#define _RC_H
 
#include <inttypes.h>
 
#define J3HIGH PORTD |= (1<<PORTD5)
#define J3LOW PORTD &= ~(1<<PORTD5)
#define J3TOGGLE PORTD ^= (1<<PORTD5)
 
#define J4HIGH PORTD |= (1<<PORTD4)
#define J4LOW PORTD &= ~(1<<PORTD4)
#define J4TOGGLE PORTD ^= (1<<PORTD4)
 
#define J5HIGH PORTD |= (1<<PORTD3)
#define J5LOW PORTD &= ~(1<<PORTD3)
#define J5TOGGLE PORTD ^= (1<<PORTD3)
 
#define MAX_CHANNELS 10
 
extern void RC_Init (void);
// the RC-Signal. todo: Not export any more.
extern volatile int16_t PPM_in[MAX_CHANNELS];
extern volatile int16_t PPM_diff[MAX_CHANNELS]; // the differentiated RC-Signal. Should that be exported??
extern volatile uint8_t NewPpmData; // 0 indicates a new recieved PPM Frame
extern volatile int16_t RC_Quality; // rc signal quality indicator (0 to 200)
 
// defines for lookup staticParams.ChannelAssignment
#define CH_PITCH 0
#define CH_ROLL 1
#define CH_THROTTLE 2
#define CH_YAW 3
#define CH_POTS 4
#define POT_OFFSET 110
 
/*
int16_t RC_getPitch (void);
int16_t RC_getYaw (void);
int16_t RC_getRoll (void);
uint16_t RC_getThrottle (void);
uint8_t RC_hasNewRCData (void);
*/
 
void RC_update(void);
int16_t* RC_getPRTY(void);
uint8_t RC_getArgument(void);
uint8_t RC_getCommand(void);
int16_t RC_getVariable(uint8_t varNum);
void RC_calibrate(void);
uint8_t RC_getSignalQuality(void);
uint8_t RC_getLooping(uint8_t looping);
 
#endif //_RC_H
/branches/dongfang_FC_rewrite/rc2.c
0,0 → 1,332
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <avr/interrupt.h>
 
#include "rc.h"
#include "configuration.h"
 
volatile int16_t PPM_in[15]; //PPM24 supports 12 channels per frame
volatile int16_t PPM_diff[15];
volatile uint8_t NewPpmData = 1;
volatile int16_t RC_Quality = 0;
int16_t stickOffsetPitch = 0, stickOffsetRoll = 0;
 
/***************************************************************/
/* 16bit timer 1 is used to decode the PPM-Signal */
/***************************************************************/
void RC_Init (void) {
uint8_t sreg = SREG;
 
// disable all interrupts before reconfiguration
cli();
 
// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
DDRD &= ~(1<<DDD6);
PORTD |= (1<<PORTD6);
 
// Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
// set as output
DDRD |= /*(1<<DDD5)|*/(1<<DDD4);
// low level
PORTD &= ~(/*(1<<PORTD5)|*/(1<<PORTD4));
 
// PD3 can't be used if 2nd UART is activated
// because TXD1 is at that port
if(CPUType != ATMEGA644P) {
DDRD |= (1<<PORTD3);
PORTD &= ~(1<<PORTD3);
}
 
// Timer/Counter1 Control Register A, B, C
 
// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
// Enable input capture noise cancler (bit: ICNC1=1)
// Trigger on positive edge of the input capture pin (bit: ICES1=1),
// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs
// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
TCCR1A &= ~((1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM11)|(1<<WGM10));
TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12));
TCCR1B |= (1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);
TCCR1C &= ~((1<<FOC1A)|(1<<FOC1B));
 
// Timer/Counter1 Interrupt Mask Register
 
// Enable Input Capture Interrupt (bit: ICIE1=1)
// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
// Enable Overflow Interrupt (bit: TOIE1=0)
TIMSK1 &= ~((1<<OCIE1B)|(1<<OCIE1A)|(1<<TOIE1));
TIMSK1 |= (1<<ICIE1);
 
RC_Quality = 0;
 
SREG = sreg;
}
 
 
/********************************************************************/
/* Every time a positive edge is detected at PD6 */
/********************************************************************/
/* t-Frame
<----------------------------------------------------------------------->
____ ______ _____ ________ ______ sync gap ____
| | | | | | | | | | |
| | | | | | | | | | |
___| |_| |_| |_| |_.............| |________________|
<-----><-------><------><--------> <------> <---
t0 t1 t2 t4 tn t0
 
The PPM-Frame length is 22.5 ms.
Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.
The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms.
The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms.
The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is
the syncronization gap.
*/
ISR(TIMER1_CAPT_vect) // typical rate of 1 ms to 2 ms
{
int16_t signal = 0, tmp;
static int16_t index;
static uint16_t oldICR1 = 0;
// 16bit Input Capture Register ICR1 contains the timer value TCNT1
// at the time the edge was detected
// calculate the time delay to the previous event time which is stored in oldICR1
// calculatiing the difference of the two uint16_t and converting the result to an int16_t
// implicit handles a timer overflow 65535 -> 0 the right way.
signal = (uint16_t) ICR1 - oldICR1;
oldICR1 = ICR1;
//sync gap? (3.52 ms < signal < 25.6 ms)
if((signal > 1100) && (signal < 8000))
{
// if a sync gap happens and there where at least 4 channels decoded before
// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
if(index >= 4)
{
NewPpmData = 0; // Null means NewData for the first 4 channels
}
// synchronize channel index
index = 1;
}
else // within the PPM frame
{
if(index < 14) // PPM24 supports 12 channels
{
// check for valid signal length (0.8 ms < signal < 2.1984 ms)
// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
if((signal > 250) && (signal < 687))
{
// shift signal to zero symmetric range -154 to 159
signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
// check for stable signal
if(abs(signal - PPM_in[index]) < 6) {
if(RC_Quality < 200) RC_Quality +=10;
else RC_Quality = 200;
}
// calculate exponential history for signal
tmp = (3 * (PPM_in[index]) + signal) / 4;
if(tmp > signal+1) tmp--; else
if(tmp < signal-1) tmp++;
// calculate signal difference on good signal level
if(RC_Quality >= 195) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
else PPM_diff[index] = 0;
PPM_in[index] = tmp; // update channel value
}
index++; // next channel
// demux sum signal for channels 5 to 7 to J3, J4, J5
// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
// channels are usually available at the receiver anyway.
// if(index == 5) J3HIGH; else J3LOW;
// if(index == 6) J4HIGH; else J4LOW;
if(CPUType != ATMEGA644P) // not used as TXD1
{
if(index == 7) J5HIGH; else J5LOW;
}
}
}
}
 
#define RCChannel(dimension) (PPM_in[ParamSet.ChannelAssignment[dimension]] * ParamSet.StickP)
#define RCDiff(dimension) (PPM_diff[ParamSet.ChannelAssignment[dimension]] * ParamSet.StickD)
// To start with, we don't attempt to normalize.
//#define rc_normalize(stick) (stick * STICK_RANGE / NORMAL_RANGE_COUNTS)
#define RCNormalize(stick) stick
 
/* Get the pitch input in the nominal range [-STICK_RANGE, STICK_RANGE]. */
int16_t RC_getPitch (void) {
return RCNormalize(RCChannel(CH_PITCH) - stickOffsetPitch + RCDiff(CH_PITCH));
}
 
/* Get the roll input in the nominal range [-STICK_RANGE, STICK_RANGE]. */
int16_t RC_getRoll (void) {
return RCNormalize(RCChannel(CH_ROLL) - stickOffsetRoll + RCDiff(CH_ROLL));
}
/* Get the yaw input in the nominal range [-STICK_RANGE, STICK_RANGE]. */
int16_t RC_getYaw (void) {
return RCNormalize(RCChannel(CH_YAW) + RCDiff(CH_YAW));
}
/* Get the throttle input in the nominal range [0, THROTTLE_RANGE]. */
uint16_t RC_getThrottle(void) {
// TODO: Some D term would actually be nice - at least for my acro style.
// return (RcChannel(CH_THROTTLE) + NORMAL_RANGE_COUNTS) * THROTTLE_RANGE / MILLISECOND_COUNTS_1;
return RcChannel(CH_THROTTLE);
}
 
uint16_t RC_getVariable(uint8_t channel) {
return RCChannel(CH_POTS + channel) + POT_OFFSET;
}
 
uint8_t RC_getSignalQuality(void) {
if (RC_Quality >= 160)
return SIGNAL_EXCELLENT;
if (RC_Quality >= 140)
return SIGNAL_OK;
if (RC_Quality >= 120)
return SIGNAL_BAD;
return SIGNAL_LOST;
}
 
/*
* To should fired only when the right stick is in the center position.
* This will cause the value of pitch and roll stick to be adjusted
* to zero (not just to near zero, as per the assumption in rc.c
* about the rc signal. I had values about 50..70 with a Futaba
* R617 receiver.) This calibration is not strictly necessary, but
* for control logic that depends on the exact (non)center position
* of a stick, it may be useful.
*/
void RC_calibrate(void) {
stickOffsetPitch += stickPitch;
stickOffsetRoll += stickRoll;
}
 
#define COMMAND_THRESHOLD 85
uint8_t RC_getCommand(void) {
if(RCChannel(CH_THROTTLE) > COMMAND_THRESHOLD) {
// throttle is up
if(RCChannel(CH_YAW) > COMMAND_THRESHOLD)
return STICK_COMMAND_GYROCAL;
if(RCChannel(CH_YAW) < -COMMAND_THRESHOLD)
return STICK_COMMAND_ACCCAL;
return STICK_COMMAND_UNDEF;
} else if(RCChannel(CH_THROTTLE) < -COMMAND_THRESHOLD) {
// pitch is down
if(RCChannel(CH_YAW) > COMMAND_THRESHOLD)
return STICK_COMMAND_STOP;
if(RCChannel(CH_YAW) < -COMMAND_THRESHOLD)
return STICK_COMMAND_START;
return STICK_COMMAND_UNDEF;
} else {
// pitch is around center
return STICK_COMMAND_UNDEF;
}
}
 
/*
* Command arguments on R/C:
* 2--3--4
* | | +
* 1 9 5 ^ 0
* | | |
* 8--7--6
*
* + <--
* 0
*
* Not in any of these positions: 0
*/
#define ARGUMENT_THRESHOLD 70
uint8_t RC_getArgument(void) {
if(RCChannel(CH_PITCH) > ARGUMENT_THRESHOLD) {
// pitch is up
if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD)
return 2;
if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD)
return 4;
return 3;
} else if(RCChannel(CH_PITCH) < -ARGUMENT_THRESHOLD) {
// pitch is down
if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD)
return 8;
if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD)
return 6;
return 7;
} else {
// pitch is around center
if(RCChannel(CH_ROLL) > ARGUMENT_THRESHOLD)
return 1;
if(RCChannel(CH_ROLL) < -ARGUMENT_THRESHOLD)
return 5;
return 9;
}
}
 
 
/*
* Abstract controls not used at the moment.
t_control rc_control = {
RC_getPitch,
RC_getRoll,
RC_getYaw,
RC_getThrottle,
RC_getSignalQuality,
RC_calibrate
};
*/
/branches/dongfang_FC_rewrite/rc2.h
0,0 → 1,32
#ifndef _RC_H
#define _RC_H
 
#include <inttypes.h>
 
#define J3HIGH PORTD |= (1<<PORTD5)
#define J3LOW PORTD &= ~(1<<PORTD5)
#define J3TOGGLE PORTD ^= (1<<PORTD5)
 
#define J4HIGH PORTD |= (1<<PORTD4)
#define J4LOW PORTD &= ~(1<<PORTD4)
#define J4TOGGLE PORTD ^= (1<<PORTD4)
 
#define J5HIGH PORTD |= (1<<PORTD3)
#define J5LOW PORTD &= ~(1<<PORTD3)
#define J5TOGGLE PORTD ^= (1<<PORTD3)
 
extern void RC_Init (void);
extern volatile int16_t PPM_in[15]; // the RC-Signal
//extern volatile int16_t PPM_diff[15]; // the differentiated RC-Signal
extern volatile uint8_t NewPpmData; // 0 indicates a new recieved PPM Frame
//extern volatile int16_t RC_Quality; // rc signal quality indicator (0 to 200)
 
// defines for lookup staticParams.ChannelAssignment
#define CH_PITCH 0
#define CH_ROLL 1
#define CH_THROTTLE 2
#define CH_YAW 3
#define CH_POTS 4
#define POT_OFFSET 110
 
#endif //_RC_H
/branches/dongfang_FC_rewrite/sensors.h
0,0 → 1,18
/*
* Common procedures for all gyro types.
*/
 
/*
* For InvenSense, set a port bit for the AUTO_ZERO line.
* Moved to gyro_calibrate.
*/
// void gyro_init(void);
 
/*
* FC 1.3 hardware: Searching the DAC values that return neutral readings.
* FC 2.0 hardware: Nothing to do.
* InvenSense hardware: Output a pulse on the AUTO_ZERO line.
*/
void gyro_calibrate(void);
 
void gyro_setDefaults(void);
/branches/dongfang_FC_rewrite/spectrum.c
0,0 → 1,309
/*#######################################################################################
Decodieren eines RC Summen Signals oder Spektrum Empfänger-Satellit
#######################################################################################*/
 
#include "Spectrum.h"
 
//--------------------------------------------------------------//
 
//--------------------------------------------------------------//
void SpektrumBinding(void)
{
unsigned int timerTimeout = SetDelay(10000); // Timeout 10 sec.
unsigned char connected = 0;
unsigned int delaycounter;
UCSR1B &= ~(1 << RXCIE1); // disable rx-interrupt
UCSR1B &= ~(1<<RXEN1); // disable Uart-Rx
PORTD &= ~(1 << PORTD2); // disable pull-up
printf("\n\rPlease connect Spektrum receiver for binding NOW...");
while(!CheckDelay(timerTimeout))
{
if (PIND & (1 << PORTD2)) { timerTimeout = SetDelay(90); connected = 1; break; }
}
if (connected)
{
printf("ok.\n\r");
DDRD |= (1 << DDD2); // Rx as output
 
while(!CheckDelay(timerTimeout)); // delay after startup of RX
for (delaycounter = 0; delaycounter < 100; delaycounter++) PORTD |= (1 << PORTD2);
for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD &= ~(1 << PORTD2);
for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD |= (1 << PORTD2);
for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD &= ~(1 << PORTD2);
for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD |= (1 << PORTD2);
for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD &= ~(1 << PORTD2);
for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD |= (1 << PORTD2);
for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD &= ~(1 << PORTD2);
for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD |= (1 << PORTD2);
for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD &= ~(1 << PORTD2);
 
for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD |= (1 << PORTD2);
for (delaycounter = 0; delaycounter < 10; delaycounter++) PORTD &= ~(1 << PORTD2);
for (delaycounter = 0; delaycounter < 400; delaycounter++) PORTD |= (1 << PORTD2);
}
else
{ printf("Timeout.\n\r");
}
DDRD &= ~(1 << DDD2); // RX as input
PORTD &= ~(1 << PORTD2);
 
Uart1Init(); // init Uart again
}
 
//############################################################################
// zum Decodieren des Spektrum Satelliten wird USART1 benutzt.
// USART1 initialisation from killagreg
void Uart1Init(void)
//############################################################################
{
// -- Start of USART1 initialisation for Spekturm seriell-mode
// USART1 Control and Status Register A, B, C and baud rate register
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * 115200) - 1);
// disable RX-Interrupt
UCSR1B &= ~(1 << RXCIE1);
// disable TX-Interrupt
UCSR1B &= ~(1 << TXCIE1);
// disable DRE-Interrupt
UCSR1B &= ~(1 << UDRIE1);
// set direction of RXD1 and TXD1 pins
// set RXD1 (PD2) as an input pin
PORTD |= (1 << PORTD2);
DDRD &= ~(1 << DDD2);
// USART0 Baud Rate Register
// set clock divider
UBRR1H = (uint8_t)(ubrr>>8);
UBRR1L = (uint8_t)ubrr;
// enable double speed operation
UCSR1A |= (1 << U2X1);
// enable receiver and transmitter
//UCSR1B = (1<<RXEN1)|(1<<TXEN1);
 
 
UCSR1B = (1<<RXEN1);
// set asynchronous mode
UCSR1C &= ~(1 << UMSEL11);
UCSR1C &= ~(1 << UMSEL10);
// no parity
UCSR1C &= ~(1 << UPM11);
UCSR1C &= ~(1 << UPM10);
// 1 stop bit
UCSR1C &= ~(1 << USBS1);
// 8-bit
UCSR1B &= ~(1 << UCSZ12);
UCSR1C |= (1 << UCSZ11);
UCSR1C |= (1 << UCSZ10);
// flush receive buffer explicit
while(UCSR1A & (1<<RXC1)) UDR1;
// enable RX-interrupts at the end
UCSR1B |= (1 << RXCIE1);
// -- End of USART1 initialisation
return;
}
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) Rainer Walther
// + RC-routines from original MK rc.c (c) H&I
// + Useful infos from Walter: http://www.rcgroups.com/forums/showthread.php?t=714299&page=2
// + only for non-profit use
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//
// 20080808 rw Modified for Spektrum AR6100 (PPM)
// 20080823 rw Add Spektrum satellite receiver on USART1 (644P only)
// 20081213 rw Add support for Spektrum DS9 Air-Tx-Module (9 channels)
// Replace AR6100-coding with original composit-signal routines
//
// ---
// Entweder Summensignal ODER Spektrum-Receiver anschließen. Nicht beides gleichzeitig betreiben!
// Binding is not implemented. Bind with external Receiver.
// Servo output J3, J4, J5 not serviced
//
// Anschuß Spektrum Receiver
// Orange: 3V von der FC (keinesfalls an 5V anschließen!)
// Schwarz: GND
// Grau: RXD1 (Pin 3) auf 10-Pol FC-Stecker
//
// ---
// Satellite-Reciever connected on USART1:
//
// DX7/DX6i: One data-frame at 115200 baud every 22ms.
// DX7se: One data-frame at 115200 baud every 11ms.
// byte1: unknown
// byte2: unknown
// byte3: and byte4: channel data (FLT-Mode)
// byte5: and byte6: channel data (Roll)
// byte7: and byte8: channel data (Nick)
// byte9: and byte10: channel data (Gier)
// byte11: and byte12: channel data (Gear Switch)
// byte13: and byte14: channel data (Gas)
// byte15: and byte16: channel data (AUX2)
//
// DS9 (9 Channel): One data-frame at 115200 baud every 11ms, alternating frame 1/2 for CH1-7 / CH8-9
// 1st Frame:
// byte1: unknown
// byte2: unknown
// byte3: and byte4: channel data
// byte5: and byte6: channel data
// byte7: and byte8: channel data
// byte9: and byte10: channel data
// byte11: and byte12: channel data
// byte13: and byte14: channel data
// byte15: and byte16: channel data
// 2nd Frame:
// byte1: unknown
// byte2: unknown
// byte3: and byte4: channel data
// byte5: and byte6: channel data
// byte7: and byte8: 0xffff
// byte9: and byte10: 0xffff
// byte11: and byte12: 0xffff
// byte13: and byte14: 0xffff
// byte15: and byte16: 0xffff
//
// Each channel data (16 bit= 2byte, first msb, second lsb) is arranged as:
//
// Bits: F 0 C3 C2 C1 C0 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0
//
// 0 means a '0' bit
// F: 1 = indicates beginning of 2nd frame for CH8-9 (DS9 only)
// C3 to C0 is the channel number. 0 to 9 (4 bit, as assigned in the transmitter)
// D9 to D0 is the channel data (10 bit) 0xaa..0x200..0x356 for 100% transmitter-travel
//
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
//############################################################################
//Diese Routine startet und inizialisiert den USART1 für seriellen Spektrum satellite reciever
SIGNAL(USART1_RX_vect)
//############################################################################
{
static unsigned int Sync=0, FrameCnt=0, ByteHigh=0, ReSync=1, Frame2=0, FrameTimer;
unsigned int Channel, index;
signed int signal, tmp;
int bCheckDelay;
uint8_t c;
c = UDR1; // get data byte
if (ReSync == 1)
{
// wait for beginning of new frame
ReSync = 0;
FrameTimer = SetDelay(7); // minimum 7ms zwischen den frames
FrameCnt = 0;
Sync = 0;
ByteHigh = 0;
}
else
{
bCheckDelay = CheckDelay(FrameTimer);
if ( Sync == 0 )
{
if(bCheckDelay)
{
// nach einer Pause von mind. 7ms erstes Sync-Character gefunden
// Zeichen ignorieren, da Bedeutung unbekannt
Sync = 1;
FrameCnt ++;
}
else
{
// Zeichen kam vor Ablauf der 7ms Sync-Pause
// warten auf erstes Sync-Zeichen
}
}
else if((Sync == 1) && !bCheckDelay)
{
// zweites Sync-Character ignorieren, Bedeutung unbekannt
Sync = 2;
FrameCnt ++;
}
else if((Sync == 2) && !bCheckDelay)
{
// Datenbyte high
ByteHigh = c;
if (FrameCnt == 2)
{
// is 1st Byte of Channel-data
// Frame 1 with Channel 1-7 comming next
Frame2 = 0;
if(ByteHigh & 0x80)
{
// DS9: Frame 2 with Channel 8-9 comming next
Frame2 = 1;
}
}
Sync = 3;
FrameCnt ++;
}
else if((Sync == 3) && !bCheckDelay)
{
// Datenbyte low
// High-Byte for next channel comes next
Sync = 2;
FrameCnt ++;
index = (ByteHigh >> 2) & 0x0f;
index ++;
Channel = (ByteHigh << 8) | c;
signal = Channel & 0x3ff;
signal -= 0x200; // Offset, range 0x000..0x3ff?
signal = signal/3; // scaling to fit PPM resolution
if(index >= 0 && index <= 10)
{
// Stabiles Signal
if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10; else SenderOkay = 200;}
tmp = (3 * (PPM_in[index]) + signal) / 4;
if(tmp > signal+1) tmp--; else
if(tmp < signal-1) tmp++;
if(SenderOkay >= 180) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;
else PPM_diff[index] = 0;
PPM_in[index] = tmp;
}
}
else
{
// hier stimmt was nicht: neu synchronisieren
ReSync = 1;
FrameCnt = 0;
Frame2 = 0;
}
// 16 Bytes per frame
if(FrameCnt >= 16)
{
// Frame complete
if(Frame2 == 0)
{
// Null bedeutet: Neue Daten
// nur beim ersten Frame (CH 0-7) setzen
NewPpmData = 0;
}
// new frame next, nach fruehestens 7ms erwartet
FrameCnt = 0;
Frame2 = 0;
Sync = 0;
}
// Zeit bis zum nächsten Zeichen messen
FrameTimer = SetDelay(7);
}
}
 
 
/branches/dongfang_FC_rewrite/spectrum.h
0,0 → 1,9
/*#######################################################################################
Dekodieren eines Spectrum Signals
#######################################################################################*/
 
#ifndef _SPECTRUM_H
#define _SPECTRUM_H
void Uart1Init(void);
void SpektrumBinding(void);
#endif //_RC_H
/branches/dongfang_FC_rewrite/spi.c
0,0 → 1,433
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <avr/io.h>
#include <avr/interrupt.h>
#include <string.h>
#include <stdlib.h>
#include "spi.h"
#include "rc.h"
#include "eeprom.h"
#include "uart0.h"
#include "timer0.h"
#include "analog.h"
#include "attitude.h"
#include "GPScontrol.h"
#include "flight.h"
 
//-----------------------------------------
#define DDR_SPI DDRB
#define DD_SS PB4
#define DD_SCK PB7
#define DD_MOSI PB5
#define DD_MISO PB6
 
// for compatibility reasons gcc3.x <-> gcc4.x
#ifndef SPCR
#define SPCR SPCR0
#endif
#ifndef SPIE
#define SPIE SPIE0
#endif
#ifndef SPE
#define SPE SPE0
#endif
#ifndef DORD
#define DORD DORD0
#endif
#ifndef MSTR
#define MSTR MSTR0
#endif
#ifndef CPOL
#define CPOL CPOL0
#endif
#ifndef CPHA
#define CPHA CPHA0
#endif
#ifndef SPR1
#define SPR1 SPR01
#endif
#ifndef SPR0
#define SPR0 SPR00
#endif
 
#ifndef SPDR
#define SPDR SPDR0
#endif
 
#ifndef SPSR
#define SPSR SPSR0
#endif
#ifndef SPIF
#define SPIF SPIF0
#endif
#ifndef WCOL
#define WCOL WCOL0
#endif
//#ifndef SPI2X
#define SPI2X SPI2X0
//#endif
// -------------------------
 
#define SLAVE_SELECT_DDR_PORT DDRC
#define SLAVE_SELECT_PORT PORTC
#define SPI_SLAVE_SELECT PC5
 
#define SPI_TXSYNCBYTE1 0xAA
#define SPI_TXSYNCBYTE2 0x83
#define SPI_RXSYNCBYTE1 0x81
#define SPI_RXSYNCBYTE2 0x55
 
typedef enum
{
SPI_SYNC1,
SPI_SYNC2,
SPI_DATA
} SPI_RXState_t;
 
 
// data exchange packets to and From NaviCtrl
ToNaviCtrl_t ToNaviCtrl;
FromNaviCtrl_t FromNaviCtrl;
 
SPI_VersionInfo_t SPI_VersionInfo;
 
// rx packet buffer
#define SPI_RXBUFFER_LEN sizeof(FromNaviCtrl)
uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN];
uint8_t SPI_RxBufferIndex = 0;
uint8_t SPI_RxBuffer_Request = 0;
 
// tx packet buffer
#define SPI_TXBUFFER_LEN sizeof(ToNaviCtrl)
uint8_t *SPI_TxBuffer;
uint8_t SPI_TxBufferIndex = 0;
 
uint8_t SPITransferCompleted, SPI_ChkSum;
uint8_t SPI_RxDataValid = 0;
uint8_t NCDataOkay = 0;
uint8_t NCSerialDataOkay = 0;
 
uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION };
uint8_t SPI_CommandCounter = 0;
 
/*********************************************/
/* Initialize SPI interface to NaviCtrl */
/*********************************************/
void SPI_MasterInit(void) {
DDR_SPI |= (1<<DD_MOSI)|(1<<DD_SCK); // Set MOSI and SCK output, all others input
SLAVE_SELECT_DDR_PORT |= (1 << SPI_SLAVE_SELECT); // set Slave select port as output port
SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1)|(0<<SPR0)|(0<<SPIE); // Enable SPI, Master, set clock rate fck/64
SPSR = 0;//(1<<SPI2X);
SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // Deselect Slave
SPI_TxBuffer = (uint8_t *) &ToNaviCtrl; // set pointer to tx-buffer
SPITransferCompleted = 1;
// initialize data packet to NaviControl
ToNaviCtrl.Sync1 = SPI_TXSYNCBYTE1;
ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2;
ToNaviCtrl.Command = SPI_CMD_USER;
ToNaviCtrl.IntegralNick = 0;
ToNaviCtrl.IntegralRoll = 0;
NCSerialDataOkay = 0;
NCDataOkay = 0;
SPI_RxDataValid = 0;
SPI_VersionInfo.Major = VERSION_MAJOR;
SPI_VersionInfo.Minor = VERSION_MINOR;
SPI_VersionInfo.Patch = VERSION_PATCH;
SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE;
}
 
 
/**********************************************************/
/* Update Data transferd by the SPI from/to NaviCtrl */
/**********************************************************/
void UpdateSPI_Buffer(void) {
uint8_t i;
int16_t tmp;
cli(); // stop all interrupts to avoid writing of new data during update of that packet.
// update content of packet to NaviCtrl
ToNaviCtrl.IntegralNick = (int16_t)((10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
ToNaviCtrl.IntegralRoll = (int16_t)((10 * rollAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
ToNaviCtrl.GyroHeading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1°
ToNaviCtrl.GyroNick = pitchRate_PID; // TODO: Which one should it be??
ToNaviCtrl.GyroRoll = rollRate_PID;
ToNaviCtrl.GyroYaw = yawRate;
ToNaviCtrl.AccNick = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1°
ToNaviCtrl.AccRoll = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1°
naviCntAcc = 0; naviAccPitch = 0; naviAccRoll = 0;
switch(ToNaviCtrl.Command) {
case SPI_CMD_USER:
for (i=0; i<sizeof(dynamicParams.UserParams); i++) {
ToNaviCtrl.Param.Byte[i] = dynamicParams.UserParams[i];
}
ToNaviCtrl.Param.Byte[8] = MKFlags;
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); // calibrate and start are temporal states that are cleared immediately after transmitting
ToNaviCtrl.Param.Byte[9] = (uint8_t)UBat;
ToNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning;
ToNaviCtrl.Param.Byte[11] = getActiveParamSet();
break;
 
case SPI_CMD_PARAMETER1:
ToNaviCtrl.Param.Byte[0] = staticParams.NaviGpsModeControl; // Parameters for the Naviboard
ToNaviCtrl.Param.Byte[1] = staticParams.NaviGpsGain;
ToNaviCtrl.Param.Byte[2] = staticParams.NaviGpsP;
ToNaviCtrl.Param.Byte[3] = staticParams.NaviGpsI;
ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsD;
ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsACC;
ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsMinSat;
ToNaviCtrl.Param.Byte[7] = staticParams.NaviStickThreshold;
ToNaviCtrl.Param.Byte[8] = staticParams.NaviOperatingRadius;
ToNaviCtrl.Param.Byte[9] = staticParams.NaviWindCorrection;
ToNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation;
ToNaviCtrl.Param.Byte[11] = staticParams.NaviAngleLimitation;
break;
case SPI_CMD_STICK:
tmp = PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
ToNaviCtrl.Param.Byte[0] = (int8_t) tmp;
tmp = PPM_in[staticParams.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
ToNaviCtrl.Param.Byte[1] = (int8_t) tmp;
tmp = PPM_in[staticParams.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
ToNaviCtrl.Param.Byte[2] = (int8_t) tmp;
tmp = PPM_in[staticParams.ChannelAssignment[CH_PITCH]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
ToNaviCtrl.Param.Byte[3] = (int8_t) tmp;
ToNaviCtrl.Param.Byte[4] = (uint8_t) variables[0];
ToNaviCtrl.Param.Byte[5] = (uint8_t) variables[1];
ToNaviCtrl.Param.Byte[6] = (uint8_t) variables[2];
ToNaviCtrl.Param.Byte[7] = (uint8_t) variables[3];
ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality;
break;
 
case SPI_CMD_MISC:
ToNaviCtrl.Param.Byte[0] = compassCalState;
if(compassCalState > 4)
{ // jump from 5 to 0
compassCalState = 0;
}
ToNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime;
ToNaviCtrl.Param.Int[1] = readingHeight; // at address of Byte 2 and 3
ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit;
ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit;
ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit;
break;
case SPI_CMD_VERSION:
ToNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major;
ToNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor;
ToNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch;
ToNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible;
ToNaviCtrl.Param.Byte[4] = BoardRelease;
break;
default:
break;
}
sei(); // enable all interrupts
// analyze content of packet from NaviCtrl if valid
if (SPI_RxDataValid) {
// update gps controls
if(abs(FromNaviCtrl.GPSStickNick) < 512 && abs(FromNaviCtrl.GPSStickRoll) < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) {
GPSStickPitch = FromNaviCtrl.GPSStickNick;
GPSStickRoll = FromNaviCtrl.GPSStickRoll;
NCDataOkay = 250;
}
// update compass readings
if(FromNaviCtrl.CompassHeading <= 360) {
compassHeading = FromNaviCtrl.CompassHeading;
}
if(compassHeading < 0) compassOffCourse = 0;
else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
// NaviCtrl wants to beep?
if (FromNaviCtrl.BeepTime > BeepTime && !compassCalState) BeepTime = FromNaviCtrl.BeepTime;
switch (FromNaviCtrl.Command) {
case SPI_KALMAN:
dynamicParams.KalmanK = FromNaviCtrl.Param.Byte[0];
dynamicParams.KalmanMaxFusion = FromNaviCtrl.Param.Byte[1];
dynamicParams.KalmanMaxDrift = FromNaviCtrl.Param.Byte[2];
NCSerialDataOkay = FromNaviCtrl.Param.Byte[3];
//DebugOut.Analog[29] = NCSerialDataOkay;
break;
default:
break;
}
}
else // no valid data from NaviCtrl
{
// disable GPS control
GPSStickPitch = 0;
GPSStickRoll = 0;
}
}
 
/*********************************************/
/* Start Transmission of packet to NaviCtrl */
/*********************************************/
void SPI_StartTransmitPacket(void){
if (!SPITransferCompleted) return; // return immediately if transfer is in progress
else // transmission was completed
{
SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // Select slave
// cyclic commands
ToNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
SPITransferCompleted = 0; // transfer is in progress
UpdateSPI_Buffer(); // update data in ToNaviCtrl
SPI_TxBufferIndex = 1; //proceed with 2nd byte
// -- Debug-Output ---
//----
asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
ToNaviCtrl.Chksum = ToNaviCtrl.Sync1; // init checksum
SPDR = ToNaviCtrl.Sync1; // send first byte
}
}
 
//------------------------------------------------------
// This is the spi data transfer between FlightCtrl and NaviCtrl
// Every time this routine is called within the mainloop one byte of the packet to
// the NaviCtrl and one byte of the packet from the NaviCtrl is possible transfered
 
void SPI_TransmitByte(void) {
static SPI_RXState_t SPI_RXState = SPI_SYNC1;
uint8_t rxdata;
static uint8_t rxchksum;
if (SPITransferCompleted) return; // return immediatly if transfer was completed
if (!(SPSR & (1 << SPIF))) return; // return if no SPI-IRQ pending
SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop
SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave
rxdata = SPDR; // save spi data register
switch (SPI_RXState) {
case SPI_SYNC1: // first sync byte
SPI_RxBufferIndex = 0; // set pointer to start of rx buffer
rxchksum = rxdata; // initialize checksum
if (rxdata == SPI_RXSYNCBYTE1 )
{ // 1st Syncbyte found
SPI_RXState = SPI_SYNC2; // trigger to state for second sync byte
}
break;
case SPI_SYNC2: // second sync byte
if (rxdata == SPI_RXSYNCBYTE2)
{ // 2nd Syncbyte found
rxchksum += rxdata; // update checksum
SPI_RXState = SPI_DATA; // trigger to state for second sync byte
}
else // 2nd Syncbyte not found
{
SPI_RXState = SPI_SYNC1; // jump back to 1st sync byte
}
break;
case SPI_DATA: // data bytes
SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata; // copy data byte to spi buffer
// if all bytes are received of a packet from the NaviCtrl
if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN)
{ // last byte transfered is the checksum of the packet
if (rxdata == rxchksum) // checksum matching?
{
// copy SPI_RxBuffer -> FromFlightCtrl
uint8_t *ptr = (uint8_t *)&FromNaviCtrl;
cli();
memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(FromNaviCtrl));
sei();
SPI_RxDataValid = 1;
//DebugOut.Analog[18]++;
}
else
{ // checksum does not match
//DebugOut.Analog[17]++;
SPI_RxDataValid = 0; // reset valid flag
}
SPI_RXState = SPI_SYNC1; // reset state sync
}
else // not all bytes transfered
{
rxchksum += rxdata; // update checksum
}
break;
}// eof switch(SPI_RXState)
// if still some bytes left for transmission to NaviCtrl
if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN)
{
SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // SelectSlave
asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte
ToNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent
SPI_TxBufferIndex++;
} else {
//Transfer of all bytes of the packet to NaviCtrl completed
SPITransferCompleted = 1;
}
}
/branches/dongfang_FC_rewrite/spi.h
0,0 → 1,100
// ######################## SPI - FlightCtrl ###################
#ifndef _SPI_H
#define _SPI_H
 
//#include <util/delay.h>
#include <inttypes.h>
 
 
#define SPI_PROTOCOL_COMP 1
 
 
#define SPI_CMD_USER 10
#define SPI_CMD_STICK 11
#define SPI_CMD_MISC 12
#define SPI_CMD_PARAMETER1 13
#define SPI_CMD_VERSION 14
 
typedef struct
{
uint8_t Sync1;
uint8_t Sync2;
uint8_t Command;
int16_t IntegralNick;
int16_t IntegralRoll;
int16_t AccNick;
int16_t AccRoll;
int16_t GyroHeading;
int16_t GyroNick;
int16_t GyroRoll;
int16_t GyroYaw;
union
{
int8_t sByte[12];
uint8_t Byte[12];
int16_t Int[6];
int32_t Long[3];
float Float[3];
} Param;
uint8_t Chksum;
} __attribute__((packed)) ToNaviCtrl_t;
 
 
 
#define SPI_CMD_OSD_DATA 100
#define SPI_CMD_GPS_POS 101
#define SPI_CMD_GPS_TARGET 102
#define SPI_KALMAN 103
 
typedef struct
{
uint8_t Command;
int16_t GPSStickNick;
int16_t GPSStickRoll;
int16_t GPS_Yaw;
int16_t CompassHeading;
int16_t Status;
uint16_t BeepTime;
union
{
int8_t Byte[12];
int16_t Int[6];
int32_t Long[3];
float Float[3];
} Param;
uint8_t Chksum;
} __attribute__((packed)) FromNaviCtrl_t;
 
 
typedef struct
{
uint8_t Major;
uint8_t Minor;
uint8_t Patch;
uint8_t Compatible;
} __attribute__((packed)) SPI_VersionInfo_t;
 
 
extern ToNaviCtrl_t ToNaviCtrl;
extern FromNaviCtrl_t FromNaviCtrl;
 
 
typedef struct
{
int8_t KalmanK;
int8_t KalmanMaxDrift;
int8_t KalmanMaxFusion;
uint8_t SerialDataOkay;
} __attribute__((packed)) NCData_t;
 
 
extern uint8_t NCDataOkay;
extern uint8_t NCSerialDataOkay;
 
void SPI_MasterInit(void);
void SPI_StartTransmitPacket(void);
void SPI_TransmitByte(void);
 
 
 
#endif //_SPI_H
/branches/dongfang_FC_rewrite/timer0.c
0,0 → 1,206
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "eeprom.h"
#include "analog.h"
 
#ifdef USE_MK3MAG
#include "mk3mag.h"
#endif
 
volatile uint16_t CountMilliseconds = 0;
volatile uint8_t runFlightControl = 0;
volatile uint16_t cntKompass = 0;
volatile uint16_t BeepTime = 0;
volatile uint16_t BeepModulation = 0xFFFF;
 
#ifdef USE_NAVICTRL
volatile uint8_t SendSPI = 0;
#endif
 
/*****************************************************
* Initialize Timer 0
*****************************************************/
// timer 0 is used for the PWM generation to control the offset voltage at the air pressure sensor
// Its overflow interrupt routine is used to generate the beep signal and the flight control motor update rate
void timer0_init(void) {
uint8_t sreg = SREG;
 
// disable all interrupts before reconfiguration
cli();
 
// Configure speaker port as output.
if(BoardRelease == 10) { // Speaker at PD2
DDRD |= (1<<DDD2);
PORTD &= ~(1<<PORTD2);
} else { // Speaker at PC7
DDRC |= (1<<DDC7);
PORTC &= ~(1<<PORTC7);
}
 
// set PB3 and PB4 as output for the PWM used as offset for the pressure sensor
DDRB |= (1<<DDB4)|(1<<DDB3);
PORTB &= ~((1<<PORTB4)|(1<<PORTB3));
 
// Timer/Counter 0 Control Register A
 
// Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1)
// Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0)
// Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0)
TCCR0A &= ~((1<<COM0A0)|(1<<COM0B0));
TCCR0A |= (1<<COM0A1)|(1<<COM0B1)|(1<<WGM01)|(1<<WGM00);
 
// Timer/Counter 0 Control Register B
 
// set clock divider for timer 0 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz
// i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz
// hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz
 
// divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0)
TCCR0B &= ~((1<<FOC0A)|(1<<FOC0B)|(1<<WGM02));
TCCR0B = (TCCR0B & 0xF8)|(0<<CS02)|(1<<CS01)|(0<<CS00);
 
// initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4
OCR0A = 0; // for PB3
OCR0B = 120; // for PB4
 
// init Timer/Counter 0 Register
TCNT0 = 0;
 
// Timer/Counter 0 Interrupt Mask Register
// enable timer overflow interrupt only
TIMSK0 &= ~((1<<OCIE0B)|(1<<OCIE0A));
TIMSK0 |= (1<<TOIE0);
 
SREG = sreg;
}
 
/*****************************************************/
/* Interrupt Routine of Timer 0 */
/*****************************************************/
ISR(TIMER0_OVF_vect) { // 9765.625 Hz
static uint8_t cnt_1ms = 1,cnt = 0;
uint8_t Beeper_On = 0;
#ifdef USE_NAVICTRL
if(SendSPI) SendSPI--; // if SendSPI is 0, the transmit of a byte via SPI bus to and from The Navicontrol is done
#endif
if(!cnt--) { // every 10th run (9.765kHz/10 = 976Hz)
cnt = 9;
cnt_1ms++;
cnt_1ms %= 2;
if(!cnt_1ms) runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz)
CountMilliseconds++; // increment millisecond counter
}
// beeper on if duration is not over
if(BeepTime) {
BeepTime--; // decrement BeepTime
if(BeepTime & BeepModulation) Beeper_On = 1;
else Beeper_On = 0;
}
else { // beeper off if duration is over
Beeper_On = 0;
BeepModulation = 0xFFFF;
}
// if beeper is on
if(Beeper_On) {
// set speaker port to high.
if(BoardRelease == 10) PORTD |= (1<<PORTD2); // Speaker at PD2
else PORTC |= (1<<PORTC7); // Speaker at PC7
} else { // beeper is off
// set speaker port to low
if(BoardRelease == 10) PORTD &= ~(1<<PORTD2);// Speaker at PD2
else PORTC &= ~(1<<PORTC7);// Speaker at PC7
}
#ifndef USE_NAVICTRL
// update compass value if this option is enabled in the settings
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) {
#ifdef USE_MK3MAG
MK3MAG_Update(); // read out mk3mag pwm
#endif
}
#endif
}
 
// -----------------------------------------------------------------------
uint16_t SetDelay (uint16_t t) {
return(CountMilliseconds + t - 1);
}
 
// -----------------------------------------------------------------------
int8_t CheckDelay(uint16_t t) {
return(((t - CountMilliseconds) & 0x8000) >> 8); // check sign bit
}
 
// -----------------------------------------------------------------------
void Delay_ms(uint16_t w) {
uint16_t t_stop = SetDelay(w);
while (!CheckDelay(t_stop));
}
 
// -----------------------------------------------------------------------
void Delay_ms_Mess(uint16_t w) {
uint16_t t_stop;
t_stop = SetDelay(w);
while (!CheckDelay(t_stop)) {
if(analogDataReady) {
analogDataReady = 0;
analog_start();
}
}
}
/branches/dongfang_FC_rewrite/timer0.h
0,0 → 1,21
#ifndef _TIMER0_H
#define _TIMER0_H
 
#include <inttypes.h>
 
extern volatile uint16_t CountMilliseconds;
extern volatile uint8_t runFlightControl;
extern volatile uint16_t cntKompass;
extern volatile uint16_t BeepModulation;
extern volatile uint16_t BeepTime;
#ifdef USE_NAVICTRL
extern volatile uint8_t SendSPI;
#endif
 
extern void timer0_init(void);
extern void Delay_ms(uint16_t w);
extern void Delay_ms_Mess(uint16_t w);
extern uint16_t SetDelay (uint16_t t);
extern int8_t CheckDelay (uint16_t t);
 
#endif //_TIMER0_H
/branches/dongfang_FC_rewrite/timer2.c
0,0 → 1,298
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <avr/io.h>
#include <avr/interrupt.h>
#include "eeprom.h"
#include "uart0.h"
#include "rc.h"
#include "attitude.h"
 
volatile int16_t ServoPitchValue = 0;
volatile int16_t ServoRollValue = 0;
volatile uint8_t ServoActive = 0;
 
#define HEF4017R_ON PORTC |= (1<<PORTC6)
#define HEF4017R_OFF PORTC &= ~(1<<PORTC6)
 
/*****************************************************
* Initialize Timer 2
*****************************************************/
// The timer 2 is used to generate the PWM at PD7 (J7)
// to control a camera servo for pitch compensation.
void timer2_init(void) {
uint8_t sreg = SREG;
 
// disable all interrupts before reconfiguration
cli();
 
// set PD7 as output of the PWM for pitch servo
DDRD |= (1<<DDD7);
PORTD &= ~(1<<PORTD7); // set PD7 to low
 
DDRC |= (1<<DDC6); // set PC6 as output (Reset for HEF4017)
//PORTC &= ~(1<<PORTC6); // set PC6 to low
HEF4017R_ON; // enable reset
 
// Timer/Counter 2 Control Register A
 
// Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1)
// PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
// PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0));
TCCR2A |= (1<<WGM21)|(1<<WGM20);
 
// Timer/Counter 2 Control Register B
 
// Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz
// The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us
// hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms
 
// divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1)
TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS22));
TCCR2B |= (1<<CS21)|(1<<CS20)|(1<<WGM22);
 
// Initialize the Timer/Counter 2 Register
TCNT2 = 0;
 
// Initialize the Output Compare Register A used for PWM generation on port PD7.
OCR2A = 255;
TCCR2A |= (1<<COM2A1); // set or clear at compare match depends on value of COM2A0
 
// Timer/Counter 2 Interrupt Mask Register
// Enable timer output compare match A Interrupt only
TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2));
TIMSK2 |= (1<<OCIE2A);
 
SREG = sreg;
}
 
void Servo_On(void) {
ServoActive = 1;
}
 
void Servo_Off(void) {
ServoActive = 0;
HEF4017R_ON; // enable reset
}
 
/*****************************************************
* Control Servo Position
*****************************************************/
ISR(TIMER2_COMPA_vect) {
// frame len 22.5 ms = 14063 * 1.6 us
// stop pulse: 0.3 ms = 188 * 1.6 us
// min servo pulse: 0.6 ms = 375 * 1.6 us
// max servo pulse: 2.4 ms = 1500 * 1.6 us
// resolution: 1500 - 375 = 1125 steps
#define PPM_STOPPULSE 188
#define PPM_FRAMELEN (1757 * .ServoRefresh) // 22.5 ms / 8 Channels = 2.8125ms per Servo Channel
#define MINSERVOPULSE 375
#define MAXSERVOPULSE 1500
#define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE)
 
#if defined(USE_NON_4017_SERVO_OUTPUTS) || defined(USE_4017_SERVO_OUTPUTS)
static uint8_t isGeneratingPulse = 0;
static uint16_t remainingPulseLength = 0;
static uint16_t ServoFrameTime = 0;
static uint8_t ServoIndex = 0;
#define MULTIPLIER 4
static int16_t ServoPitchOffset = (255 / 2) * MULTIPLIER; // initial value near center position
static int16_t ServoRollOffset = (255 / 2) * MULTIPLIER; // initial value near center position
#endif
#ifdef USE_NON_4017_SERVO_OUTPUTS
//---------------------------
// Pitch servo state machine
//---------------------------
if (!isGeneratingPulse) { // pulse output complete on _next_ interrupt
if(TCCR2A & (1<<COM2A0)) { // we are still outputting a high pulse
TCCR2A &= ~(1<<COM2A0); // make a low pulse on _next_ interrupt, and now
remainingPulseLength = MINSERVOPULSE + SERVORANGE / 2; // center position ~ 1.5ms
ServoPitchOffset = (ServoPitchOffset * 3 + (int16_t)dynamicParams.ServoPitchControl) / 4; // lowpass offset
if(staticParams.ServoPitchCompInvert & 0x01) {
// inverting movement of servo
// todo: function.
ServoPitchValue = ServoPitchOffset + (int16_t)(((int32_t)staticParams.ServoPitchComp (integralGyroPitch / 128L )) / (256L));
} else {
// todo: function.
// non inverting movement of servo
ServoPitchValue = ServoPitchOffset - (int16_t)(((int32_t)staticParams.ServoPitchComp (integralGyroPitch / 128L )) / (256L));
}
// limit servo value to its parameter range definition
if(ServoPitchValue < (int16_t)staticParams.ServoPitchMin) {
ServoPitchValue = (int16_t)staticParams.ServoPitchMin;
} else if(ServoPitchValue > (int16_t)staticParams.ServoPitchMax) {
ServoPitchValue = (int16_t)staticParams.ServoPitchMax;
}
remainingPulseLength = (ServoPitchValue - 256 / 2) * MULTIPLIER; // shift ServoPitchValue to center position
// range servo pulse width
if(remainingPulseLength > MAXSERVOPULSE ) remainingPulseLength = MAXSERVOPULSE; // upper servo pulse limit
else if(remainingPulseLength < MINSERVOPULSE) remainingPulseLength = MINSERVOPULSE; // lower servo pulse limit
 
// accumulate time for correct update rate
ServoFrameTime = remainingPulseLength;
} else { // we had a high pulse
TCCR2A |= (1<<COM2A0); // make a low pulse
remainingPulseLength = PPM_FRAMELEN - ServoFrameTime;
}
// set pulse output active
isGeneratingPulse = 1;
} // EOF Pitch servo state machine
 
#elseif defined(USE_4017_SERVOS)
//-----------------------------------------------------
// PPM state machine, onboard demultiplexed by HEF4017
//-----------------------------------------------------
if(!isGeneratingPulse) { // pulse output complete
if(TCCR2A & (1<<COM2A0)) { // we had a low pulse
TCCR2A &= ~(1<<COM2A0);// make a high pulse
if(ServoIndex == 0) { // if we are at the sync gap
remainingPulseLength = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time
ServoFrameTime = 0; // reset servo frame time
HEF4017R_ON; // enable HEF4017 reset
} else { // servo channels
remainingPulseLength = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
switch(ServoIndex) { // map servo channels
case 1: // Pitch Compensation Servo
ServoPitchOffset = (ServoPitchOffset * 3 + (int16_t)dynamicParams.ServoPitchControl * MULTIPLIER) / 4; // lowpass offset
ServoPitchValue = ServoPitchOffset; // offset (Range from 0 to 255 * 3 = 765)
if(staticParams.ServoPitchCompInvert & 0x01) {
// inverting movement of servo
ServoPitchValue += (int16_t)( ( (int32_t)staticParams.ServoPitchComp * MULTIPLIER * (integralGyroPitch / 128L ) ) / (256L) );
} else { // non inverting movement of servo
ServoPitchValue -= (int16_t)( ( (int32_t)staticParams.ServoPitchComp * MULTIPLIER * (integralGyroPitch / 128L ) ) / (256L) );
}
// limit servo value to its parameter range definition
if(ServoPitchValue < ((int16_t)staticParams.ServoPitchMin * MULTIPLIER)) {
ServoPitchValue = (int16_t)staticParams.ServoPitchMin * MULTIPLIER;
} else if(ServoPitchValue > ((int16_t)staticParams.ServoPitchMax * MULTIPLIER)) {
ServoPitchValue = (int16_t)staticParams.ServoPitchMax * MULTIPLIER;
}
remainingPulseLength += ServoPitchValue - (256 / 2) * MULTIPLIER; // shift ServoPitchValue to center position
ServoPitchValue /= MULTIPLIER;
// DebugOut.Analog[20] = ServoPitchValue;
break;
case 2: // Roll Compensation Servo
ServoRollOffset = (ServoRollOffset * 3 + (int16_t)80 * MULTIPLIER) / 4; // lowpass offset
ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
//if(staticParams.ServoRollCompInvert & 0x01)
{ // inverting movement of servo
ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLIER * (integralGyroRoll / 128L ) ) / (256L) );
}
/* else
{ // non inverting movement of servo
ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLIER * (IntegralGyroRoll / 128L ) ) / (256L) );
}
*/ // limit servo value to its parameter range definition
if(ServoRollValue < ((int16_t)staticParams.ServoPitchMin * MULTIPLIER)) {
ServoRollValue = (int16_t)staticParams.ServoPitchMin * MULTIPLIER;
} else if(ServoRollValue > ((int16_t)staticParams.ServoPitchMax * MULTIPLIER)) {
ServoRollValue = (int16_t)staticParams.ServoPitchMax * MULTIPLIER;
}
remainingPulseLength += ServoRollValue - (256 / 2) * MULTIPLIER; // shift ServoRollValue to center position
ServoRollValue /= MULTIPLIER;
//DebugOut.Analog[20] = ServoRollValue;
break;
default: // other servo channels
remainingPulseLength += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs
break;
}
// range servo pulse width
if(remainingPulseLength > MAXSERVOPULSE) remainingPulseLength = MAXSERVOPULSE; // upper servo pulse limit
else if(remainingPulseLength < MINSERVOPULSE) remainingPulseLength = MINSERVOPULSE; // lower servo pulse limit
// substract stop pulse width
remainingPulseLength -= PPM_STOPPULSE;
// accumulate time for correct sync gap
ServoFrameTime += remainingPulseLength;
}
} else { // we had a high pulse
TCCR2A |= (1<<COM2A0); // make a low pulse
// set pulsewidth to stop pulse width
remainingPulseLength = PPM_STOPPULSE;
// accumulate time for correct sync gap
ServoFrameTime += remainingPulseLength;
if(ServoActive && RC_Quality > 180) HEF4017R_OFF; // disable HEF4017 reset
ServoIndex++; // change to next servo channel
if(ServoIndex > staticParams.ServoRefresh) ServoIndex = 0; // reset to the sync gap
}
// set pulse output active
isGeneratingPulse = 1;
}
#endif
 
/*
* Cases:
* 1) 255 + 128 <= remainingPulseLength --> delta = 255
* 2) 255 <= remainingPulseLength < 255 + 128 --> delta = 255 - 128
* this is to avoid a too short delta on the last cycle, which would cause
* an interupt-on-interrupt condition and the loss of the last interrupt.
* 3) remainingPulseLength < 255 --> delta = remainingPulseLength
*/
#if defined(USE_NON_4017_SERVO_OUTPUTS) || defined(USE_4017_SERVO_OUTPUTS)
uint8_t delta;
if (remainingPulseLength >= (255 + 128)) {
delta = 255;
} else if (remainingPulseLength >= 255) {
delta = 255- 128;
} else {
delta = remainingPulseLength;
isGeneratingPulse = 0; // trigger to stop pulse
}
OCR2A = delta;
remainingPulseLength -= delta;
#endif
}
/branches/dongfang_FC_rewrite/timer2.h
0,0 → 1,14
#ifndef _TIMER2_H
#define _TIMER2_H
 
#include <inttypes.h>
 
extern volatile int16_t ServoNickValue;
extern volatile int16_t ServoRollValue;
 
void timer2_init(void);
void Servo_On(void);
void Servo_Off(void);
 
#endif //_TIMER2_H
 
/branches/dongfang_FC_rewrite/twimaster.c
0,0 → 1,310
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <avr/io.h>
#include <avr/interrupt.h>
#include <util/twi.h>
#include <util/delay.h>
//#include "eeprom.h"
#include "twimaster.h"
//#include "analog.h"
#include "configuration.h"
#include "printf_P.h"
 
volatile uint8_t twi_state = TWI_STATE_MOTOR_TX;
volatile uint8_t dac_channel = 0;
volatile uint8_t motor_write = 0;
volatile uint8_t motor_read = 0;
volatile uint16_t I2CTimeout = 100;
uint8_t missingMotor = 0;
 
MotorData_t Motor[MAX_MOTORS];
 
volatile uint8_t DACValues[4];
uint8_t DACChannel = 0;
 
#define SCL_CLOCK 200000L
#define I2C_TIMEOUT 30000
 
/**************************************************
* Initialize I2C (TWI)
**************************************************/
void I2C_init(void) {
uint8_t i;
uint8_t sreg = SREG;
cli();
// SDA is INPUT
DDRC &= ~(1<<DDC1);
// SCL is output
DDRC |= (1<<DDC0);
// pull up SDA
PORTC |= (1<<PORTC0)|(1<<PORTC1);
// TWI Status Register
// prescaler 1 (TWPS1 = 0, TWPS0 = 0)
TWSR &= ~((1<<TWPS1)|(1<<TWPS0));
// set TWI Bit Rate Register
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
twi_state = TWI_STATE_MOTOR_TX;
motor_write = 0;
motor_read = 0;
for(i=0; i < MAX_MOTORS; i++) {
Motor[i].SetPoint = 0;
Motor[i].Present = 0;
Motor[i].Error = 0;
Motor[i].MaxPWM = 0;
}
SREG = sreg;
}
 
/****************************************
* Start I2C
****************************************/
void I2C_Start(uint8_t start_state) {
twi_state = start_state;
// TWI Control Register
// clear TWI interrupt flag (TWINT=1)
// disable TWI Acknowledge Bit (TWEA = 0)
// enable TWI START Condition Bit (TWSTA = 1), MASTER
// disable TWI STOP Condition Bit (TWSTO = 0)
// disable TWI Write Collision Flag (TWWC = 0)
// enable i2c (TWEN = 1)
// enable TWI Interrupt (TWIE = 1)
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE);
}
 
/****************************************
* Stop I2C
****************************************/
void I2C_Stop(uint8_t start_state) {
twi_state = start_state;
// TWI Control Register
// clear TWI interrupt flag (TWINT=1)
// disable TWI Acknowledge Bit (TWEA = 0)
// diable TWI START Condition Bit (TWSTA = 1), no MASTER
// enable TWI STOP Condition Bit (TWSTO = 1)
// disable TWI Write Collision Flag (TWWC = 0)
// enable i2c (TWEN = 1)
// disable TWI Interrupt (TWIE = 0)
TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN);
}
 
/****************************************
* Write to I2C
****************************************/
void I2C_WriteByte(int8_t byte) {
// move byte to send into TWI Data Register
TWDR = byte;
// clear interrupt flag (TWINT = 1)
// enable i2c bus (TWEN = 1)
// enable interrupt (TWIE = 1)
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
}
 
/****************************************
* Receive byte and send ACK
****************************************/
void I2C_ReceiveByte(void) {
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);
}
 
/****************************************
* I2C receive last byte and send no ACK
****************************************/
void I2C_ReceiveLastByte(void){
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
}
 
/****************************************
* Reset I2C
****************************************/
void I2C_Reset(void) {
// stop i2c bus
I2C_Stop(TWI_STATE_MOTOR_TX);
twi_state = 0;
motor_write = TWDR;
motor_write = 0;
motor_read = 0;
TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset
TWAMR = 0;
TWAR = 0;
TWDR = 0;
TWSR = 0;
TWBR = 0;
I2C_init();
I2C_Start(TWI_STATE_MOTOR_TX);
}
 
/****************************************
* I2C ISR
****************************************/
ISR (TWI_vect) {
static uint8_t missing_motor = 0;
switch (twi_state++) { // First i2c_start from SendMotorData()
// Master Transmit
case 0: // TWI_STATE_MOTOR_TX
// skip motor if not used in mixer
while((Mixer.Motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write < MAX_MOTORS)) motor_write++;
if(motor_write >= MAX_MOTORS) { // writing finished, read now
motor_write = 0;
twi_state = TWI_STATE_MOTOR_RX;
I2C_WriteByte(0x53 + (motor_read * 2)); // select slave adress in rx mode
}
else I2C_WriteByte(0x52 + (motor_write * 2)); // select slave adress in tx mode
break;
case 1: // Send Data to Slave
I2C_WriteByte(Motor[motor_write].SetPoint); // transmit rotation rate setpoint
break;
case 2: // repeat case 0+1 for all motors
if(TWSR == TW_MT_DATA_NACK) { // Data transmitted, NACK received
if(!missing_motor) missing_motor = motor_write + 1;
if(++Motor[motor_write].Error == 0) Motor[motor_write].Error = 255; // increment error counter and handle overflow
}
I2C_Stop(TWI_STATE_MOTOR_TX);
I2CTimeout = 10;
motor_write++; // next motor
I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive
break;
// Master Receive Data
case 3:
if(TWSR != TW_MR_SLA_ACK) { // SLA+R transmitted, if not ACK received
// no response from the addressed slave received
Motor[motor_read].Present = 0;
motor_read++; // next motor
if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one
I2C_Stop(TWI_STATE_MOTOR_TX);
} else {
Motor[motor_read].Present = ('1' - '-') + motor_read;
I2C_ReceiveByte(); //Transmit 1st byte
}
missingMotor = missing_motor;
missing_motor = 0;
break;
case 4: //Read 1st byte and transmit 2nd Byte
Motor[motor_read].Current = TWDR;
I2C_ReceiveLastByte(); // nack
break;
case 5:
//Read 2nd byte
Motor[motor_read].MaxPWM = TWDR;
motor_read++; // next motor
if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one
I2C_Stop(TWI_STATE_MOTOR_TX);
break;
// Writing ADC values.
case 7:
I2C_WriteByte(0x98); // Address the DAC
break;
case 8:
I2C_WriteByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C)
break;
case 9:
I2C_WriteByte(DACValues[DACChannel]);
break;
case 10:
I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80
break;
case 11:
I2C_Stop(TWI_STATE_MOTOR_TX);
I2CTimeout = 10;
// repeat case 7...10 until all DAC Channels are updated
if(DACChannel < 2) {
DACChannel ++; // jump to next channel
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel
} else {
DACChannel = 0; // reset dac channel counter
}
break;
default:
I2C_Stop(TWI_STATE_MOTOR_TX);
I2CTimeout = 10;
motor_write = 0;
motor_read = 0;
}
}
 
extern void twi_diagnostics(void) {
// Check connected BL-Ctrls
uint8_t i;
printf("\n\rFound BL-Ctrl: ");
for(i = 0; i < MAX_MOTORS; i++) {
Motor[i].SetPoint = 0;
}
 
I2C_Start(TWI_STATE_MOTOR_TX);
_delay_ms(2);
motor_read = 0; // read the first I2C-Data
 
for(i = 0; i < MAX_MOTORS; i++) {
I2C_Start(TWI_STATE_MOTOR_TX);
_delay_ms(2);
if(Motor[i].Present) printf("%d ",i+1);
}
 
for(i = 0; i < MAX_MOTORS; i++) {
if(!Motor[i].Present && Mixer.Motor[i][MIX_THROTTLE] > 0) printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1);
Motor[i].Error = 0;
}
}
/branches/dongfang_FC_rewrite/twimaster.h
0,0 → 1,35
#ifndef _I2C_MASTER_H
#define _I2C_MASTER_H
 
#include <inttypes.h>
 
#define TWI_STATE_MOTOR_TX 0
#define TWI_STATE_MOTOR_RX 3
#define TWI_STATE_GYRO_OFFSET_TX 7
 
extern volatile uint8_t twi_state;
 
extern uint8_t missingMotor;
 
volatile extern uint8_t DACValues[4];
 
typedef struct {
uint8_t SetPoint; // written by attitude controller
uint8_t Present; // 0 if BL was found
uint8_t Error; // I2C error counter
uint8_t Current; // read byck from BL
uint8_t MaxPWM; // read back from BL
} __attribute__((packed)) MotorData_t;
 
#define MAX_MOTORS 12
extern MotorData_t Motor[MAX_MOTORS];
 
extern volatile uint16_t I2CTimeout;
 
extern void I2C_init (void); // Initialize I2C
extern void I2C_Start(uint8_t start_state); // Start I2C
extern void I2C_Stop (uint8_t start_state); // Stop I2C
extern void I2C_Reset(void); // Reset I2C
extern void twi_diagnostics(void);
 
#endif
/branches/dongfang_FC_rewrite/uart0.c
0,0 → 1,677
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <avr/pgmspace.h>
#include <stdarg.h>
#include <string.h>
 
#include "eeprom.h"
#include "menu.h"
#include "timer0.h"
#include "uart0.h"
#include "attitude.h"
#include "rc.h"
#include "externalControl.h"
 
#if defined (USE_MK3MAG)
#include "ubx.h"
#endif
#ifdef USE_MK3MAG
#include "mk3mag.h"
#endif
 
#define FC_ADDRESS 1
#define NC_ADDRESS 2
#define MK3MAG_ADDRESS 3
 
#define FALSE 0
#define TRUE 1
//int8_t test __attribute__ ((section (".noinit")));
uint8_t Request_VerInfo = FALSE;
uint8_t Request_ExternalControl = FALSE;
uint8_t Request_Display = FALSE;
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 DisplayLine = 0;
 
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
volatile uint8_t rxd_buffer_locked = FALSE;
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
volatile uint8_t txd_complete = TRUE;
volatile uint8_t ReceivedBytes = 0;
volatile uint8_t *pRxData = 0;
volatile uint8_t RxDataLen = 0;
 
uint8_t motorTestActive = 0;
uint8_t motorTest[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
uint8_t ConfirmFrame;
 
typedef struct {
int16_t Heading;
} __attribute__((packed)) Heading_t;
 
DebugOut_t DebugOut;
Data3D_t Data3D;
UART_VersionInfo_t UART_VersionInfo;
 
uint16_t DebugData_Timer;
uint16_t Data3D_Timer;
uint16_t DebugData_Interval = 500; // in 1ms
uint16_t Data3D_Interval = 0; // in 1ms
 
#ifdef USE_MK3MAG
int16_t Compass_Timer;
#endif
 
// keep lables in flash to save 512 bytes of sram space
const prog_uint8_t ANALOG_LABEL[32][16] = {
//1234567890123456
"AnglePitch ", //0
"AngleRoll ",
"AngleYaw ",
"GyroPitch ",
"GyroRoll ",
"GyroYaw ", //5
"ACGyroPitch ",
"ACGyroRoll ",
"ACGyroYaw ",
"SetPointYaw ",
"YawRateIFactor ", //10
"Gyro I Factor ",
"R/C Var 0 ",
"User Param 0 ",
"R/C Var 4 ",
"User Param 4 ", //15
"RCQuality ",
"Ext. Quality ",
"Looping ",
"throttleTerm ",
"pitchTerm ", //20
"rollTerm ",
"yawTerm ",
"P Pitch ",
"I Pitch ",
"P+D Pitch ", //25
"Pitch acc noise ",
"Roll acc noise ",
"Pitch Corr ",
"Roll Corr ",
"Pitch Noise ", //30
"Roll Noise "
};
 
/****************************************************************/
/* Initialization of the USART0 */
/****************************************************************/
void USART0_Init (void) {
uint8_t sreg = SREG;
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1);
// disable all interrupts before configuration
cli();
// disable RX-Interrupt
UCSR0B &= ~(1 << RXCIE0);
// disable TX-Interrupt
UCSR0B &= ~(1 << TXCIE0);
// set direction of RXD0 and TXD0 pins
// set RXD0 (PD0) as an input pin
PORTD |= (1 << PORTD0);
DDRD &= ~(1 << DDD0);
// set TXD0 (PD1) as an output pin
PORTD |= (1 << PORTD1);
DDRD |= (1 << DDD1);
// USART0 Baud Rate Register
// set clock divider
UBRR0H = (uint8_t)(ubrr >> 8);
UBRR0L = (uint8_t)ubrr;
// USART0 Control and Status Register A, B, C
// enable double speed operation in
UCSR0A |= (1 << U2X0);
// enable receiver and transmitter in
UCSR0B = (1 << TXEN0) | (1 << RXEN0);
// set asynchronous mode
UCSR0C &= ~(1 << UMSEL01);
UCSR0C &= ~(1 << UMSEL00);
// no parity
UCSR0C &= ~(1 << UPM01);
UCSR0C &= ~(1 << UPM00);
// 1 stop bit
UCSR0C &= ~(1 << USBS0);
// 8-bit
UCSR0B &= ~(1 << UCSZ02);
UCSR0C |= (1 << UCSZ01);
UCSR0C |= (1 << UCSZ00);
// flush receive buffer
while ( UCSR0A & (1<<RXC0) ) UDR0;
// enable interrupts at the end
// enable RX-Interrupt
UCSR0B |= (1 << RXCIE0);
// enable TX-Interrupt
UCSR0B |= (1 << TXCIE0);
// initialize the debug timer
DebugData_Timer = SetDelay(DebugData_Interval);
// unlock rxd_buffer
rxd_buffer_locked = FALSE;
pRxData = 0;
RxDataLen = 0;
// no bytes to send
txd_complete = TRUE;
#ifdef USE_MK3MAG
Compass_Timer = SetDelay(220);
#endif
UART_VersionInfo.SWMajor = VERSION_MAJOR;
UART_VersionInfo.SWMinor = VERSION_MINOR;
UART_VersionInfo.SWPatch = VERSION_PATCH;
UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
// restore global interrupt flags
SREG = sreg;
}
 
/****************************************************************/
/* USART0 transmitter ISR */
/****************************************************************/
ISR(USART0_TX_vect) {
static uint16_t ptr_txd_buffer = 0;
uint8_t tmp_tx;
if(!txd_complete) { // transmission not completed
ptr_txd_buffer++; // die [0] wurde schon gesendet
tmp_tx = txd_buffer[ptr_txd_buffer];
// if terminating character or end of txd buffer was reached
if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN)) {
ptr_txd_buffer = 0; // reset txd pointer
txd_complete = 1; // stop transmission
}
UDR0 = tmp_tx; // send current byte will trigger this ISR again
}
// transmission completed
else ptr_txd_buffer = 0;
}
 
/****************************************************************/
/* USART0 receiver ISR */
/****************************************************************/
ISR(USART0_RX_vect) {
static uint16_t crc;
static uint8_t ptr_rxd_buffer = 0;
uint8_t crc1, crc2;
uint8_t c;
 
c = UDR0; // catch the received byte
 
#if (defined (USE_MK3MAG))
// If the cpu is not an Atmega644P the ublox module should be conneced to rxd of the 1st uart.
if(CPUType != ATMEGA644P) ubx_parser(c);
#endif
if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return
 
// the rxd buffer is unlocked
if((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received
rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
crc = c; // init crc
}
#if 0
else if (ptr_rxd_buffer == 1) { // handle address
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
crc += c; // update crc
}
#endif
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes
if(c != '\r') { // no termination character
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
crc += c; // update crc
} else { // termination character was received
// the last 2 bytes are no subject for checksum calculation
// they are the checksum itself
crc -= rxd_buffer[ptr_rxd_buffer-2];
crc -= rxd_buffer[ptr_rxd_buffer-1];
// calculate checksum from transmitted data
crc %= 4096;
crc1 = '=' + crc / 64;
crc2 = '=' + crc % 64;
// compare checksum to transmitted checksum bytes
if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1])) {
// checksum valid
rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes
rxd_buffer_locked = TRUE; // lock the rxd buffer
// if 2nd byte is an 'R' enable watchdog that will result in an reset
if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando
} else { // checksum invalid
rxd_buffer_locked = FALSE; // unlock rxd buffer
}
ptr_rxd_buffer = 0; // reset rxd buffer pointer
}
} else { // rxd buffer overrun
ptr_rxd_buffer = 0; // reset rxd buffer
rxd_buffer_locked = FALSE; // unlock rxd buffer
}
}
 
// --------------------------------------------------------------------------
void AddCRC(uint16_t datalen) {
uint16_t tmpCRC = 0, i;
for(i = 0; i < datalen; i++) {
tmpCRC += txd_buffer[i];
}
tmpCRC %= 4096;
txd_buffer[i++] = '=' + tmpCRC / 64;
txd_buffer[i++] = '=' + tmpCRC % 64;
txd_buffer[i++] = '\r';
txd_complete = FALSE;
UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
}
 
// --------------------------------------------------------------------------
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
va_list ap;
uint16_t pt = 0;
uint8_t a,b,c;
uint8_t ptr = 0;
uint8_t *pdata = 0;
int len = 0;
txd_buffer[pt++] = '#'; // Start character
txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...)
txd_buffer[pt++] = cmd; // Command
va_start(ap, numofbuffers);
 
if(numofbuffers) {
pdata = va_arg(ap, uint8_t*);
len = va_arg(ap, int);
ptr = 0;
numofbuffers--;
}
 
while(len){
if(len) {
a = pdata[ptr++];
len--;
if((!len) && numofbuffers) {
pdata = va_arg(ap, uint8_t*);
len = va_arg(ap, int);
ptr = 0;
numofbuffers--;
}
}
else a = 0;
if(len) {
b = pdata[ptr++];
len--;
if((!len) && numofbuffers) {
pdata = va_arg(ap, uint8_t*);
len = va_arg(ap, int);
ptr = 0;
numofbuffers--;
}
} else b = 0;
if(len) {
c = pdata[ptr++];
len--;
if((!len) && numofbuffers) {
pdata = va_arg(ap, uint8_t*);
len = va_arg(ap, int);
ptr = 0;
numofbuffers--;
}
}
else c = 0;
txd_buffer[pt++] = '=' + (a >> 2);
txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
txd_buffer[pt++] = '=' + ( c & 0x3f);
}
va_end(ap);
AddCRC(pt); // add checksum after data block and initates the transmission
}
 
// --------------------------------------------------------------------------
void Decode64(void) {
uint8_t a,b,c,d;
uint8_t x,y,z;
uint8_t ptrIn = 3;
uint8_t ptrOut = 3;
uint8_t len = ReceivedBytes - 6;
while(len) {
a = rxd_buffer[ptrIn++] - '=';
b = rxd_buffer[ptrIn++] - '=';
c = rxd_buffer[ptrIn++] - '=';
d = rxd_buffer[ptrIn++] - '=';
//if(ptrIn > ReceivedBytes - 3) break;
x = (a << 2) | (b >> 4);
y = ((b & 0x0f) << 4) | (c >> 2);
z = ((c & 0x03) << 6) | d;
if(len--) rxd_buffer[ptrOut++] = x; else break;
if(len--) rxd_buffer[ptrOut++] = y; else break;
if(len--) rxd_buffer[ptrOut++] = z; else break;
}
pRxData = &rxd_buffer[3];
RxDataLen = ptrOut - 3;
}
 
// --------------------------------------------------------------------------
void USART0_ProcessRxData(void) {
// if data in the rxd buffer are not locked immediately return
if(!rxd_buffer_locked) return;
uint8_t tempchar1, tempchar2;
Decode64(); // decode data block in rxd_buffer
switch(rxd_buffer[1] - 'a') {
case FC_ADDRESS:
switch(rxd_buffer[2]) {
#ifdef USE_MK3MAG
case 'K':// compass value
CompassHeading = ((Heading_t *)pRxData)->Heading;
CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
break;
#endif
case 't':// motor test
if(RxDataLen > 20) //
{
memcpy(&motorTest[0], (uint8_t*)pRxData, sizeof(motorTest));
}
else
{
memcpy(&motorTest[0], (uint8_t*)pRxData, 4);
}
//Request_MotorTest = TRUE;
motorTestActive = 255;
externalControlActive = 255;
break;
case 'n':// "Get Mixer Table
while(!txd_complete); // wait for previous frame to be sent
SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer));
break;
 
case 'm':// "Set Mixer Table
if(pRxData[0] == EEMIXER_REVISION)
{
memcpy(&Mixer, (uint8_t*)pRxData, sizeof(Mixer));
MixerTable_WriteToEEProm();
while(!txd_complete); // wait for previous frame to be sent
tempchar1 = 1;
}
else
{
tempchar1 = 0;
}
SendOutData('M', FC_ADDRESS, 1, &tempchar1, 1);
break;
 
case 'p': // get PPM channels
Request_PPMChannels = TRUE;
break;
 
case 'q':// request settings
if(pRxData[0] == 0xFF)
{
pRxData[0] = GetParamByte(PID_ACTIVE_SET);
}
// limit settings range
if(pRxData[0] < 1) pRxData[0] = 1; // limit to 1
else if(pRxData[0] > 5) pRxData[0] = 5; // limit to 5
// load requested parameter set
ParamSet_ReadFromEEProm(pRxData[0]);
tempchar1 = pRxData[0];
tempchar2 = EEPARAM_REVISION;
while(!txd_complete); // wait for previous frame to be sent
SendOutData('Q', FC_ADDRESS,3, &tempchar1, sizeof(tempchar1), &tempchar2, sizeof(tempchar2), (uint8_t *) &staticParams, sizeof(staticParams));
break;
 
case 's': // save settings
if(!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors ar off
{
if((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EEPARAM_REVISION)) // check for setting to be in range and version of settings
{
memcpy(&staticParams, (uint8_t*)&pRxData[2], sizeof(staticParams));
ParamSet_WriteToEEProm(pRxData[0]);
/*
TODO: Remove this encapsulation breach
turnOver180Pitch = (int32_t) staticParams.AngleTurnOverPitch * 2500L;
turnOver180Roll = (int32_t) staticParams.AngleTurnOverRoll * 2500L;
*/
tempchar1 = getActiveParamSet();
beepNumber(tempchar1);
}
else
{
tempchar1 = 0; //indicate bad data
}
while(!txd_complete); // wait for previous frame to be sent
SendOutData('S', FC_ADDRESS,1, &tempchar1, sizeof(tempchar1));
}
break;
 
default:
//unsupported command received
break;
} // case FC_ADDRESS:
 
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;
externalControlActive = 255;
break;
 
case 'b': // submit extern control
memcpy(&externalControl, (uint8_t*)pRxData, sizeof(externalControl));
ConfirmFrame = externalControl.frame;
externalControlActive = 255;
break;
 
case 'h':// request for display columns
externalControlActive = 255;
RemoteKeys |= pRxData[0];
if(RemoteKeys) DisplayLine = 0;
Request_Display = TRUE;
break;
 
case 'l':// request for display columns
externalControlActive = 255;
MenuItem = pRxData[0];
Request_Display1 = TRUE;
break;
 
case 'v': // request for version and board release
Request_VerInfo = TRUE;
break;
 
case 'g':// get external control data
Request_ExternalControl = TRUE;
break;
 
case 'd': // request for the debug data
DebugData_Interval = (uint16_t) pRxData[0] * 10;
if(DebugData_Interval > 0) Request_DebugData = TRUE;
break;
 
case 'c': // request for the 3D data
Data3D_Interval = (uint16_t) pRxData[0] * 10;
if(Data3D_Interval > 0) Request_Data3D = TRUE;
break;
 
default:
//unsupported command received
break;
}
break; // default:
}
// unlock the rxd buffer after processing
pRxData = 0;
RxDataLen = 0;
rxd_buffer_locked = FALSE;
}
 
//############################################################################
//Routine für die Serielle Ausgabe
//############################################################################
int16_t uart_putchar (int8_t c) {
if (c == '\n')
uart_putchar('\r');
// wait until previous character was send
loop_until_bit_is_set(UCSR0A, UDRE0);
// send character
UDR0 = c;
return (0);
}
 
//---------------------------------------------------------------------------------------------
void USART0_TransmitTxData(void) {
if(!txd_complete) return;
 
if(Request_VerInfo && txd_complete) {
SendOutData('V', FC_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, sizeof(UART_VersionInfo));
Request_VerInfo = FALSE;
}
if(Request_Display && txd_complete) {
LCD_PrintMenu();
SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20);
DisplayLine++;
if(DisplayLine >= 4) DisplayLine = 0;
Request_Display = FALSE;
}
if(Request_Display1 && txd_complete) {
LCD_PrintMenu();
SendOutData('L', FC_ADDRESS, 3, &MenuItem, sizeof(MenuItem), &MaxMenuItem, sizeof(MaxMenuItem), DisplayBuff, sizeof(DisplayBuff));
Request_Display1 = FALSE;
}
if(Request_DebugLabel != 0xFF) { // 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;
}
if(ConfirmFrame && txd_complete) { // Datensatz ohne CRC bestätigen
SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame));
ConfirmFrame = 0;
}
if(((DebugData_Interval && CheckDelay(DebugData_Timer)) || Request_DebugData) && txd_complete) {
SendOutData('D', FC_ADDRESS, 1,(uint8_t *) &DebugOut, sizeof(DebugOut));
DebugData_Timer = SetDelay(DebugData_Interval);
Request_DebugData = FALSE;
}
if( ((Data3D_Interval && CheckDelay(Data3D_Timer)) || Request_Data3D) && txd_complete) {
SendOutData('C', FC_ADDRESS, 1,(uint8_t *) &Data3D, sizeof(Data3D));
Data3D.AngleNick = (int16_t)((10 * pitchAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
Data3D.AngleRoll = (int16_t)((10 * rollAngle) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
Data3D.Heading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1°
Data3D_Timer = SetDelay(Data3D_Interval);
Request_Data3D = FALSE;
}
 
if(Request_ExternalControl && txd_complete) {
SendOutData('G', FC_ADDRESS, 1,(uint8_t *) &externalControl, sizeof(externalControl));
Request_ExternalControl = FALSE;
}
 
#ifdef USE_MK3MAG
if((CheckDelay(Compass_Timer)) && txd_complete) {
ToMk3Mag.Attitude[0] = (int16_t)((10 * pitchAngle) / GYRO_DEG_FACTOR_PITCH_ROLL); // approx. 0.1 deg
ToMk3Mag.Attitude[1] = (int16_t)((10 * rollAngle) / GYRO_DEG_FACTOR_PITCH_ROLL); // approx. 0.1 deg
ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0];
ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1];
ToMk3Mag.CalState = CompassCalState;
SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag));
// the last state is 5 and should be send only once to avoid multiple flash writing
if(CompassCalState > 4) CompassCalState = 0;
Compass_Timer = SetDelay(99);
}
#endif
if(Request_MotorTest && txd_complete) {
SendOutData('T', FC_ADDRESS, 0);
Request_MotorTest = FALSE;
}
 
if(Request_PPMChannels && txd_complete) {
SendOutData('P', FC_ADDRESS, 1, (uint8_t *)&PPM_in, sizeof(PPM_in));
Request_PPMChannels = FALSE;
}
}
/branches/dongfang_FC_rewrite/uart0.h
0,0 → 1,47
#ifndef _UART0_H
#define _UART0_H
 
#define RXD_BUFFER_LEN 150
// must be at least 4('#'+Addr+'CmdID'+'\r')+ (80 * 4)/3 = 111 bytes
#define TXD_BUFFER_LEN 150
#define RXD_BUFFER_LEN 150
 
#include <inttypes.h>
 
//Baud rate of the USART
#define USART0_BAUD 57600
 
extern void USART0_Init (void);
extern void USART0_TransmitTxData(void);
extern void USART0_ProcessRxData(void);
extern int16_t uart_putchar(int8_t c);
 
extern uint8_t RemotePollDisplayLine;
 
extern uint8_t motorTestActive;
extern uint8_t motorTest[16];
 
typedef struct {
uint8_t Digital[2];
uint16_t Analog[32]; // Debugvalues
} __attribute__((packed)) DebugOut_t;
 
extern DebugOut_t DebugOut;
 
typedef struct {
int16_t AngleNick; // in 0.1 deg
int16_t AngleRoll; // in 0.1 deg
int16_t Heading; // in 0.1 deg
uint8_t reserve[8];
} __attribute__((packed)) Data3D_t;
 
typedef struct {
uint8_t SWMajor;
uint8_t SWMinor;
uint8_t ProtoMajor;
uint8_t ProtoMinor;
uint8_t SWPatch;
uint8_t Reserved[5];
} __attribute__((packed)) UART_VersionInfo_t;
 
#endif //_UART0_H
/branches/dongfang_FC_rewrite/uart1.c
0,0 → 1,168
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <avr/io.h>
#include <avr/interrupt.h>
#include "uart1.h"
#include "configuration.h"
#if defined (USE_MK3MAG)
#include "ubx.h"
#else
#ifdef USE_RC_DSL
#include "dsl.h"
#endif
#ifdef USE_RC_SPECTRUM
#include "spectrum.h"
#endif
#endif
 
#ifndef USART1_BAUD
#define USART1_BAUD 57600
#endif
 
/****************************************************************/
/* Initialization of the USART1 */
/****************************************************************/
void USART1_Init (void) {
// USART1 Control and Status Register A, B, C and baud rate register
uint8_t sreg = SREG;
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1);
 
// disable all interrupts before reconfiguration
cli();
 
// disable RX-Interrupt
UCSR1B &= ~(1 << RXCIE1);
// disable TX-Interrupt
UCSR1B &= ~(1 << TXCIE1);
// disable DRE-Interrupt
UCSR1B &= ~(1 << UDRIE1);
 
// set direction of RXD1 and TXD1 pins
// set RXD1 (PD2) as an input pin
PORTD |= (1 << PORTD2);
DDRD &= ~(1 << DDD2);
 
// set TXD1 (PD3) as an output pin
PORTD |= (1 << PORTD3);
DDRD |= (1 << DDD3);
 
// USART0 Baud Rate Register
// set clock divider
UBRR1H = (uint8_t)(ubrr>>8);
UBRR1L = (uint8_t)ubrr;
 
// enable double speed operation
UCSR1A |= (1 << U2X1);
// enable receiver and transmitter
UCSR1B = (1 << TXEN1) | (1 << RXEN1);
// set asynchronous mode
UCSR1C &= ~(1 << UMSEL11);
UCSR1C &= ~(1 << UMSEL10);
// no parity
UCSR1C &= ~(1 << UPM11);
UCSR1C &= ~(1 << UPM10);
// 1 stop bit
UCSR1C &= ~(1 << USBS1);
// 8-bit
UCSR1B &= ~(1 << UCSZ12);
UCSR1C |= (1 << UCSZ11);
UCSR1C |= (1 << UCSZ10);
 
// flush receive buffer explicit
while ( UCSR1A & (1<<RXC1) ) UDR1;
 
// enable interrupts at the end
// enable RX-Interrupt
UCSR1B |= (1 << RXCIE1);
// enable TX-Interrupt
//UCSR1B |= (1 << TXCIE1);
// enable DRE interrupt
//UCSR1B |= (1 << UDRIE1);
 
// restore global interrupt flags
SREG = sreg;
}
 
/****************************************************************/
/* USART1 data register empty ISR */
/****************************************************************/
/*ISR(USART1_UDRE_vect)
{
 
}
*/
 
/****************************************************************/
/* USART1 transmitter ISR */
/****************************************************************/
/*ISR(USART1_TX_vect)
{
 
}
*/
/****************************************************************/
/* USART1 receiver ISR */
/****************************************************************/
ISR(USART1_RX_vect)
{
uint8_t c;
c = UDR1; // get data byte
#if defined (USE_MK3MAG)
ubx_parser(c); // and put it into the ubx protocol parser
#else
#ifdef USE_RC_DSL
dsl_parser(c); // parse dsl data stream
#endif
#ifdef USE_RC_SPECTRUM
spectrum_parser(c); // parse spectrum data stream
#endif
#endif
}
/branches/dongfang_FC_rewrite/uart1.h
0,0 → 1,8
#ifndef _UART1_H
#define _UART1_H
 
 
extern void USART1_Init (void);
 
 
#endif //_UART1_H
/branches/dongfang_FC_rewrite/ubx.c
0,0 → 1,288
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <inttypes.h>
#include "ubx.h"
#include <avr/io.h>
 
//#include "uart0.h"
 
// ubx protocol parser state machine
#define UBXSTATE_IDLE 0
#define UBXSTATE_SYNC1 1
#define UBXSTATE_SYNC2 2
#define UBXSTATE_CLASS 3
#define UBXSTATE_LEN1 4
#define UBXSTATE_LEN2 5
#define UBXSTATE_DATA 6
#define UBXSTATE_CKA 7
#define UBXSTATE_CKB 8
 
// ublox protocoll identifier
#define UBX_CLASS_NAV 0x01
 
#define UBX_ID_POSLLH 0x02
#define UBX_ID_SOL 0x06
#define UBX_ID_VELNED 0x12
 
#define UBX_SYNC1_CHAR 0xB5
#define UBX_SYNC2_CHAR 0x62
 
typedef struct {
uint32_t ITOW; // ms GPS Millisecond Time of Week
int32_t Frac; // ns remainder of rounded ms above
int16_t week; // GPS week
uint8_t GPSfix; // GPSfix Type, range 0..6
uint8_t Flags; // Navigation Status Flags
int32_t ECEF_X; // cm ECEF X coordinate
int32_t ECEF_Y; // cm ECEF Y coordinate
int32_t ECEF_Z; // cm ECEF Z coordinate
uint32_t PAcc; // cm 3D Position Accuracy Estimate
int32_t ECEFVX; // cm/s ECEF X velocity
int32_t ECEFVY; // cm/s ECEF Y velocity
int32_t ECEFVZ; // cm/s ECEF Z velocity
uint32_t SAcc; // cm/s Speed Accuracy Estimate
uint16_t PDOP; // 0.01 Position DOP
uint8_t res1; // reserved
uint8_t numSV; // Number of SVs used in navigation solution
uint32_t res2; // reserved
Status_t Status;
} UBX_SOL_t;
 
typedef struct {
uint32_t ITOW; // ms GPS Millisecond Time of Week
int32_t LON; // 1e-07 deg Longitude
int32_t LAT; // 1e-07 deg Latitude
int32_t HEIGHT; // mm Height above Ellipsoid
int32_t HMSL; // mm Height above mean sea level
uint32_t Hacc; // mm Horizontal Accuracy Estimate
uint32_t Vacc; // mm Vertical Accuracy Estimate
Status_t Status;
} UBX_POSLLH_t;
 
typedef struct {
uint32_t ITOW; // ms GPS Millisecond Time of Week
int32_t VEL_N; // cm/s NED north velocity
int32_t VEL_E; // cm/s NED east velocity
int32_t VEL_D; // cm/s NED down velocity
uint32_t Speed; // cm/s Speed (3-D)
uint32_t GSpeed; // cm/s Ground Speed (2-D)
int32_t Heading; // 1e-05 deg Heading 2-D
uint32_t SAcc; // cm/s Speed Accuracy Estimate
uint32_t CAcc; // deg Course / Heading Accuracy Estimate
Status_t Status;
} UBX_VELNED_t;
 
UBX_SOL_t UbxSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
UBX_POSLLH_t UbxPosLlh = {0,0,0,0,0,0,0, INVALID};
UBX_VELNED_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
GPS_INFO_t GPSInfo = {0,0,0,0,0,0,0,0,0,0, INVALID};
 
volatile uint8_t GPSTimeout = 0;
 
void UpdateGPSInfo (void)
{
 
if ((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA))
{
RED_FLASH;
if(GPSInfo.status != NEWDATA)
{
GPSInfo.status = INVALID;
// NAV SOL
GPSInfo.flags = UbxSol.Flags;
GPSInfo.satfix = UbxSol.GPSfix;
GPSInfo.satnum = UbxSol.numSV;
GPSInfo.PAcc = UbxSol.PAcc;
GPSInfo.VAcc = UbxSol.SAcc;
// NAV POSLLH
GPSInfo.longitude = UbxPosLlh.LON;
GPSInfo.latitude = UbxPosLlh.LAT;
GPSInfo.altitude = UbxPosLlh.HEIGHT;
 
GPSInfo.veleast = UbxVelNed.VEL_E;
GPSInfo.velnorth = UbxVelNed.VEL_N;
GPSInfo.veltop = -UbxVelNed.VEL_D;
GPSInfo.velground = UbxVelNed.GSpeed;
 
GPSInfo.status = NEWDATA;
 
}
// set state to collect new data
UbxSol.Status = PROCESSED; // never update old data
UbxPosLlh.Status = PROCESSED; // never update old data
UbxVelNed.Status = PROCESSED; // never update old data
}
 
 
}
 
 
// this function should be called within the UART RX ISR
void ubx_parser(uint8_t c)
{
static uint8_t ubxstate = UBXSTATE_IDLE;
static uint8_t cka, ckb;
static uint16_t msglen;
static int8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered
 
switch(ubxstate)
{
case UBXSTATE_IDLE: // check 1st sync byte
if (c == UBX_SYNC1_CHAR) ubxstate = UBXSTATE_SYNC1;
else ubxstate = UBXSTATE_IDLE; // out of synchronization
break;
 
case UBXSTATE_SYNC1: // check 2nd sync byte
if (c == UBX_SYNC2_CHAR) ubxstate = UBXSTATE_SYNC2;
else ubxstate = UBXSTATE_IDLE; // out of synchronization
break;
 
case UBXSTATE_SYNC2: // check msg class to be NAV
if (c == UBX_CLASS_NAV) ubxstate = UBXSTATE_CLASS;
else ubxstate = UBXSTATE_IDLE; // unsupported message class
break;
 
case UBXSTATE_CLASS: // check message identifier
switch(c)
{
case UBX_ID_POSLLH: // geodetic position
ubxP = (int8_t *)&UbxPosLlh; // data start pointer
ubxEp = (int8_t *)(&UbxPosLlh + 1); // data end pointer
ubxSp = (int8_t *)&UbxPosLlh.Status; // status pointer
break;
 
case UBX_ID_SOL: // navigation solution
ubxP = (int8_t *)&UbxSol; // data start pointer
ubxEp = (int8_t *)(&UbxSol + 1); // data end pointer
ubxSp = (int8_t *)&UbxSol.Status; // status pointer
break;
 
case UBX_ID_VELNED: // velocity vector in tangent plane
ubxP = (int8_t *)&UbxVelNed; // data start pointer
ubxEp = (int8_t *)(&UbxVelNed + 1); // data end pointer
ubxSp = (int8_t *)&UbxVelNed.Status; // status pointer
break;
 
default: // unsupported identifier
ubxstate = UBXSTATE_IDLE;
break;
}
if (ubxstate != UBXSTATE_IDLE)
{
ubxstate = UBXSTATE_LEN1;
cka = UBX_CLASS_NAV + c;
ckb = UBX_CLASS_NAV + cka;
}
break;
 
case UBXSTATE_LEN1: // 1st message length byte
msglen = c;
cka += c;
ckb += cka;
ubxstate = UBXSTATE_LEN2;
break;
 
case UBXSTATE_LEN2: // 2nd message length byte
msglen += ((uint16_t)c)<<8;
cka += c;
ckb += cka;
// if the old data are not processed so far then break parsing now
// to avoid writing new data in ISR during reading by another function
if ( *ubxSp == NEWDATA )
{
UpdateGPSInfo(); //update GPS info respectively
ubxstate = UBXSTATE_IDLE;
}
else // data invalid or allready processd
{
*ubxSp = INVALID;
ubxstate = UBXSTATE_DATA;
}
break;
 
case UBXSTATE_DATA:
if (ubxP < ubxEp) *ubxP++ = c; // copy curent data byte if any space is left
cka += c;
ckb += cka;
if (--msglen == 0) ubxstate = UBXSTATE_CKA; // switch to next state if all data was read
break;
 
case UBXSTATE_CKA:
if (c == cka) ubxstate = UBXSTATE_CKB;
else
{
*ubxSp = INVALID;
ubxstate = UBXSTATE_IDLE;
}
break;
 
case UBXSTATE_CKB:
if (c == ckb)
{
*ubxSp = NEWDATA; // new data are valid
UpdateGPSInfo(); //update GPS info respectively
GPSTimeout = 255;
}
else
{ // if checksum not fit then set data invalid
*ubxSp = INVALID;
}
ubxstate = UBXSTATE_IDLE; // ready to parse new data
break;
 
default: // unknown ubx state
ubxstate = UBXSTATE_IDLE;
break;
}
 
}
 
 
/branches/dongfang_FC_rewrite/ubx.h
0,0 → 1,61
#ifndef _UBX_H
#define _UBX_H
 
#include <inttypes.h>
 
 
typedef enum
{
INVALID,
NEWDATA,
PROCESSED
} Status_t;
 
// Satfix types for GPSData.satfix
#define SATFIX_NONE 0x00
#define SATFIX_DEADRECKOING 0x01
#define SATFIX_2D 0x02
#define SATFIX_3D 0x03
#define SATFIX_GPS_DEADRECKOING 0x04
#define SATFIX_TIMEONLY 0x05
// Flags for interpretation of the GPSData.flags
#define FLAG_GPSFIXOK 0x01 // (i.e. within DOP & ACC Masks)
#define FLAG_DIFFSOLN 0x02 // (is DGPS used)
#define FLAG_WKNSET 0x04 // (is Week Number valid)
#define FLAG_TOWSET 0x08 // (is Time of Week valid)
 
 
/* enable the UBX protocol at the gps receiver with the following messages enabled
01-02 NAV - POSLLH
01-06 Nav - SOL
01-12 NAV - VELNED */
 
typedef struct
{
uint8_t flags; // flags
uint8_t satnum; // number of satelites
uint8_t satfix; // type of satfix
int32_t longitude; // in 1e-07 deg
int32_t latitude; // in 1e-07 deg
int32_t altitude; // in mm
uint32_t PAcc; // in cm 3d position accuracy
int32_t velnorth; // in cm/s
int32_t veleast; // in cm/s
int32_t veltop; // in cm/s
uint32_t velground; // 2D ground speed in cm/s
uint32_t VAcc; // in cm/s 3d velocity accuracy
Status_t status; // status of data: invalid | valid
} GPS_INFO_t;
 
//here you will find the current gps info
extern GPS_INFO_t GPSInfo; // measured position (last gps record)
 
// this variable should be decremted by the application
extern volatile uint8_t GPSTimeout; // is reset to 255 if a new UBX msg was received
 
 
#define USART1_BAUD 57600
// this function should be called within the UART RX ISR
extern void ubx_parser(uint8_t c);
 
#endif //_UBX_H
/branches/dongfang_FC_rewrite/userparams.txt
0,0 → 1,24
Userparam 5: Filter control.
bits 0-1: Gyros 1st order
bits 2-3: Gyros 2nd order
bits 4-5: GyroD (in series with 1st order)
bits 6-7: Acc.
 
Userparam6: Motor smoothing.
0: No filter
1: 50% new 50% old
2: As H&I
3: Reverse H&I
 
Userparam7: Yaw I factor. Default 0 (!)
 
Userparam8: Acc integral correction Z axis limit. Default 0 (!)
 
 
Other params:
GyroAccFactor = Promille acc. i 0'te orden integralkorrektion. Default 5 (ENC-03)
GyroAccTrim = Offset korrektioner divideres med dette foer det laegges til offsets. Default 2.
DriftComp = Max offsetkorrektur pr. omgang (1/2 sek). Det er _foer_ division med GyroAccTrim! Default 32 (ENC-03)
 
Rotary rate limiter flag ON = dongfang axis coupling
Rotary rate limiter flag OFF = partial H&I
/branches/dongfang_FC_rewrite/version.txt
0,0 → 1,287
 
-------
V0.53 27.04.2007 H.Buss
- erste öffentliche Version
 
V0.53b 29.04.2007 H.Buss
- der FAKTOR_I war versehentlich auf Null, dann liegt der MikroKopter nicht so hart in der Luft
 
V0.53c 29.04.2007 H.Buss
- es gib ein Menü, in dem die Werte der Kanäle nach Nick, Roll, Gas,... sortiert sind.
Die angezeigten Werte waren nicht die Werte der Funke
 
V0.54 01.05.2007 H.Buss
- die Paramtersätze können jetzt vor dem Start ausgewählt werden
Dazu wird beim Kalibrieren der Messwerte (Gashebel oben links) der Nick-Rollhebel abgefragt:
2 3 4
1 x 5
- - -
Bedeutet: Nick-Rollhebel Links Mitte = Setting:1 Links Oben = Setting:2 usw.
- der Faktor_I für den Hauptregler ist hinzugekommen. Im Heading-Hold-Modus sollte er vergössert werden, was Stabilität bringt
 
V0.55 14.05.2007 H.Buss
- es können nun Servos an J3,J4,J5 mit den Kanälen 5-7 gesteuert werden
 
V0.56 14.05.2007 H.Buss
- es gab Probleme mit Funken, die mehr als 8 Kanäle haben, wenn mehrere Kanäle dann auf Null waren
- Funken, die nicht bis +-120 aussteuern können, sollten jetzt auch gehen
V0.57 24.05.2007 H.Buss
- Der Höhenregler kann nun auch mittels Schalter bedient werden
- Bug im Gier-Algorithmus behoben; Schnelles Gieren fürhrte dazu, dass der MK zu weit gedreht hat
- Kompass-Einfluss dämpfen bei Neigung
- Man kann zwischen Kompass FIX (Richtung beim Kalibrieren) und Variabel (einstellbar per Gier) wählen
- Der Motortest vom Kopter-Tool geht jetzt
- Man kann den Parametersätzen einen Namen geben
- Das Kamerasetting ist unter Setting 2 defaultmässig integriert
V0.58 30.05.2007 H.Buss
- Der Höhenregler-Algorithmus wird nun umgangen, wenn der Höhenreglerschalter aus ist
 
V0.60 17.08.2007 H.Buss
- "Schwindel-Bug" behoben
- Die Poti-Werte werden jetzt auf Unterlauf (<0) überprüft
- Poti4 zugefügt
- Es werden jetzt 8 Kanäle ausgewertet
- Kamera-Servo (an J7)
- Die Settings müssen überschrieben werden
V0.61 - V0.63 H.Buss 27.09.2007
- Poti 4 und Kanal 8 werden im Menü angezeigt
- ein paar Kleinigkeiten bei den DefaultKonstanten2 bereinigt
- Analog.c: Aktuell_ax korrigiert
- auf 32 Debug-Kanäle erweitert
- Loopings sind jetzt möglich und einzeln im KopterTool freischaltbar
- leichte Anpassungen im Gier - Geschwindigkeit und Drift
- die Hardwareversion V1.1 wird erkannt und das Programm stellt sich auf die geänderte Gyroverstärkung und die geänderten Portpins ein
- die Software startet nach dem Einschalten schneller, weil der Luftdruckoffset schneller gefunden wird
- die PPM-Ausgänge liegen wieder an den Pins an
- Details an der Sensordatenverarbeitung -> es fliegt sich geringfügig anders
- der MK ist bei wenig Gas nicht mehr so giftig -> soll das Landen vereinfachen
- I2C-Bus läuft jetzt sicher nach einer Störung wieder an
- Sticksignale werden präziser ausgewertet
- Stick-Kanäle werden ans Kopter-Tool übertragen
- Es muss die Version V1.47 des Kopter-Tool verwendet werden
- Die Settings werden auf Default zurückgesetzt
- am Piepen kann man die Fehlerart unterscheiden
1. einzelnes Piepen beim Einschalten und Kalibrieren
2. langsames Intervall mindestens 1 Sek -> Empfangsausfall
3. schnelleres Intervall mindestens 1 Sek -> Akku
4. sehr schnelles Intervall mindestens 1 Sek -> Kommunikation zu den Reglern gestört
V0.64 H.Buss 30.09.2007
- beim Gieren wurden die Achsen nicht hart genug geregelt
V0.65a H.Buss 15.10.2007
- Integral im Mischer wieder integriert
- Feinabstimmung im ACC/Gyro Abgleich -> 1/32 & 100
- ACC/Gyro Abgleich auch bei HH
 
V0.66a H.Buss 3.11.2007
- Messwertverarbeitung aus dem Analog-Interrupt entfernt
- Analogmessung hängt jetzt am FC-Timing
- Looping-Stick-Hysterese eingebaut
- Looping-180°-Umschlag einstellbar
- Achsenkopplung: Gierbewegung verkoppelt Nick und Roll
- Lageregelung nach ACC-Sensor verbessert
- zusätzlicher I-Anteil in der Lageregelung verbessert die Neutrallage
- Gyrodriftkompensation überarbeitet
- Bug in der Gier-Stick-Berechnung behoben
- Gyro-Messung auf 1kHz beschleunigt
V0.67a H.Buss 16.11.2007
- der Hauptregler-I-Anteil wirkt jetzt nur noch auf den Winkel (ausser im HH-Mode)
- Gyro-Acc-Abgleich jetzt wieder in jedem Zyklus
- Feinabstimmung
- Beim HH-Modus gab es noch Bugs
 
V0.67e H.Buss 29.11.2007
- Parameter: Dynamic Stability und Driftfaktor eingeführt
- Die Namen der Analogwerte werden jetzt zum Koptertool übertragen
- Kompatibilität zum Koptertool erhöht
 
V0.67f H.Buss 04.12.2007
- Das Integral des Hauptreglers wird jetzt linear entladen und nicht mehr proportional
- Schub für Gier wird jetzt auf den Gaswert begrenzt, dadurch steigt der MK nicht mehr beim Gieren. Gier ist allerdings nicht mehr so agressiv
- Die ACC-Nullwerte können jetzt dauerhaft im EEPROM gespeichert werden (Stick:Vollgas und Gier rechts)
V0.68a I.Busker 28.12.2007
- SPI.c & SPI.h ins Projekt aufgenommen
SPI-Kommuikation kann in SPI.h aktiviert/deaktivert werden
 
V0.68c H.Buss 05.01.2008
- Stickauswertung verbessert -> träger und präziser
- Alle Settings angepasst
V0.69g H.Buss 05.05.2008
- kleinere Bugs beseitigt
- Schneller Sinkflug jetzt möglich
- Min- und Maxgas in den Settings geändert
- Lagewinkel wird jetzt in 0,1 Grad an Kompass und Navi gesendet
- Kalibrierung für MK3Mag -> Nick unten beim Kalibrieren
- Kompassroutine um den Ersatzkompass (Gyro unterstützt Kompasswert) erweitert
V0.69h H.Buss 21.05.2008
- STICK_GAIN = 4 eingeführt. Das erhöht die Auflösung der Sollwerte. Stick_P und Stick_I müssen nun um Faktor 4 erhöht werden
- SenderOkay auch an das Naviboard übertragen
- Bessere Parameter bei Senderausfall
 
V0.69j H.Buss 30.05.2008
- Höhere Auflösung der Achsenkopplung
V0.69k H.Buss 31.05.2008
- Bug in SPI.C behoben
- in 0.69h war ein Bug, der zu ungewollten Loopings führen konnte
 
V0.69L H.Buss 14.06.2008
- feinere Cam-Servo-Auflösung
V0.70a H.Buss 01.07.2008
- Unterstützung der V1.3-Hardware mit automatischem Hardware-Gyro-Abgleich
 
V0.70b H.Buss 14.07.2008
- flexible Einstellungsmöglichkeit von J16 und J17 (Transistorausgänge)
- eigene Parameter für GPS-Naviboard
- eigener Parameter für ExternalControl (war vorher UserParameter1 bzw. 8)
- neue Parameter im EEPROM-Datensatz: J16Bitmask, J16Timing, ExternalControl, Navi...
- MikroKopterFlags eingeführt, damit das Navi den Status des MKs kennt
- KopterTool-Kompatibilität auf 8 erhöht
 
V0.70c H.Buss 30.07.2008
- Parameter der Datenfusion leicht modifiziert
- EEPROM-Parameter für Looping-Umschlag angepasst (von 100 auf 85)
- MaxStick wird auf 100 begrenzt
V0.70d H.Buss 02.08.2008
- Transistorausgänge: das oberste Bit der Blinkmaske (im KopterTool linkes Bit) gibt nun den Zustand des Ausgangs im Schalterbetrieb an
 
0.71b: H.Buss 19.10.2008
Kommunikation zum Navi erweitert
- Beeptime jetzt 32Bit
- Datenfusion und Driftkopensation wird durch NaviBoard unterstützt
 
0.71c: H.Buss 20.10.2008
- LoopConfig heisst jetzt BitConfig
- 3-Fach-Schalter für Höhensteuerung möglich -> kann man mit GPS-Schalter zusammenlegen
- bei den Settings wurde Setting[0] mit abgespeichert, welches es nicht gab.
- in Zukunft werden bei neuen EEPROM-Settings die Kanäle von Setting 1 übernommen
- Variablen NaviWindCorrection, NaviSpeedCompensation, NaviOperatingRadius eingeführt
 
0.71f: H.Buss 15.11.2008
- Ausschalten der Höhenregelung per Schalter um 0,3 sek verzögert
- bei der seriellen Übertragung hat die FC jetzt als SlaveAdresse die 1
- VersionInfo.NaviKompatibel eingeführt
- wenn manuell gegiert wird, wird der GyroKompass-Wert auf den Kompasswert gesetzt
- Luftdruckwert wird an das Navi übertragen
- Der Baro-Offset wird jetzt nachgeführt, um den Messbereich zu erweitern. Geht nur bei Höhenregler mit Schalter
- Debugdaten können jetzt mit 'f' gepollt werden
0.71g: Gregor 09.12.2008
- Kommunikation überarbeitet
Infos hier: http://www.mikrokopter.de/ucwiki/en/SerialCommands
0.71h: H.Buss 15.12.2008 - Freigegebene Version
- NaviAngleLimitation als Parameter zum Navi implementiert
- Antwort auf CMD: 't' entfernt
0.72d: H.Buss 22.01.2009
- OCTO als Compilerschalter
- Unterstützung der FC 2.0 (ME)
- GYRO_D eingeführt
- Achsenkopplung jetzt auch auf Nick/Roll-Bewegung
0.72e: H.Buss 27.01.2009
- die 0.72d hatte kein Integral im Gier
- Parameter eingeführt:
EE_Parameter.NaviGpsPLimit
EE_Parameter.NaviGpsILimit
EE_Parameter.NaviGpsDLimit
EE_Parameter.NaviPH_LoginTime
EE_Parameter.AchsKopplung2
EE_Parameter.CouplingYawCorrection
 
0.72f: H.Buss 28.01.2009
- Bug im Ersatzkompass entfernt
 
0.72h: H.Buss 05.02.2009
- Algorithmen beschleunigt -> Floats durch Fixkomma ersetzt
- Achsentkopplung weiter verbessert
- Nick- und Roll im Octo-Mischer auf jeweils vier Motoren aufgeteilt
 
0.72i: H.Buss 07.02.2009
- Abtastrate von 1kHz auf 2kHz erhöht
 
0.72j: H.Buss 09.02.2009
- neue Implementierung der Servoausgänge
 
0.72k: H.Buss 10.02.2009
- Abtastrate auf 5kHz erhöht
 
0.72l: H.Buss 13.02.2009
- Signalfilterung überarbeitet
- OCTO2 implementiert
 
0.72m: H.Buss 13.02.2009
- Code Cleanup
 
0.72o: H.Buss 24.02.2009
- Abtastrate auf 2kHz
- HW-Version an Navi
- neuer Datensatz 'c' -> Lagedaten für 3D-Grafik
- Auswerteroutine für Spectrum-Satteliten implementiert
- Kanalsettings werden beim Parameterreset nicht mehr gelöscht
- die Driftkompensation wird jetzt feiner aufgelöst --> EE_Parameter.Driftkomp muss mal 8 genommen werden
- die Integrale und ACC-Werte werden jetzt im Scope in ca. 0,1° angezeigt (wie beim NaviBrd)
 
0.72p: H.Buss 01.03.2009
- Octo3 erstellt
- Analogwerte umbenannt
0.73a: H.Buss 12.03.2009
- MixerTabelle implementiert
 
0.73b: H.Buss 14.03.2009
- Es wird geprüft, ob alle notwendigen BL-Regler angeschlossen sind
 
0.73a-d: H.Buss 05.04.2009
- MixerTabelle implementiert
- I2C-Bus auf bis zu 12 Motoren erweitert
- die Busfehler der BL-Regler werden im Menü angezeigt
- Revision der MixerTabelle eingeführt
- MixerTabelle wird bei Parameterreset neu initialisiert
- Motortest auf [12] erweitert
- Motorschalter nicht mehr 3-Stufig
 
0.74a
- Datenfusion im Flug auch, wenn ACC-Z < 512
- Wert für die Messbereichserweiterung abgefangen
 
0.74d
- Die Driftkompensation ist jetzt dreistufig -> 0,5% pro sekunde zusätzlich eingeführt
 
 
Anpassungen bzgl. V0.74d
G.Stobrawa 28.04.2009:
 
- Code stärker modularisiert und restrukturiert
- viele Kommentare zur Erklärug eingefügt
- konsequent englische Variablennamen
- PPM24 Support für bis zu 12 RC-Kanäle.
- Suport for DSL Receiver at 2nd UART
- Makefile: EXT=NAVICTRL Unterstützung der SPI Communikation zum Naviboard
 
- Makefile: EXT=MK3MAG Unerstützung des MK3MAG/CMPS03 direkt an FC und Conrad UBLOX Modul
 
- Makefile: EXT=KILLAGREG Unterstützung vom KillagregBoard mit MM3 und Conrad UBLOX Modul
 
- Ausertung des UBX-Protocols an 1. oder 2. Uart
- GPS-Hold-Funktion hinzugefügt
- GPS-Home-Funktion hinzugefügt (wird beim Motorstart gelernt, und bei Motorenstop wieder gelöscht)
- Zusätzliche Punkte im Menü des KopterTool zur Anzeige des GPS-Status und der MM3-Kalibierparameter
 
Weiter Detail siehe Readme.txt im Verzeichnis Hex-Files.