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