/branches/dongfang_FC_rewrite/backup/ADXRS610_FC2.0.c |
---|
0,0 → 1,18 |
#include "ADXRS610_FC2.0.h" |
#include "configuration.h" |
const uint8_t GYRO_REVERSED[3] = {0,0,0}; |
const uint8_t ACC_REVERSED[3] = {0,1,0}; |
void gyro_calibrate(void) {} |
void gyro_setDefaults(void) { |
staticParams.GyroD = 5; |
staticParams.DriftComp = 10; |
staticParams.GyroAccFactor = 1; |
staticParams.GyroAccTrim = 5; |
// Not used. |
staticParams.AngleTurnOverPitch = 78; |
staticParams.AngleTurnOverRoll = 78; |
} |
/branches/dongfang_FC_rewrite/backup/ADXRS610_FC2.0.h |
---|
0,0 → 1,28 |
/* |
#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 |
#endif |
*/ |
/branches/dongfang_FC_rewrite/backup/ENC-03_FC1.3.c |
---|
0,0 → 1,69 |
//#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 |
const uint8_t GYRO_REVERSED[3] = {0,0,1}; |
const uint8_t ACC_REVERSED[3] = {0,1,0}; |
// void gyro_init(void) {} |
void gyro_calibrate(void) { |
uint8_t i, axis, factor, 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; |
for (axis=PITCH; axis<=YAW; axis++) { |
if (axis==YAW) factor = GYRO_SUMMATION_FACTOR_YAW; |
else factor = GYRO_SUMMATION_FACTOR_PITCHROLL; |
if(rawGyroSum[axis] < 510 * factor) DACValues[axis]--; |
else if(rawGyroSum[axis] > 515 * factor) DACValues[axis]++; |
else numberOfAxesInRange++; |
/* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). */ |
if(DACValues[axis] < 10) { |
DACValues[axis] = 10; |
} else if(DACValues[axis] > 245) { |
DACValues[axis] = 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 Error! 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 = 100; |
staticParams.GyroAccFactor = 1; |
staticParams.GyroAccTrim = 5; |
// Not used. |
staticParams.AngleTurnOverPitch = 85; |
staticParams.AngleTurnOverRoll = 85; |
} |
/branches/dongfang_FC_rewrite/backup/ENC-03_FC1.3.h |
---|
0,0 → 1,26 |
/* |
#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/backup/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/backup/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/backup/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 / CONTROL_SCALING); |
MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / CONTROL_SCALING); |
MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / CONTROL_SCALING) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / CONTROL_SCALING)); |
messwertNick er nu P-part.... |
/branches/dongfang_FC_rewrite/backup/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/backup/TODO-list.txt |
---|
0,0 → 1,20 |
- Check that acc. mode looping is still complete and "safe" to use. |
- Find that pitch and roll rate limiter missing at least in HH mode. DONE |
Reng. i badevaer. DONE |
Reng. i sovevaer. DONE |
Rydde op i vasketoej DONE |
Mail til Phat DONE |
SMS til Jonggil DONE |
Skift servo paa fly DONE |
Velo flicken DONE |
Rep. CKF kopter DONE aber wieder |
Montere LED lys paa kopter |
Skrive GPS software |
Jakke til rens |
GPS hack? |
Rydde op paa bord... |
LED Flasher Problem beh vers. |
Krankenkasse Anfrage |
Still More Streamlined Steam and Early Diesels |
/branches/dongfang_FC_rewrite/backup/analog.c |
---|
0,0 → 1,558 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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" |
/* |
* For each A/D conversion cycle, each analog channel 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 rawGyroSum[3]; |
volatile int16_t acc[3]; |
volatile int16_t filteredAcc[2]={0,0}; |
/* |
* These 4 exported variables are zero-offset. The "PID" ones are used |
* in the attitude control as rotation rates. The "ATT" ones are for |
* integration to angles. |
*/ |
volatile int16_t gyro_PID[2]; |
volatile int16_t gyro_ATT[2]; |
volatile int16_t gyroD[2]; |
volatile int16_t yawGyro; |
/* |
* 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 centered on zero. |
*/ |
volatile int16_t gyroOffset[3] = { |
512 * GYRO_SUMMATION_FACTOR_PITCHROLL, |
512 * GYRO_SUMMATION_FACTOR_PITCHROLL, |
512 * GYRO_SUMMATION_FACTOR_YAW |
}; |
volatile int16_t accOffset[3] = { |
512 * ACC_SUMMATION_FACTOR_PITCHROLL, |
512 * ACC_SUMMATION_FACTOR_PITCHROLL, |
512 * ACC_SUMMATION_FACTOR_Z |
}; |
/* |
* This allows some experimentation with the gyro filters. |
* Should be replaced by #define's later on... |
*/ |
volatile uint8_t GYROS_PID_FILTER; |
volatile uint8_t GYROS_ATT_FILTER; |
volatile uint8_t GYROS_D_FILTER; |
volatile uint8_t ACC_FILTER; |
/* |
* Air pressure |
*/ |
volatile uint8_t rangewidth = 106; |
// Direct from sensor, irrespective of range. |
// volatile uint16_t rawAirPressure; |
// Value of 2 samples, with range. |
volatile uint16_t simpleAirPressure; |
// Value of AIRPRESSURE_SUMMATION_FACTOR samples, with range, filtered. |
volatile int32_t filteredAirPressure; |
// Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples. |
volatile int32_t airPressureSum; |
// The number of samples summed into airPressureSum so far. |
volatile uint8_t pressureMeasurementCount; |
/* |
* 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 gyroNoisePeak[2]; |
volatile uint16_t accNoisePeak[2]; |
// 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_PITCH, |
AD_ACC_ROLL, |
AD_AIRPRESSURE, |
AD_GYRO_PITCH, |
AD_GYRO_ROLL, |
AD_ACC_Z, // at 8, measure Z acc. |
AD_GYRO_PITCH, |
AD_GYRO_ROLL, |
AD_GYRO_YAW, // at 11, finish yaw gyro |
AD_ACC_PITCH, // at 12, finish pitch axis acc. |
AD_ACC_ROLL, // at 13, finish roll axis acc. |
AD_AIRPRESSURE, // at 14, finish air pressure. |
AD_GYRO_PITCH, // at 15, finish pitch gyro |
AD_GYRO_ROLL, // at 16, finish roll gyro |
AD_UBAT // at 17, 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; |
} |
} |
uint16_t getSimplePressure(int advalue) { |
return (uint16_t)OCR0A * (uint16_t)rangewidth + advalue; |
} |
/***************************************************** |
* Interrupt Service Routine for ADC |
* Runs at 312.5 kHz or 3.2 µs. When all states are |
* processed the interrupt is disabled and further |
* AD conversions are 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}; |
static uint16_t pressureAutorangingWait = 25; |
uint16_t rawAirPressure; |
uint8_t i, axis; |
int16_t newrange; |
// for various filters... |
int16_t tempOffsetGyro, tempGyro; |
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 8: // Z acc |
if (ACC_REVERSED[Z]) |
acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z]; |
else |
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z]; |
break; |
case 11: // yaw gyro |
rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW]; |
if (GYRO_REVERSED[YAW]) |
yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW]; |
else |
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW]; |
break; |
case 12: // pitch axis acc. |
if (ACC_REVERSED[PITCH]) |
acc[PITCH] = accOffset[PITCH] - sensorInputs[AD_ACC_PITCH]; |
else |
acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH]; |
filteredAcc[PITCH] = (filteredAcc[PITCH] * (ACC_FILTER-1) + acc[PITCH]) / ACC_FILTER; |
measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1); |
break; |
case 13: // roll axis acc. |
if (ACC_REVERSED[ROLL]) |
acc[ROLL] = accOffset[ROLL] - sensorInputs[AD_ACC_ROLL]; |
else |
acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL]; |
filteredAcc[ROLL] = (filteredAcc[ROLL] * (ACC_FILTER-1) + acc[ROLL]) / ACC_FILTER; |
measureNoise(acc[ROLL], &accNoisePeak[ROLL], 1); |
break; |
case 14: // air pressure |
if (pressureAutorangingWait) { |
//A range switch was done recently. Wait for steadying. |
pressureAutorangingWait--; |
break; |
} |
rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
if (rawAirPressure < MIN_RAWPRESSURE) { |
// value is too low, so decrease voltage on the op amp minus input, making the value higher. |
newrange = OCR0A - (MAX_RAWPRESSURE - rawAirPressure) / rangewidth - 1; |
if (newrange > MIN_RANGES_EXTRAPOLATION) { |
pressureAutorangingWait = (OCR0A - newrange) * AUTORANGE_WAIT_FACTOR; |
OCR0A = newrange; |
} else { |
if (OCR0A) { |
OCR0A--; |
pressureAutorangingWait = AUTORANGE_WAIT_FACTOR; |
} |
} |
} else if (rawAirPressure > MAX_RAWPRESSURE) { |
// value is too high, so increase voltage on the op amp minus input, making the value lower. |
// If near the end, make a limited increase |
newrange = OCR0A + (rawAirPressure - MIN_RAWPRESSURE) / rangewidth - 1; |
if (newrange < MAX_RANGES_EXTRAPOLATION) { |
pressureAutorangingWait = (newrange - OCR0A) * AUTORANGE_WAIT_FACTOR; |
OCR0A = newrange; |
} else { |
if (OCR0A<254) { |
OCR0A++; |
pressureAutorangingWait = AUTORANGE_WAIT_FACTOR; |
} |
} |
} |
// Even if the sample is off-range, use it. |
simpleAirPressure = getSimplePressure(rawAirPressure); |
if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) { |
// Danger: pressure near lower end of range. If the measurement saturates, the |
// copter may climb uncontrolled... Simulate a drastic reduction in pressure. |
airPressureSum += (int16_t)MIN_RANGES_EXTRAPOLATION * rangewidth + (simpleAirPressure - (int32_t)MIN_RANGES_EXTRAPOLATION * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
} else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) { |
// Danger: pressure near upper end of range. If the measurement saturates, the |
// copter may fall uncontrolled... Simulate a drastic increase in pressure. |
airPressureSum += (int16_t)MAX_RANGES_EXTRAPOLATION * rangewidth + (simpleAirPressure - (int32_t)MAX_RANGES_EXTRAPOLATION * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
} else { |
// normal case. |
airPressureSum += simpleAirPressure; |
} |
// 2 samples were added. |
pressureMeasurementCount += 2; |
if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR) { |
filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER-1) + airPressureSum + AIRPRESSURE_FILTER/2) / AIRPRESSURE_FILTER; |
pressureMeasurementCount = airPressureSum = 0; |
} |
// DebugOut.Analog[14] = OCR0A; |
// DebugOut.Analog[15] = simpleAirPressure; |
DebugOut.Analog[11] = UBat; |
break; |
case 15: |
case 16: // pitch or roll gyro. |
axis = state - 16; |
tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH - axis]; |
// DebugOut.Analog[6 + 3 * axis ] = tempGyro; |
/* |
* Process the gyro data for the PID controller. |
*/ |
// 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
// gyro with a wider range, and helps counter saturation at full control. |
if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) { |
if (tempGyro < SENSOR_MIN_PITCHROLL) { |
tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
} |
else if (tempGyro > SENSOR_MAX_PITCHROLL) { |
tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL; |
} |
} |
// 2) Apply sign and offset, scale before filtering. |
if (GYRO_REVERSED[axis]) { |
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
} else { |
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL; |
} |
// 3) Scale and filter. |
tempOffsetGyro = (gyro_PID[axis] * (GYROS_PID_FILTER-1) + tempOffsetGyro) / GYROS_PID_FILTER; |
// 4) Measure noise. |
measureNoise(tempOffsetGyro, &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
// 5) Differential measurement. |
gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER-1) + (tempOffsetGyro - gyro_PID[axis])) / GYROS_D_FILTER; |
// 6) Done. |
gyro_PID[axis] = tempOffsetGyro; |
/* |
* Now process the data for attitude angles. |
*/ |
tempGyro = rawGyroSum[axis]; |
// 1) Apply sign and offset, scale before filtering. |
if (GYRO_REVERSED[axis]) { |
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
} else { |
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL; |
} |
// 2) Filter. |
gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_ATT_FILTER-1) + tempOffsetGyro) / GYROS_ATT_FILTER; |
break; |
case 17: |
// Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v). |
// This is divided by 3 --> 10.34 counts per volt. |
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 10 |
uint8_t i, axis; |
int32_t deltaOffsets[3] = {0,0,0}; |
// Set the filters... to be removed again, once some good settings are found. |
GYROS_PID_FILTER = (dynamicParams.UserParams[4] & 0b00000011) + 1; |
GYROS_ATT_FILTER = ((dynamicParams.UserParams[4] & 0b00001100) >> 2) + 1; |
GYROS_D_FILTER = ((dynamicParams.UserParams[4] & 0b00110000) >> 4) + 1; |
ACC_FILTER = ((dynamicParams.UserParams[4] & 0b11000000) >> 6) + 1; |
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(20); |
for (axis=PITCH; axis<=YAW; axis++) { |
deltaOffsets[axis] += rawGyroSum[axis]; |
} |
} |
for (axis=PITCH; axis<=YAW; axis++) { |
gyroOffset[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES/2) / GYRO_OFFSET_CYCLES; |
DebugOut.Analog[20+axis] = gyroOffset[axis]; |
} |
// Noise is relative to offset. So, reset noise measurements when changing offsets. |
gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
accOffset[PITCH] = GetParamWord(PID_ACC_PITCH); |
accOffset[ROLL] = GetParamWord(PID_ACC_ROLL); |
accOffset[Z] = GetParamWord(PID_ACC_Z); |
// Rough estimate. Hmm no nothing happens at calibration anyway. |
// airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
// pressureMeasurementCount = 0; |
// Experiment! |
// filteredAirPressureOffset = filteredAirPressure - 1000L; |
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, axis; |
int32_t deltaOffset[3] = {0,0,0}; |
int16_t filteredDelta; |
// int16_t pressureDiff, savedRawAirPressure; |
for(i=0; i < ACC_OFFSET_CYCLES; i++) { |
Delay_ms_Mess(10); |
for (axis=PITCH; axis<=YAW; axis++) { |
deltaOffset[axis] += acc[axis]; |
} |
} |
for (axis=PITCH; axis<=YAW; axis++) { |
filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
accOffset[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta; |
} |
// Save ACC neutral settings to eeprom |
SetParamWord(PID_ACC_PITCH, accOffset[PITCH]); |
SetParamWord(PID_ACC_ROLL, accOffset[ROLL]); |
SetParamWord(PID_ACC_Z, accOffset[Z]); |
// Noise is relative to offset. So, reset noise measurements when |
// changing offsets. |
accNoisePeak[PITCH] = accNoisePeak[ROLL] = 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); |
// Set the feedback so that air pressure ends up in the middle of the range. |
// (raw pressure high --> OCR0A also high...) |
// OCR0A += (rawAirPressure - 512) / rangewidth; |
// Delay_ms_Mess(500); |
/* |
pressureDiff = 0; |
// DebugOut.Analog[16] = rawAirPressure; |
#define PRESSURE_CAL_CYCLE_COUNT 2 |
for (i=0; i<PRESSURE_CAL_CYCLE_COUNT; i++) { |
savedRawAirPressure = rawAirPressure; |
OCR0A++; |
Delay_ms_Mess(200); |
// raw pressure will decrease. |
pressureDiff += (savedRawAirPressure - rawAirPressure); |
savedRawAirPressure = rawAirPressure; |
OCR0A--; |
Delay_ms_Mess(200); |
// raw pressure will increase. |
pressureDiff += (rawAirPressure - savedRawAirPressure); |
} |
// DebugOut.Analog[16] = |
rangewidth = (pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2); |
*/ |
} |
/branches/dongfang_FC_rewrite/backup/analog.h |
---|
0,0 → 1,260 |
#ifndef _ANALOG_H |
#define _ANALOG_H |
#include <inttypes.h> |
//#include "invenSense.h" |
//#include "ENC-03_FC1.3.h" |
//#include "ADXRS610_FC2.0.h" |
/* |
* How much low pass filtering to apply for gyro_PID. |
* 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_PID_FILTER 1 |
/* |
* How much low pass filtering to apply for gyro_ATT. |
* 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_ATT_FILTER 1 |
// Temporarily replaced by userparam-configurable variable. |
// #define ACC_FILTER 4 |
/* |
About setting constants 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 outputs a voltage > the no-rotation voltage. |
A gyro is considered, in this code, to be "forward" if its positive |
direction is: |
- Nose down for pitch |
- Left hand side down for roll |
- Clockwise seen from above for yaw. |
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: |
gyro = 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: |
gyro = 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: gyro = I * H * rotation rate [units / (deg/s)]. |
*/ |
/* |
* A factor that the raw gyro values are multiplied by, |
* before being 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. |
* Experiment: Set it to 1. |
*/ |
#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 |
#define ACC_SUMMATION_FACTOR_PITCHROLL 2 |
#define ACC_SUMMATION_FACTOR_Z 1 |
/* |
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 |
gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL]. |
Assuming a constant rotation rate v and a zero initial gyroIntegral |
(for this explanation), we get: |
gyroIntegral = |
N * gyro / 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 gyro[PITCH/ROLL] 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) |
/* |
* Gyro saturation prevention. |
*/ |
// How far from the end of its range a gyro is considered near-saturated. |
#define SENSOR_MIN_PITCHROLL 32 |
// Other end of the range (calculated) |
#define SENSOR_MAX_PITCHROLL (GYRO_SUMMATION_FACTOR_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL) |
// Max. boost to add "virtually" to gyro signal at total saturation. |
#define EXTRAPOLATION_LIMIT 2500 |
// Slope of the boost (calculated) |
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL) |
/* |
* This value is subtracted from the gyro noise measurement in each iteration, |
* making it return towards zero. |
*/ |
#define GYRO_NOISE_MEASUREMENT_DAMPING 5 |
#define PITCH 0 |
#define ROLL 1 |
#define YAW 2 |
#define Z 2 |
/* |
* The values that this module outputs |
* These first 2 exported arrays are zero-offset. The "PID" ones are used |
* in the attitude control as rotation rates. The "ATT" ones are for |
* integration to angles. For the same axis, the PID and ATT variables |
* generally have about the same values. There are just some differences |
* in filtering, and when a gyro becomes near saturated. |
* Maybe this distinction is not really necessary. |
*/ |
extern volatile int16_t gyro_PID[2]; |
extern volatile int16_t gyro_ATT[2]; |
extern volatile int16_t gyroD[2]; |
extern volatile int16_t yawGyro; |
extern volatile uint16_t ADCycleCount; |
extern volatile int16_t UBat; |
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3. |
#define UBAT_AT_5V (int16_t)((5.0 / 3.0) * (1.0/11.0) / (3.0 * 1024)) |
/* |
* This is not really for external use - but the ENC-03 gyro modules needs it. |
*/ |
extern volatile int16_t rawGyroSum[3]; |
/* |
* The acceleration values that this module outputs. They are zero based. |
*/ |
extern volatile int16_t acc[3]; |
extern volatile int16_t filteredAcc[2]; |
/* |
* 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 gyroNoisePeak[2]; |
extern volatile uint16_t accNoisePeak[2]; |
/* |
* Air pressure. |
* The sensor has a sensitivity of 46 mV/kPa. |
* An approximate p(h) formula is = p(h[m])[Pa] = p_0 - 1195 * 10^-6 * h |
* |
*/ |
#define AIRPRESSURE_SUMMATION_FACTOR 14 |
#define AIRPRESSURE_FILTER 8 |
// Minimum A/D value before a range change is performed. |
#define MIN_RAWPRESSURE (200 * 2) |
// Maximum A/D value before a range change is performed. |
#define MAX_RAWPRESSURE (1023 * 2 - MIN_RAWPRESSURE) |
#define MIN_RANGES_EXTRAPOLATION 10 |
#define MAX_RANGES_EXTRAPOLATION 250 |
#define PRESSURE_EXTRAPOLATION_COEFF 25L |
#define AUTORANGE_WAIT_FACTOR 1 |
extern volatile uint16_t simpleAirPressure; |
extern volatile int32_t filteredAirPressure; |
/* |
* At saturation, the filteredAirPressure may actually be (simulated) negative. |
*/ |
extern volatile int32_t filteredAirPressure; |
/* |
* Flag: Interrupt handler has done all A/D conversion and processing. |
*/ |
extern volatile uint8_t analogDataReady; |
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/backup/attitude.c |
---|
0,0 → 1,483 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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" |
// For scope debugging only! |
#include "rc.h" |
// where our main data flow comes from. |
#include "analog.h" |
#include "configuration.h" |
#include "output.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 rate_ATT[2], yawRate; |
// With different (less) filtering |
int16_t rate_PID[2]; |
int16_t differential[2]; |
/* |
* 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 ACRate[2], ACYawRate; |
/* |
* Gyro integrals. These are the rotation angles of the airframe compared to the |
* horizontal plane, yaw relative to yaw at start. |
*/ |
int32_t angle[2], yawAngleDiff; |
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; |
uint16_t badCompassHeading = 500; |
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
int16_t yawGyroDrift; |
#define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L) |
#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L) |
#define YAWOVER360 (GYRO_DEG_FACTOR_YAW * 360L) |
int16_t correctionSum[2] = {0,0}; |
// For NaviCTRL use. |
int16_t averageAcc[2] = {0,0}, averageAccCount = 0; |
/* |
* Experiment: Compensating for dynamic-induced gyro biasing. |
*/ |
int16_t driftComp[2] = {0,0}, driftCompYaw = 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 since: 1) the angles are rather small 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 getAngleEstimateFromAcc(uint8_t axis) { |
return GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis]; |
} |
void setStaticAttitudeAngles(void) { |
#ifdef ATTITUDE_USE_ACC_SENSORS |
angle[PITCH] = getAngleEstimateFromAcc(PITCH); |
angle[ROLL] = getAngleEstimateFromAcc(ROLL); |
#else |
angle[PITCH] = angle[ROLL] = 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; |
driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
correctionSum[PITCH] = correctionSum[ROLL] = 0; |
// Calibrate hardware. |
analog_calibrate(); |
// reset gyro readings |
// rate_ATT[PITCH] = rate_ATT[ROLL] = yawRate = 0; |
// reset gyro integrals to acc guessing |
setStaticAttitudeAngles(); |
yawAngleDiff = 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). |
* The rate variable end up in a range of about [-1024, 1023]. |
*************************************************************************/ |
void getAnalogData(void) { |
uint8_t axis; |
for (axis=PITCH; axis <=ROLL; axis++) { |
rate_PID[axis] = (gyro_PID[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
rate_ATT[axis] = (gyro_ATT[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
differential[axis] = gyroD[axis]; |
averageAcc[axis] += acc[axis]; |
} |
averageAccCount++; |
yawRate = yawGyro + driftCompYaw; |
// We are done reading variables from the analog module. |
// Interrupt-driven sensor reading may restart. |
analogDataReady = 0; |
analog_start(); |
} |
/* |
* 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(angle[PITCH]); |
int16_t cosroll = int_cos(angle[ROLL]); |
int16_t sinroll = int_sin(angle[ROLL]); |
int16_t tanpitch = int_tan(angle[PITCH]); |
#define ANTIOVF 512 |
ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t)rate_ATT[PITCH] * sinroll / ANTIOVF * tanpitch + (int32_t)yawRate * int_cos(angle[ROLL]) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
} |
// 480 usec with axis coupling - almost no time without. |
void integrate(void) { |
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
uint8_t axis; |
if(!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
// The rotary rate limiter bit is abused for selecting axis coupling algorithm instead. |
trigAxisCoupling(); |
} else { |
ACRate[PITCH] = rate_ATT[PITCH]; |
ACRate[ROLL] = rate_ATT[ROLL]; |
ACYawRate = yawRate; |
} |
/* |
* Yaw |
* Calculate yaw gyro integral (~ to rotation angle) |
* Limit yawGyroHeading proportional to 0 deg to 360 deg |
*/ |
yawGyroHeading += ACYawRate; |
yawAngleDiff += yawRate; |
if(yawGyroHeading >= YAWOVER360) { |
yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
} else if(yawGyroHeading < 0) { |
yawGyroHeading += YAWOVER360; |
} |
/* |
* Pitch axis integration and range boundary wrap. |
*/ |
for (axis=PITCH; axis<=ROLL; axis++) { |
angle[axis] += ACRate[axis]; |
if(angle[axis] > PITCHROLLOVER180) { |
angle[axis] -= PITCHROLLOVER360; |
} else if (angle[axis] <= -PITCHROLLOVER180) { |
angle[axis] += PITCHROLLOVER360; |
} |
} |
} |
/************************************************************************ |
* 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 a drift compensation |
* - 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. |
uint8_t axis; |
int32_t correction; |
if(!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] <= dynamicParams.UserParams[7]) { |
DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
uint8_t debugFullWeight = 1; |
int32_t accDerived; |
if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 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. |
*/ |
for (axis=PITCH; axis<=ROLL; axis++) { |
accDerived = getAngleEstimateFromAcc(axis); |
DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
DebugOut.Analog[18 + axis] = getAngleEstimateFromAcc(axis); |
// 1000 * the correction amount that will be added to the gyro angle in next line. |
correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
angle[axis] = ((int32_t)(1000L - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived) / 1000L; |
correctionSum[axis] += angle[axis] - correction; |
DebugOut.Analog[14+axis] = permilleAcc; |
DebugOut.Analog[16+axis] = angle[axis] - correction; |
} |
// DebugOut.Digital[1] |= DEBUG_ACC0THORDER; |
} else { |
DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
} |
} |
/************************************************************************ |
* 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 |
* DRIFTCORRECTION_TIME cycles are summed up. This number is |
* then divided by DRIFTCORRECTION_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. = 488/2 |
#define DRIFTCORRECTION_TIME 256L |
void driftCorrection(void) { |
static int16_t timer = DRIFTCORRECTION_TIME; |
int16_t deltaCorrection; |
uint8_t axis; |
if (! --timer) { |
timer = DRIFTCORRECTION_TIME; |
for (axis=PITCH; axis<=ROLL; axis++) { |
// Take the sum of corrections applied, add it to delta |
deltaCorrection = ((correctionSum[axis] + DRIFTCORRECTION_TIME / 2) * HIRES_GYRO_INTEGRATION_FACTOR) / DRIFTCORRECTION_TIME; |
// Add the delta to the compensation. So positive delta means, gyro should have higher value. |
driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
// DebugOut.Analog[11 + axis] = correctionSum[axis]; |
DebugOut.Analog[28 + axis] = driftComp[axis]; |
correctionSum[axis] = 0; |
} |
} |
} |
/************************************************************************ |
* Main procedure. |
************************************************************************/ |
void calculateFlightAttitude(void) { |
// part1: 550 usec. |
// part1a: 550 usec. |
// part1b: 60 usec. |
getAnalogData(); |
// end part1b |
integrate(); |
// end part1a |
DebugOut.Analog[6] = ACRate[PITCH]; |
DebugOut.Analog[7] = ACRate[ROLL]; |
DebugOut.Analog[8] = ACYawRate; |
DebugOut.Analog[3] = rate_PID[PITCH]; |
DebugOut.Analog[4] = rate_PID[ROLL]; |
DebugOut.Analog[5] = yawRate; |
#ifdef ATTITUDE_USE_ACC_SENSORS |
correctIntegralsByAcc0thOrder(); |
driftCorrection(); |
#endif |
// end part1 |
} |
void updateCompass(void) { |
int16_t w, v, r,correction, error; |
if(compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) { |
if (controlMixer_testCompassCalState()) { |
compassCalState++; |
if(compassCalState < 5) beepNumber(compassCalState); |
else beep(1000); |
} |
} else { |
// get maximum attitude angle |
w = abs(angle[PITCH] / 512); |
v = abs(angle[ROLL] / 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) { |
yawGyroDrift += error; |
if(updateCompassCourse) { |
beep(200); |
yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW; |
compassCourse = compassHeading; //(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 + (maxControl[PITCH] + maxControl[ROLL]) / 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; |
yawAngleDiff += 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 |
driftCompPitch = dynamicParams.UserParam7 - 128; |
driftCompRoll = dynamicParams.UserParam8 - 128; |
} else { |
// use the sampled value (does not seem to work so well....) |
driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
driftCompYaw = -dynamicCalYaw / dynamicCalCount; |
} |
// keep resetting these meanwhile, to avoid accumulating errors. |
setStaticAttitudeIntegrals(); |
yawAngle = 0; |
} |
*/ |
/branches/dongfang_FC_rewrite/backup/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 8 for ADXRS610. |
* 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 rate_PID[2], rate_ATT[2], yawRate; |
extern int16_t differential[2]; |
/* |
* Attitudes calculated by numerical integration of gyro rates |
*/ |
extern int32_t angle[2], yawAngleDiff; |
// 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; |
/* |
* 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 dynamicOffset[2], dynamicOffsetYaw; |
/* |
* For NaviCtrl use. |
*/ |
extern int16_t averageAcc[2], averageAccCount; |
/* |
* 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); |
int32_t getAngleEstimateFromAcc(uint8_t axis); |
/* |
* Main routine, called from the flight loop. |
*/ |
void calculateFlightAttitude(void); |
#endif //_ATTITUDE_H |
/branches/dongfang_FC_rewrite/backup/attitudeControl.c |
---|
0,0 → 1,46 |
#include <inttypes.h> |
#include "attitude.h" |
#include "uart0.h" |
#include "configuration.h" |
#include "dongfangMath.h" |
// For scope debugging only! |
#include "rc.h" |
// = cos^2(45 degs). |
const int32_t MINPROJECTION = (int32_t)MATH_UNIT_FACTOR * MATH_UNIT_FACTOR / 2; |
// Takes 380 - 400 usec. Way too slow. |
// With static MINPROJECTION: 220 usec. |
uint16_t AC_getThrottle(uint16_t throttle) { |
int32_t projection; |
// part1 start: 150 usec |
// It's factor (int32_t)MATH_UNIT_FACTOR^2 too high. |
projection = (int32_t)int_cos(angle[PITCH]) * (int32_t)int_cos(angle[ROLL]); |
// part1 end. |
uint8_t effect = dynamicParams.UserParams[2]; |
int16_t deltaThrottle; |
if (projection < MINPROJECTION && projection >= 0) { |
// projection = MINPROJECTION; |
deltaThrottle = 0; |
} else if (projection >- MINPROJECTION && projection<0) { |
// projection = -MINPROJECITON; |
deltaThrottle = 0; |
} else |
/* |
* We need delta throttle = constant/projection1 |
* (constant * MATH_UNIT_FACTOR^2) / projection. |
*/ |
deltaThrottle = ((int32_t)effect * (int32_t)MATH_UNIT_FACTOR * (int32_t)MATH_UNIT_FACTOR) / (projection / 10) - effect * 10; |
// DebugOut.Analog[13] = deltaThrottle; |
return throttle; |
} |
/* |
har: R = e * k/p |
vil R = e * ( 1 - k/p ) |
= e - ek/p |
*/ |
/branches/dongfang_FC_rewrite/backup/attitudeControl.h |
---|
0,0 → 1,5 |
#ifndef _ATTITUDECONTROL_H |
#define _ATTITUDECONTROL_H |
#include <inttypes.h> |
uint16_t AC_getThrottle(uint16_t throttle); |
#endif |
/branches/dongfang_FC_rewrite/backup/bugs.txt |
---|
0,0 → 1,9 |
tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
Wer sieht hier Fehler? |
1) Nick- und Rollwinkeln werden mit Gierkonstant berechnet (1160 od. 1291) |
2) Pythagoras wird auf Winklen (statt Seitenlängen) verwendet. |
/branches/dongfang_FC_rewrite/backup/commands.c |
---|
0,0 → 1,125 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "commands.h" |
#include "controlMixer.h" |
#include "flight.h" |
#include "eeprom.h" |
#include "attitude.h" |
void commands_handleCommands(void) { |
/* |
* 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(); |
if(!(MKFlags & MKFLAG_MOTOR_RUN)) { |
if (command == COMMAND_GYROCAL && !repeated) { |
// 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(); |
controlMixer_setNeutral(); |
beepNumber(getActiveParamSet()); |
} else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && argument == 7) { |
// If right stick is centered and down |
// TODO: Out of here! State machine instead. |
compassCalState = 1; |
beep(1000); |
} |
} |
// save the ACC neutral setting to eeprom |
else { |
if(command == COMMAND_ACCCAL && !repeated) { |
// Run gyro and acc. meter calibration but do not repeat it. |
GRN_OFF; |
analog_calibrateAcc(); |
attitude_setNeutral(); |
flight_setNeutral(); |
controlMixer_setNeutral(); |
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); |
} |
} |
/branches/dongfang_FC_rewrite/backup/commands.h |
---|
0,0 → 1,17 |
#ifndef _COMMANDS_H |
#define _COMMANDS_H |
#include <inttypes.h> |
void commands_handleCommands(void); |
/* |
* 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 |
#endif |
/branches/dongfang_FC_rewrite/backup/commands.txt |
---|
0,0 → 1,8 |
These are the control commands: |
- startMotors |
- stopMotors |
- calibrateGyros |
- calibrateAcc (+height?) |
- calibrateCompass |
- captureHeightSetpoint |
/branches/dongfang_FC_rewrite/backup/configuration.c |
---|
0,0 → 1,292 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 <stddef.h> |
#include "configuration.h" |
#include "eeprom.h" |
#include "timer0.h" |
int16_t variables[8] = {0,0,0,0,0,0,0,0}; |
dynamicParam_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 |
* Replacing this code by the code below saved almost 1 kbyte. |
************************************************************************/ |
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); |
} |
const XLATION XLATIONS[] = { |
{offsetof(paramset_t, MaxHeight), offsetof(dynamicParam_t, MaxHeight)}, |
{offsetof(paramset_t, Height_ACC_Effect), offsetof(dynamicParam_t, Height_ACC_Effect)}, |
{offsetof(paramset_t, CompassYawEffect), offsetof(dynamicParam_t, CompassYawEffect)}, |
{offsetof(paramset_t, GyroI), offsetof(dynamicParam_t, GyroI)}, |
{offsetof(paramset_t, GyroD), offsetof(dynamicParam_t, GyroD)}, |
{offsetof(paramset_t, IFactor), offsetof(dynamicParam_t, IFactor)}, |
{offsetof(paramset_t, ServoPitchControl), offsetof(dynamicParam_t, ServoPitchControl)}, |
{offsetof(paramset_t, LoopGasLimit), offsetof(dynamicParam_t, LoopGasLimit)}, |
{offsetof(paramset_t, AxisCoupling1), offsetof(dynamicParam_t, AxisCoupling1)}, |
{offsetof(paramset_t, AxisCoupling2), offsetof(dynamicParam_t, AxisCoupling2)}, |
{offsetof(paramset_t, AxisCouplingYawCorrection), offsetof(dynamicParam_t, AxisCouplingYawCorrection)}, |
{offsetof(paramset_t, DynamicStability), offsetof(dynamicParam_t, DynamicStability)}, |
{offsetof(paramset_t, NaviGpsModeControl), |
offsetof(dynamicParam_t, NaviGpsModeControl)}, |
{offsetof(paramset_t, NaviGpsGain), offsetof(dynamicParam_t, NaviGpsGain)}, |
{offsetof(paramset_t, NaviGpsP), offsetof(dynamicParam_t, NaviGpsP)}, |
{offsetof(paramset_t, NaviGpsI), offsetof(dynamicParam_t, NaviGpsI)}, |
{offsetof(paramset_t, NaviGpsD), offsetof(dynamicParam_t, NaviGpsD)}, |
{offsetof(paramset_t, NaviGpsACC), offsetof(dynamicParam_t, NaviGpsACC)}, |
{offsetof(paramset_t, NaviWindCorrection), offsetof(dynamicParam_t, NaviWindCorrection)}, |
{offsetof(paramset_t, NaviSpeedCompensation), offsetof(dynamicParam_t, NaviSpeedCompensation)}, |
{offsetof(paramset_t, ExternalControl), offsetof(dynamicParam_t, ExternalControl)} |
}; |
const MMXLATION MMXLATIONS[] = { |
{offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100}, |
{offsetof(paramset_t, HeightP), offsetof(dynamicParam_t, HeightD),0,150}, |
{offsetof(paramset_t, GyroP), offsetof(dynamicParam_t, GyroP),0,255}, |
{offsetof(paramset_t, J16Timing), offsetof(dynamicParam_t, J16Timing),1,255}, |
{offsetof(paramset_t, J17Timing), offsetof(dynamicParam_t, J17Timing),1,255}, |
{offsetof(paramset_t, NaviOperatingRadius), offsetof(dynamicParam_t, NaviOperatingRadius),10,255} |
}; |
uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) { |
uint8_t result; |
if (src>=251) result = variables[src-251]; |
else result = src; |
if (result < min) result = min; |
else if (result > max) result = max; |
return result; |
} |
void configuration_applyVariablesToParams_dead(void) { |
uint8_t i, src; |
uint8_t* pointerToTgt; |
for(i=0; i<sizeof(XLATIONS)/sizeof(XLATION); i++) { |
src = *((uint8_t*)(&staticParams + XLATIONS[i].sourceIdx)); |
pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx); |
if (src < 255) { |
*pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
} |
} |
for(i=0; i<sizeof(MMXLATIONS)/sizeof(MMXLATION); i++) { |
src = *((uint8_t*)(&staticParams + MMXLATIONS[i].sourceIdx)); |
pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx); |
if (src < 255) { |
*pointerToTgt = configuration_applyVariableToParam(src, MMXLATIONS[i].min, MMXLATIONS[i].max); |
} |
} |
for (i=0; i<sizeof(staticParams.UserParams1); i++) { |
src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams1) + i)); |
pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + i); |
if (src < 255) { |
*pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
} |
} |
for (i=0; i<sizeof(staticParams.UserParams2); i++) { |
src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams2) + i)); |
pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + sizeof(staticParams.UserParams1) + i); |
if (src < 255) { |
*pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
} |
} |
} |
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/backup/configuration.h |
---|
0,0 → 1,205 |
#ifndef _CONFIGURATION_H |
#define _CONFIGURATION_H |
#include <inttypes.h> |
#include <avr/io.h> |
typedef struct { |
/*PMM*/ uint8_t HeightD; |
/* P */ uint8_t MaxHeight; |
/*PMM*/ uint8_t HeightP; |
/* P */ uint8_t Height_ACC_Effect; |
/* P */ uint8_t CompassYawEffect; |
/* P */ uint8_t GyroD; |
/*PMM*/ uint8_t GyroP; |
/* P */ uint8_t GyroI; |
/* Never used */ uint8_t StickYawP; |
/* P */ uint8_t IFactor; |
/* P */ uint8_t UserParams[8]; |
/* P */ uint8_t ServoPitchControl; |
/* P */ uint8_t LoopGasLimit; |
/* P */ uint8_t AxisCoupling1; |
/* P */ uint8_t AxisCoupling2; |
/* P */ uint8_t AxisCouplingYawCorrection; |
/* P */ uint8_t DynamicStability; |
/* P */ uint8_t ExternalControl; |
/*PMM*/ uint8_t J16Timing; |
/*PMM*/ uint8_t J17Timing; |
/* P */ uint8_t NaviGpsModeControl; |
/* P */ uint8_t NaviGpsGain; |
/* P */ uint8_t NaviGpsP; |
/* P */ uint8_t NaviGpsI; |
/* P */ uint8_t NaviGpsD; |
/* P */ uint8_t NaviGpsACC; |
/*PMM*/ uint8_t NaviOperatingRadius; |
/* P */ uint8_t NaviWindCorrection; |
/* P */ uint8_t NaviSpeedCompensation; |
int8_t KalmanK; |
int8_t KalmanMaxDrift; |
int8_t KalmanMaxFusion; |
} dynamicParam_t; |
extern dynamicParam_t dynamicParams; |
typedef struct { |
uint8_t sourceIdx, targetIdx; |
uint8_t min, max; |
} MMXLATION; |
typedef struct { |
uint8_t sourceIdx, targetIdx; |
} XLATION; |
// 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 MinThrottle; // Value : 0-32 |
uint8_t MaxThrottle; // 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 Unused0; // |
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 staticParams.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/backup/control.c |
---|
0,0 → 1,376 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
/* |
OBSOLETED BY controlMixer.c. But this is how it looked - maybe somebody will find it simpler? |
#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 / CONTROL_SCALING) > maxStickPitch) { |
maxStickPitch = abs(stickPitch) / CONTROL_SCALING; |
if(maxStickPitch > 100) maxStickPitch = 100; |
} |
else if (maxStickPitch) maxStickPitch--; |
if(abs(stickRoll / CONTROL_SCALING) > maxStickRoll) { |
maxStickRoll = abs(stickRoll) / CONTROL_SCALING; |
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/backup/control.h |
---|
0,0 → 1,76 |
/*********************************************************************************/ |
/* Stick control interface */ |
/*********************************************************************************/ |
/* |
OBSOLETED BY controlMixer.h. But this is how it looked - maybe somebody will find it simpler? |
#ifndef _CONTROL_H |
#define _CONTROL_H |
#include <inttypes.h> |
#define CONTROL_SCALING 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/backup/controlMixer.c |
---|
0,0 → 1,215 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "heightControl.h" |
#include "attitudeControl.h" |
#include "externalControl.h" |
#include "configuration.h" |
#include "attitude.h" |
#include "commands.h" |
//#include "eeprom.h" |
//#include "flight.h" |
uint16_t maxControl[2] = {0,0}; |
int16_t control[2] = {0,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 lastArgument; |
uint8_t isCommandRepeated = 0; |
// MK flags. TODO: Replace by enum. State machine. |
uint16_t isFlying = 0; |
volatile uint8_t MKFlags = 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 lastArgument; |
} |
/* |
* 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) { |
return lastCommand; |
} |
uint8_t controlMixer_isCommandRepeated(void) { |
return isCommandRepeated; |
} |
void controlMixer_setNeutral() { |
EC_setNeutral(); |
HC_setGround(); |
} |
/* |
* 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. |
* TODO: It assumes R/C as source. Not necessarily true. |
*/ |
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(); |
// 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. |
uint8_t axis; |
// takes almost no time... |
RC_update(); |
// takes almost no time... |
EC_update(); |
// takes about 80 usec. |
HC_update(); |
int16_t* RC_PRTY = RC_getPRTY(); |
int16_t* EC_PRTY = EC_getPRTY(); |
control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]; |
control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]; |
// This can be a CPU time killer if the function implementations are inefficient. |
controlThrottle = HC_getThrottle(AC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE])); |
controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
if (controlMixer_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; |
} |
// part1a end. |
/* This is not really necessary with the pull-towards-zero of all sticks (see rc.c) |
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 |
*/ |
for (axis=PITCH; axis<=ROLL; axis++) { |
if(abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) { |
maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
if(maxControl[axis] > 100) maxControl[axis] = 100; |
} else if (maxControl[axis]) maxControl[axis]--; |
} |
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE; |
uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE; |
if (rcCommand != COMMAND_NONE) { |
isCommandRepeated = (lastCommand == rcCommand); |
lastCommand = rcCommand; |
lastArgument = RC_getArgument(); |
} else if (ecCommand != COMMAND_NONE) { |
isCommandRepeated = (lastCommand == ecCommand); |
lastCommand = ecCommand; |
lastArgument = EC_getArgument(); |
} else { |
// Both sources have no command, or one or both are out. |
// Just set to false. There is no reason to check if the none-command was repeated anyway. |
isCommandRepeated = 0; |
lastCommand = COMMAND_NONE; |
} |
// part1 end. |
} |
// TODO: Integrate into command system. |
uint8_t controlMixer_testCompassCalState(void) { |
return RC_testCompassCalState(); |
} |
/branches/dongfang_FC_rewrite/backup/controlMixer.h |
---|
0,0 → 1,127 |
#include <inttypes.h> |
/* |
* 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 priorities 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 |
/* |
* 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 control[2], controlYaw, controlThrottle; |
extern uint16_t maxControl[2]; |
extern uint8_t looping; |
extern volatile uint8_t MKFlags; |
extern uint16_t isFlying; |
void controlMixer_initVariables(void); |
void controlMixer_updateVariables(void); |
void controlMixer_setNeutral(void); |
/* |
* 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); |
/* |
* The controls operate in a [-150 * CONTROL_SCALING, 150 * CONTROL_SCALING] interval |
* Throttle is [0..300 * CONTROL_SCALING]. |
* (just about. No precision needed). |
*/ |
#define CONTROL_SCALING (1024/256) |
/* |
* 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 |
*/ |
// void controlMixer_handleCommands(void); |
uint8_t controlMixer_getArgument(void); |
uint8_t controlMixer_isCommandRepeated(void); |
// TODO: Abstract away if possible. |
uint8_t controlMixer_testCompassCalState(void); |
// void controlMixer_updateCompass(void); |
/branches/dongfang_FC_rewrite/backup/displays.txt |
---|
0,0 → 1,10 |
Timerstyret eller Anforderung0 : 'H' format. Timerkonstant = interval fra request * 10 |
H format response: H, address, buffer(80) |
h format request: keys(invers!), interval |
keys: key1(2): baglaens, key2(4): forlaens, begge: #0, ingen: Bliv paa samme side. |
andre keys: key4(8): reset(???) flyvetimer. |
Kun h request setter keys. |
DisplayAnfordering1: 'L' format. |
L format response: L, address, menupunkt, maxmenu, buffer(80) |
l format request: l, address, menupunkt |
/branches/dongfang_FC_rewrite/backup/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/backup/dongfangMath.h |
---|
0,0 → 1,21 |
#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 |
// Changed: We want to be able to multiply 2 sines/cosines and still stay comfortably (factor 100) within 31 bits. |
// 4096 = 12 bits, square = 24 bits, 7 bits to spare. |
#define MATH_UNIT_FACTOR 4096 |
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/backup/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/backup/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/backup/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/backup/eeprom.c |
---|
0,0 → 1,394 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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; |
} |
/* |
* While we are still using userparams for flight parameters, do set |
* some safe & meaningful default values. |
*/ |
staticParams.UserParams1[3] = 10; // Throttle stick D=10 |
staticParams.UserParams2[0] = 0b11010101; // All gyro filter constants 2; acc. 4 |
staticParams.UserParams2[1] = 2; // H&I motor smoothing. |
staticParams.UserParams2[2] = 120; // Yaw I factor |
staticParams.UserParams2[3] = 100; // Max Z acceleration for acc. correction of angles. |
} |
void setOtherDefaults(void) { |
/* Channel assignments were changed to the normal: |
* Aileron/roll=1, elevator/pitch=2, throttle=3, yaw/rudder=4 |
*/ |
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_HEADING_HOLD; // 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.MinThrottle = 8; |
staticParams.MaxThrottle = 230; |
staticParams.CompassYawEffect = 128; |
staticParams.GyroP = 80; |
staticParams.GyroI = 100; |
staticParams.LowVoltageWarning = 95; |
staticParams.EmergencyGas = 35; |
staticParams.EmergencyGasDuration = 30; |
staticParams.Unused0 = 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.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 |
setOtherDefaults(); |
gyro_setDefaults(); |
setDefaultUserParams(); |
staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE; |
memcpy(staticParams.Name, "Sport\0",6); |
} |
/***************************************************/ |
/* Default Values for parameter set 2 */ |
/***************************************************/ |
void ParamSet_DefaultSet2(void) { // normal |
setOtherDefaults(); |
gyro_setDefaults(); |
setDefaultUserParams(); |
staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE; |
staticParams.Height_Gain = 3; |
staticParams.J16Timing = 20; |
staticParams.J17Timing = 20; |
memcpy(staticParams.Name, "Normal\0", 7); |
} |
/***************************************************/ |
/* Default Values for parameter set 3 */ |
/***************************************************/ |
void ParamSet_DefaultSet3(void) { // beginner |
setOtherDefaults(); |
gyro_setDefaults(); |
setDefaultUserParams(); |
staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE; |
staticParams.Height_Gain = 3; |
staticParams.EmergencyGasDuration = 20; |
staticParams.AxisCouplingYawCorrection = 70; |
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=1, i, j; |
// 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 |
for (j=0; j<4 && Channel_Backup; j++) { |
if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+0]) >= 12) |
Channel_Backup = 0; |
} |
// 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 |
for (j=0; j<8; j++) { |
staticParams.ChannelAssignment[j] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+j]); |
} |
} |
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/backup/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_PITCH 4 // word |
#define PID_ACC_ROLL 6 // word |
#define PID_ACC_Z 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/backup/encoding-tester.txt |
---|
0,0 → 1,3 |
UTF-8: äöü |
ISO-8859-1: äöü |
/branches/dongfang_FC_rewrite/backup/externalControl.c |
---|
0,0 → 1,67 |
#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; |
} |
uint8_t EC_getArgument(void) { |
return externalControl.config; |
} |
uint8_t EC_getCommand(void) { |
return externalControl.free; |
} |
// 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 (externalControlActive > 80) |
// Configured and heard from recently |
return SIGNAL_GOOD; |
if (externalControlActive) |
// Configured and heard from |
return SIGNAL_OK; |
if (!(externalControl.config & 0x01 && dynamicParams.ExternalControl > 128)) |
// External control is not even configured. |
return NO_SIGNAL; |
// Configured but expired. |
return SIGNAL_LOST; |
} |
/branches/dongfang_FC_rewrite/backup/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 remoteButtons; |
int8_t pitch; |
int8_t roll; |
int8_t yaw; |
uint8_t throttle; |
int8_t height; |
uint8_t free; // Let's use that for commands now. |
uint8_t frame; |
uint8_t config; // Let's use that for arguemnts. |
} __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/backup/fc-0.74_bugs.txt |
---|
0,0 → 1,6 |
NaviAcc. variables - overflow |
A/D values are read after AD cycle restart |
Axis coupling bug (can be demonstrated by flying some fast maneuvers in H H mode with |
axis coupling on, and watching the copter lose control. Once axis coupling is turned |
back off, it is OK again). |
/branches/dongfang_FC_rewrite/backup/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/backup/flight.c |
---|
0,0 → 1,463 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + Nur für den privaten Gebrauch |
// + www.MikroKopter.com |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt und genannt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zulässig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <stdlib.h> |
#include <avr/io.h> |
#include "eeprom.h" |
#include "flight.h" |
// Only for debug. Remove. |
//#include "analog.h" |
//#include "rc.h" |
// Necessary for external control and motor test |
#include "uart0.h" |
// for scope debugging |
#include "rc.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; |
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() { |
MKFlags |= MKFLAG_CALIBRATE; |
// not really used here any more. |
dynamicParams.KalmanK = -1; |
dynamicParams.KalmanMaxDrift = 0; |
dynamicParams.KalmanMaxFusion = 32; |
controlMixer_initVariables(); |
} |
void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) { |
Ki = 10300 / _Ki; |
gyroPFactor = _gyroPFactor; |
gyroIFactor = _gyroIFactor; |
yawPFactor = _yawPFactor; |
yawIFactor = _yawIFactor; |
} |
void setNormalFlightParameters(void) { |
setFlightParameters(dynamicParams.IFactor + 1, |
dynamicParams.GyroP + 10, |
staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
dynamicParams.GyroP + 10, |
dynamicParams.UserParams[6] |
); |
} |
void setStableFlightParameters(void) { |
setFlightParameters(33, 90, 120, 90, 120); |
} |
/************************************************************************/ |
/* Main Flight Control */ |
/************************************************************************/ |
void flight_control(void) { |
int16_t tmp_int; |
// Mixer Fractions that are combined for Motor Control |
int16_t yawTerm, throttleTerm, term[2]; |
// PID controller variables |
int16_t PDPart[2], PDPartYaw, PPart[2]; |
static int32_t IPart[2] = {0,0}; |
// static int32_t yawControlRate = 0; |
// Removed. Too complicated, and apparently not necessary with MEMS gyros anyway. |
// static int32_t IntegralGyroPitchError = 0, IntegralGyroRollError = 0; |
// static int32_t CorrectionPitch, CorrectionRoll; |
static uint16_t emergencyFlightTime; |
static int8_t debugDataTimer = 1; |
// High resolution motor values for smoothing of PID motor outputs |
static int16_t motorFilters[MAX_MOTORS]; |
uint8_t i, axis; |
// Fire the main flight attitude calculation, including integration of angles. |
calculateFlightAttitude(); |
/* |
* TODO: update should: Set the stick variables if good signal, set them to zero if bad. |
* Set variables also. |
*/ |
// start part 1: 750-800 usec. |
// start part 1a: 750-800 usec. |
// start part1b: 700 usec |
// start part1c: 700 usec!!!!!!!!! WAY too slow. |
controlMixer_update(); |
// end part1c |
throttleTerm = controlThrottle; |
if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10; |
else if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20); |
// end part1b: 700 usec. |
/************************************************************************/ |
/* 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 > 256) { |
// 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 = (uint16_t)staticParams.EmergencyGasDuration * 488; |
} |
// If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
if(throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) { |
// increment flight-time counter until overflow. |
if(isFlying != 0xFFFF) isFlying++; |
} else |
/* |
* When standing on the ground, do not apply I controls and zero the yaw stick. |
* Probably to avoid integration effects that will cause the copter to spin |
* or flip when taking off. |
*/ |
if(isFlying < 256) { |
IPart[PITCH] = IPart[ROLL] = 0; |
// TODO: Don't stomp on other modules' variables!!! |
controlYaw = 0; |
if(isFlying == 250) { |
// HC_setGround(); |
updateCompassCourse = 1; |
yawAngleDiff = 0; |
} |
} else { |
// Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
// Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
MKFlags |= (MKFLAG_FLY); |
} |
commands_handleCommands(); |
// if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
setNormalFlightParameters(); |
// } |
} // end else (not bad signal case) |
// end part1a: 750-800 usec. |
/* |
* 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; |
} |
} |
// yawControlRate = controlYaw; |
// Trim drift of yawAngleDiff with controlYaw. |
// TODO: We want NO feedback of control related stuff to the attitude related stuff. |
// This seems to be used as: Difference desired <--> real heading. |
yawAngleDiff -= controlYaw; |
// limit the effect |
CHECK_MIN_MAX(yawAngleDiff, -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 |
// end part 1: 750-800 usec. |
// start part 3: 350 - 400 usec. |
#define SENSOR_LIMIT (4096 * 4) |
/************************************************************************/ |
/* Calculate control feedback from angle (gyro integral) */ |
/* and angular velocity (gyro signal) */ |
/************************************************************************/ |
// The P-part is the P of the PID controller. That's the angle integrals (not rates). |
for (axis=PITCH; axis<=ROLL; axis++) { |
if(looping & ((1<<4)<<axis)) { |
PPart[axis] = 0; |
} else { // TODO: Where do the 44000 come from??? |
PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
} |
/* |
* Now blend in the D-part - proportional to the Differential of the integral = the rate. |
* Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
* where pfactor is in [0..1]. |
*/ |
PDPart[axis] = PPart[axis] + (int32_t)((int32_t)rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) |
+ (differential[axis] * (int16_t)dynamicParams.GyroD) / 16; |
CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
} |
PDPartYaw = |
(int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING) |
+ (int32_t)(yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
// limit control feedback |
CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
/* |
* Compose throttle term. |
* If a Bl-Ctrl is missing, prevent takeoff. |
*/ |
if(missingMotor) { |
// if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
if((isFlying > 1) && (isFlying < 50) && (throttleTerm > 0)) |
isFlying = 1; // keep within lift off condition |
throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of |
} |
throttleTerm *= CONTROL_SCALING; |
/* |
* Compose yaw term. |
* The yaw term is limited: Absolute value is max. = the throttle term / 2. |
* However, at low throttle the yaw term is limited to a fixed value, |
* and at high throttle it is limited by the throttle reserve (the difference |
* between current throttle and maximum throttle). |
*/ |
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING; |
// limit yawTerm |
if(throttleTerm > MIN_YAWGAS) { |
CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2)); |
} else { |
CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
} |
tmp_int = staticParams.MaxThrottle * CONTROL_SCALING; |
CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
tmp_int = (int32_t)((int32_t)dynamicParams.DynamicStability * (int32_t)(throttleTerm + abs(yawTerm) / 2)) / 64; |
for (axis=PITCH; axis<=ROLL; axis++) { |
/* |
* Compose pitch and roll terms. This is finally where the sticks come into play. |
*/ |
if(gyroIFactor) { |
// Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
// That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
// TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos. |
} else { |
// "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
// To keep up with a full stick PDPart should be about 156... |
IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
} |
// TODO: From which planet comes the 16000? |
CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
// Add (P, D) parts minus stick pos. to the scaled-down I part. |
term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch |
/* |
* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |
* The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity |
* (max. pitch or roll term is the throttle value). |
* TODO: Why a growing function of yaw? |
*/ |
CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
} |
// end part 3: 350 - 400 usec. |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Universal Mixer |
// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
for(i = 0; i < MAX_MOTORS; i++) { |
int16_t tmp; |
if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
tmp = ((int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L; |
tmp += ((int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]) / 64L; |
tmp += ((int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]) / 64L; |
tmp += ((int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]) / 64L; |
motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
tmp = motorFilters[i] / CONTROL_SCALING; |
// So this was the THIRD time a throttle was limited. But should the limitation |
// apply to the common throttle signal (the one used for setting the "power" of |
// all motors together) or should it limit the throttle set for each motor, |
// including mix components of pitch, roll and yaw? I think only the common |
// throttle should be limited. |
// CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle); |
DebugOut.Analog[22+i] = tmp; |
CHECK_MIN_MAX(tmp, 1, 255); |
Motor[i].SetPoint = tmp; |
} |
else if (motorTestActive) { |
Motor[i].SetPoint = motorTest[i]; |
} else { |
Motor[i].SetPoint = 0; |
} |
} |
I2C_Start(TWI_STATE_MOTOR_TX); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugging |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!(--debugDataTimer)) { |
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
/* |
DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |
DebugOut.Analog[24] = controlYaw; |
DebugOut.Analog[25] = yawAngleDiff / 100L; |
*/ |
DebugOut.Analog[26] = accNoisePeak[PITCH]; |
DebugOut.Analog[27] = accNoisePeak[ROLL]; |
DebugOut.Analog[30] = gyroNoisePeak[PITCH]; |
DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
} |
} |
/branches/dongfang_FC_rewrite/backup/flight.h |
---|
0,0 → 1,43 |
/*********************************************************************************/ |
/* Flight Control */ |
/*********************************************************************************/ |
#ifndef _FLIGHT_H |
#define _FLIGHT_H |
#include <inttypes.h> |
#include "analog.h" |
#include "configuration.h" |
#define PITCH 0 |
#define ROLL 1 |
/* |
* The output BLC throttles can be set in an [0..256[ interval. Some staticParams limits |
* (min, throttle, max throttle etc) are in a [0..256[ interval. |
* The calculation of motor throttle values from sensor and control information (basically |
* flight.c) take place in an [0..1024[ interval for better precision. |
* This is the conversion factor. |
* --> Replaced back again by CONTROL_SCALING. Even though the 2 things are not quite the |
* same, they are unseperable anyway. |
*/ |
extern uint8_t RequiredMotors; |
// looping params |
// extern long TurnOver180Nick, TurnOver180Roll; |
// external control |
// extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw; |
// extern int16_t naviAccPitch, naviAccRoll, naviCntAcc; |
// TODO: Rip em up, replace by a flight-state machine. |
// OK first step: Move to control mixer, where the state machine will be located anyway. |
// extern volatile uint8_t MKFlags; |
// extern uint16_t isFlying; |
void flight_control(void); |
void transmitMotorThrottleData(void); |
void flight_setNeutral(void); |
#endif //_FLIGHT_H |
/branches/dongfang_FC_rewrite/backup/gps.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 <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/backup/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/backup/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/backup/heightControl.c |
---|
0,0 → 1,168 |
#include <inttypes.h> |
#include "attitude.h" |
#include "uart0.h" |
#include "configuration.h" |
#include "controlMixer.h" |
// for digital / LED debug. |
#include "output.h" |
// For scope debugging only! |
#include "rc.h" |
#define INTEGRAL_LIMIT 100000 |
/* |
#define DEBUGINTEGRAL 0 |
#define DEBUGDIFFERENTIAL 0 |
#define DEBUGHOVERTHROTTLE 0 |
#define DEBUGHEIGHTSWITCH 0 |
*/ |
#define LATCH_TIME 40 |
int32_t groundPressure; |
int32_t targetHeight; |
int32_t rampedTargetHeight; |
#define DEFAULT_HOVERTHROTTLE 50 |
int32_t stronglyFilteredHeightDiff = 0; |
uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE; |
uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE; |
#define HOVERTHROTTLEFILTER 25 |
uint8_t heightRampingTimer = 0; |
int32_t maxHeight; |
int32_t iHeight; |
/* |
These parameters are free to take: |
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 |
*/ |
int32_t getHeight(void) { |
return groundPressure - filteredAirPressure; |
} |
void HC_setGround(void) { |
groundPressure = filteredAirPressure; |
// This should also happen when height control is enabled in-flight. |
rampedTargetHeight = getHeight(); |
maxHeight = 0; |
iHeight = 0; |
} |
void HC_update(void) { |
int32_t height = getHeight(); |
static uint8_t setHeightLatch = 0; |
if (height > maxHeight) |
maxHeight = height; |
if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) { |
DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH; |
if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255-40) { |
if (setHeightLatch <= LATCH_TIME) { |
if (setHeightLatch == LATCH_TIME) { |
targetHeight = height; |
DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH; |
} |
setHeightLatch++; |
} |
} else { |
setHeightLatch = 0; |
DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH; |
} |
} else { |
DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH; |
targetHeight = (uint16_t)dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100; |
} |
if (++heightRampingTimer == INTEGRATION_FREQUENCY/10) { |
heightRampingTimer = 0; |
if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) { |
rampedTargetHeight += staticParams.Height_Gain; |
} else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) { |
rampedTargetHeight -= staticParams.Height_Gain; |
} |
} |
//DebugOut.Analog[16] = (int16_t)(height / 10); |
//DebugOut.Analog[17] = (int16_t)(rampedTargetHeight / 10); |
} |
// ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
// ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH |
// ParamSet.BitConfig & CFG_HEIGHT_3SWITCH |
// takes 180-200 usec (with integral term). That is too heavy!!! |
// takes 100 usec without integral term. |
uint16_t HC_getThrottle(uint16_t throttle) { |
int32_t height = getHeight(); |
int32_t heightError = rampedTargetHeight - height; |
static int32_t lastHeight; |
int16_t dHeight = height - lastHeight; |
lastHeight = height; |
// DebugOut.Analog[20] = dHeight; |
// DebugOut.Analog[21] = dynamicParams.MaxHeight; |
// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
// iHeight += heightError; |
if (dHeight > 0) { |
DebugOut.Digital[0] |= DEBUG_HEIGHT_DIFF; |
DebugOut.Digital[1] &= ~DEBUG_HEIGHT_DIFF; |
} else if (dHeight < 0) { |
DebugOut.Digital[1] |= DEBUG_HEIGHT_DIFF; |
DebugOut.Digital[0] &= ~DEBUG_HEIGHT_DIFF; |
} |
/* |
if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}} |
else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }} |
else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;} |
else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;} |
*/ |
int16_t dThrottle = heightError * staticParams.HeightP / 1000 /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */ - dHeight * staticParams.HeightD; |
// the "minGas" is now a limit for how much up / down the throttle can be varied |
if (dThrottle > staticParams.HeightMinGas) dThrottle = staticParams.HeightMinGas; |
else if (dThrottle < -staticParams.HeightMinGas) dThrottle = -staticParams.HeightMinGas; |
//DebugOut.Analog[18] = dThrottle; |
//DebugOut.Analog[19] = iHeight / 10000L; |
// TODO: Eliminate repitition. |
if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) { |
if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) { |
// If switch in use, we only apply height control when switch is also on. |
if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255-40) { |
throttle += dThrottle; |
} |
} else { |
// Switch not in use - just apply height control. |
throttle += dThrottle; |
} |
} |
/* Experiment: Find hover-throttle */ |
stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER; |
stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER - 1) + throttle) / HOVERTHROTTLEFILTER; |
if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 && stronglyFilteredHeightDiff > -3) { |
hoverThrottle = stronglyFilteredThrottle; |
DebugOut.Digital[0] |= DEBUG_HOVERTHROTTLE; |
DebugOut.Analog[18] = hoverThrottle; |
} else |
DebugOut.Digital[0] &= ~DEBUG_HOVERTHROTTLE; |
return throttle; |
} |
/branches/dongfang_FC_rewrite/backup/heightControl.h |
---|
0,0 → 1,4 |
void HC_setGround(void); |
void HC_update(void); |
uint16_t HC_getThrottle(uint16_t throttle); |
/branches/dongfang_FC_rewrite/backup/invenSense.c |
---|
0,0 → 1,42 |
#include "invenSense.h" |
#include "timer0.h" |
#include "configuration.h" |
#include <avr/io.h> |
/* |
* Configuration for my prototype board with InvenSense gyros. |
* The FC 1.3 board is installed upside down, therefore Z acc is reversed but not roll. |
*/ |
const uint8_t GYRO_REVERSED[3] = {0,0,0}; |
const uint8_t ACC_REVERSED[3] = {0,0,1}; |
#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 = 10; |
// Not used. |
staticParams.AngleTurnOverPitch = 85; |
staticParams.AngleTurnOverRoll = 85; |
} |
/branches/dongfang_FC_rewrite/backup/invenSense.h |
---|
0,0 → 1,30 |
/* |
#ifndef _INVENSENSE_H |
#define _INVENSENSE_H |
#include "sensors.h" |
#define GYRO_HW_NAME "ISens" |
/ * |
* 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 |
#endif |
*/ |
/branches/dongfang_FC_rewrite/backup/main.c |
---|
0,0 → 1,305 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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); |
OUTPUT_ON(0); |
GRN_OFF; |
RED_ON; |
while(!CheckDelay(timer)); |
timer = SetDelay(200); |
OUTPUT_OFF(0); |
OUTPUT_ON(1); |
RED_OFF; |
GRN_ON; |
while(!CheckDelay(timer)); |
timer = SetDelay(200); |
while(!CheckDelay(timer)); |
OUTPUT_OFF(1); |
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(); |
// 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; |
/* |
* If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit |
* the throttle vector just computed. Otherwise, if motor test is engaged, transmit |
* the test throttle vector. If no testing, stop all motors. |
*/ |
// Obsoleted. |
// transmitMotorThrottleData(); |
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 |
if( !runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
usart0_TransmitTxData(); |
} |
usart0_ProcessRxData(); |
if(CheckDelay(timer)) { |
if (UBat <= UBAT_AT_5V) { |
// Do nothing. The voltage on the input side of the regulator is <5V; |
// we must be running off USB power. Keep it quiet. |
} else 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/backup/main.h |
---|
0,0 → 1,3 |
#ifndef _MAIN_H |
#define _MAIN_H |
#endif //_MAIN_H |
/branches/dongfang_FC_rewrite/backup/makefile |
---|
0,0 → 1,524 |
#-------------------------------------------------------------------- |
# MCU name |
MCU = atmega644p |
F_CPU = 20000000 |
#------------------------------------------------------------------- |
VERSION_MAJOR = 0 |
VERSION_MINOR = 74 |
VERSION_PATCH = 100 |
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_HW_NAME=ENC |
GYRO_HW_FACTOR=1.304f |
GYRO_PITCHROLL_CORRECTION=1.11f |
GYRO_YAW_CORRECTION=1.28f |
#GYRO=ADXRS610_FC2.0 |
#GYRO_HW_NAME=ADXR |
#GYRO_HW_FACTOR=1.2288f |
#GYRO_PITCHROLL_CORRECTION=1.0f |
#GYRO_YAW_CORRECTION=1.0f |
#GYRO=invenSense |
#GYRO_HW_NAME=Isense |
#GYRO_HW_FACTOR=0.6827f |
#GYRO_PITCHROLL_CORRECTION=0.93f |
#GYRO_YAW_CORRECTION=0.97f |
#GYRO= |
#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 |
ifeq ($(VERSION_PATCH), 100) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)df_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 |
SRC += externalControl.c GPSControl.c dongfangMath.c twimaster.c rc.c attitude.c flight.c |
SRC += eeprom.c uart1.c heightControl.c configuration.c attitudeControl.c commands.c $(GYRO).c |
ifeq ($(EXT), MK3MAG) |
SRC += mk3mag.c gps.c ubx.c |
#mymath.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) -DGYRO_HW_NAME=${GYRO_HW_NAME} -DGYRO_HW_FACTOR=${GYRO_HW_FACTOR} -DGYRO_PITCHROLL_CORRECTION=${GYRO_PITCHROLL_CORRECTION} -DGYRO_YAW_CORRECTION=${GYRO_YAW_CORRECTION} |
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/backup/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", angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL); |
LCD_printfxy(0,2,"Roll: %5i", angle[ROLL ] / 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/backup/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/backup/mk3mag.c |
---|
0,0 → 1,131 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "attitude.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/backup/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/backup/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/backup/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/backup/navi-lang.txt |
---|
0,0 → 1,52 |
Navigation language def: |
Som funktion af t: |
- Pos. |
- Hoejde |
- Synsretning (X-akse antaget vandret?) (som: Fast, lineaer, as-takeoff, at-position, look-forward) |
- Evt. naeseretning (som: Fast, lineaer, follow-look, as-takeoff, head-at, tail-at, point-forward) |
- Positionsfunktion interpoleret mellem waypoints |
- Hoejde det samme |
- Retninger det samme |
- For hvert waypoint: |
Position |
Hoejde |
Standtid |
Se-retning??? (i hvert optional) |
- Valgmuligheder for positions-interpolation: Rette linier, curvefitting af en eller flere slags... |
<flight> |
<waypoint lon="...." lat="...." height="...." /> |
<leg pos-interpolation="linear" /> |
<waypoint lon="...." lat="...." height="...." head="270" /> |
<leg pos-interpolation="linear" head="linear" /> |
<waypoint lon="...." lat="...." height="...." head="180" /> |
</flight> |
class Waypoint { |
Coordinates pos; |
int height; |
int heading; |
} |
function linear-height(Waypoint w1, Waypoint w2, float x) { |
return w1.height + (w2.height - w1.height) * x; |
} |
Fancy udgave: |
<waypoint> |
<lat></lat> |
<lon></lon> |
<height></height> |
</waypoint> |
<leg> |
<height>linear(w_o, w_t)</height> |
</leg> |
2 interpreters: 1 til Google Maps ell l og 1 til navi-ctrl. |
/branches/dongfang_FC_rewrite/backup/naviControl.c |
---|
--- naviControl.h (nonexistent) |
+++ naviControl.h (revision 1775) |
@@ -0,0 +1,95 @@ |
+//void NC_update(void); |
+//int16_t* NC_getPRTY(void); |
+//uint8_t NC_getArgument(void); |
+//uint8_t NC_getCommand(void); |
+//int16_t NC_getVariable(uint8_t varNum); |
+//void NC_calibrate(void); |
+//uint8_t NC_getSignalQuality (void); |
+//void NC_setNeutral(void); |
+ |
+// ######################## SPI - FlightCtrl ################### |
+#ifndef _NAVICONTROL_H |
+#define _NAVICONTROL_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 IntegralPitch; |
+ int16_t IntegralRoll; |
+ int16_t AccPitch; |
+ int16_t AccRoll; |
+ int16_t GyroHeading; |
+ int16_t GyroPitch; |
+ 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 GPSStickPitch; |
+ 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; |
+ // unsigned char Hardware; |
+} __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); |
+// new: |
+// extern void UpdateSPI_Buffer(void); |
+ |
+#endif //_NAVICONTROL_H |
/branches/dongfang_FC_rewrite/backup/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/backup/output.c |
---|
0,0 → 1,112 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 flashCnt[2], flashMask[2]; |
// 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); |
OUTPUT_OFF(0); OUTPUT_OFF(1); |
flashCnt[0] = flashCnt[1] = 0; |
flashMask[0] = flashMask[1] = 128; |
} |
void flashingLight(uint8_t port, uint8_t timing, uint8_t bitmask, uint8_t manual) { |
if (timing > 250 && manual > 230) { |
// "timing" is set to "manual" and the value is very high --> Set to the value in bitmask bit 7. |
OUTPUT_SET(port, bitmask & 128); |
} else if (timing > 250 && manual < 10) { |
// "timing" is set to "manual" and the value is very low --> Set to the negated value in bitmask bit 7. |
OUTPUT_SET(port, !(bitmask & 128)); |
} else if(!flashCnt[port]--) { |
// rotating mask over bitmask... |
flashCnt[port] = timing - 1; |
if(flashMask[port] == 1) flashMask[port] = 128; else flashMask[port] >>= 1; |
OUTPUT_SET(port, flashMask[port] & bitmask); |
} |
} |
void flashingLights(void) { |
static int8_t delay = 0; |
if(!delay--) { // 10 ms intervall |
delay = 4; |
flashingLight(0, staticParams.J16Timing, staticParams.J16Bitmask, dynamicParams.J16Timing); |
flashingLight(1, staticParams.J17Timing, staticParams.J17Bitmask, dynamicParams.J17Timing); |
} |
} |
#define DIGITAL_DEBUG_MASK DEBUG_SERIAL |
#define DIGITAL_DEBUG_INVERT 1 |
void output_update(void) { |
uint8_t output0, output1; |
if (!DIGITAL_DEBUG_MASK) |
flashingLights(); |
else { |
if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST) { |
// Show the state for a SET bit. If inverse, then invert. |
output0 = output1 = ~DIGITAL_DEBUG_INVERT; |
} else if (DIGITAL_DEBUG_INVERT) { |
output0 = (~DebugOut.Digital[0]) & DIGITAL_DEBUG_MASK; |
output1 = (~DebugOut.Digital[1]) & DIGITAL_DEBUG_MASK; |
} else { |
output0 = DebugOut.Digital[0] & DIGITAL_DEBUG_MASK; |
output1 = DebugOut.Digital[1] & DIGITAL_DEBUG_MASK; |
} |
OUTPUT_SET(0, output0); |
OUTPUT_SET(1, output1); |
} |
} |
/branches/dongfang_FC_rewrite/backup/output.h |
---|
0,0 → 1,26 |
#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 OUTPUT_ON(num) {PORTC |= (4 << (num));} |
#define OUTPUT_OFF(num) {PORTC &= ~(4 << (num));} |
#define OUTPUT_SET(num, state) {if ((state)) OUTPUT_ON(num) else OUTPUT_OFF(num)} |
#define OUTPUT_TOGGLE(num) ( {PORTC ^= (4 << (num));} |
#define DEBUG_LEDTEST 256 |
#define DEBUG_HEIGHT_SWITCH 1 |
#define DEBUG_HEIGHT_DIFF 2 |
#define DEBUG_HOVERTHROTTLE 4 |
#define DEBUG_ACC0THORDER 8 |
#define DEBUG_SERIAL 16 |
void output_init(void); |
void output_update(void); |
#endif //_output_H |
/branches/dongfang_FC_rewrite/backup/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/backup/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/backup/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/backup/rc.c |
---|
0,0 → 1,436 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "uart0.h" |
#include "controlMixer.h" |
#include "configuration.h" |
#include "commands.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]; |
uint8_t lastRCCommand = COMMAND_NONE; |
uint8_t commandTimer = 0; |
// Useless. Just trim on the R/C instead. |
// 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) | (1<<DDD3); |
// low level |
PORTD &= ~((1<<PORTD5) | (1<<PORTD4) | (1<<PORTD3)); |
// 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 -= 470; // 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; |
} |
// If signal is the same as before +/- 1, just keep it there. |
if (signal>=PPM_in[index]-1 && signal<=PPM_in[index]+1) { |
// In addition, if the signal is very close to 0, just set it to 0. |
if (signal >=-1 && signal <= 1) { |
tmp = 0; |
} else { |
tmp = PPM_in[index]; |
} |
} |
else |
tmp = signal; |
// 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]] |
#define COMMAND_THRESHOLD 85 |
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
// Internal. |
uint8_t RC_getStickCommand(void) { |
if(RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) { |
// vertical is up |
if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
return COMMAND_GYROCAL; |
if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
return COMMAND_ACCCAL; |
return COMMAND_NONE; |
} else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) { |
// vertical is down |
if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
return COMMAND_STOP; |
if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
return COMMAND_START; |
return COMMAND_NONE; |
} |
// vertical is around center |
return COMMAND_NONE; |
} |
/* |
* 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) * staticParams.StickP - /* stickOffsetPitch */ + RCDiff(CH_PITCH) * staticParams.StickD; |
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP - /* stickOffsetRoll */ + RCDiff(CH_ROLL) * staticParams.StickD; |
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 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 yawing 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; |
DebugOut.Analog[12] = RCChannel(CH_PITCH); |
DebugOut.Analog[13] = RCChannel(CH_ROLL); |
} |
uint8_t command = RC_getStickCommand(); |
if (lastRCCommand == command && commandTimer < COMMAND_TIMER) { |
commandTimer++; |
} else { |
lastRCCommand = command; |
commandTimer = 0; |
} |
} 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 + 4) + POT_OFFSET; |
/* |
* 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] + POT_OFFSET; |
} |
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) { |
// Do nothing. |
} |
/* |
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) * staticParams.StickP; |
stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP; |
} |
} |
*/ |
uint8_t RC_getCommand(void) { |
if (commandTimer == COMMAND_TIMER) { |
// Stick has been held long enough; command committed. |
return lastRCCommand; |
} |
// Not yet sure what the command is. |
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 |
#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH |
#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL |
uint8_t RC_getArgument(void) { |
if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) { |
// vertical is up |
if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 2; |
if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 4; |
return 3; |
} else if(RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) { |
// vertical is down |
if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 8; |
if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 6; |
return 7; |
} else { |
// vertical is around center |
if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 1; |
if(RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -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; |
} |
uint8_t RC_testCompassCalState(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; |
return 1; |
} |
return 0; |
} |
/* |
* 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/backup/rc.h |
---|
0,0 → 1,55 |
#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 |
// Number of cycles a command must be repeated before commit. |
#define COMMAND_TIMER 200 |
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 115 |
/* |
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); |
uint8_t RC_testCompassCalState(void); |
#endif //_RC_H |
/branches/dongfang_FC_rewrite/backup/readme.txt |
---|
0,0 → 1,45 |
Dongfang's FC firmware rewrite |
------------------------------ |
The basis was V0.74d Code Redesign Killagreg. |
Goal: |
- To be the preferred hacker-friendly FC firmware. |
- To use H&Is design experience but make the code clearer and easier to extend+modify. |
Non-goal: |
- To follow up on all changes and all new features in the H&I firmware. |
Features: |
- Readable, reasonably documented source code makes it easy to implement the features you |
want yourself (and to remove those you don't like). |
- The code was broken into smaller pieces and modularized (fc.c in particular). |
Global variables are (almost) only written to by one module each. |
- New or experimental hardware is easy to incorporate. Gyro and acc. meter axes are reversible, |
and resetting the sensitivity actually works. |
- New or experiemental controls are easy to incorporate. All controls (eg. R/C, external |
serial, NC, automatic emergency landing pilot and automatic altitude controller pilot) |
are potentially abstractable to one interface. |
- Reversal of gyro or accelerometer axes is easy. It is easy to adapt the firmware for |
upside down installation of the FC too. |
- The firmware is compatible with MK-Tool. This may be changed, if somebody writes a new |
MK-Tool which is easy to adapt for addition of / removal of features. |
Non-features (currently): |
- Navi support temporarily removed (should be added again later). |
- Compass support temporarily removed (should be added again later). |
- Parts of menu.c are dummy implemented or removed. It is possible that menu.c and all use |
of printf will be removed later, and replaced by something else (debud on steroids). |
- Control rate limiter removed. |
- Altitude control temporarily removed (should be added again later). |
- Automatic board detection removed. This firmware is for compiling yourself, possibly |
with nonstandard or experimental hardware. That conflicts with automatically switching |
between standard hardware versions, so the feature was removed. Instead, is was made |
easy to choose gyro types etc. in the makefile. |
How to build: |
- Choose a gyro definition, depending on your hardware, and enable it in the makefile |
and (GYRO=.....) and #include its header file in analog.h (how to make that follow the |
makefile automatically?). Currently, ENC-03_FC1.3, ADXRS610_FC2.0 and invenSense are |
supported (each has a .c and a .h file). |
- make all |
/branches/dongfang_FC_rewrite/backup/sendOutData.c |
---|
0,0 → 1,45 |
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
va_list ap; |
uint16_t txd_bufferIndex = 0; |
uint8_t *currentBuffer; |
uint8_t currentBufferIndex; |
uint16_t lengthOfCurrentBuffer; |
uint8_t shift = 0; |
txd_buffer[txd_bufferIndex++] = '#'; // Start character |
txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...) |
txd_buffer[txd_bufferIndex++] = cmd; // Command |
va_start(ap, numofbuffers); |
while(numofbuffers) { |
currentBuffer = va_arg(ap, uint8_t*); |
lengthOfCurrentBuffer = va_arg(ap, int); |
currentBufferIndex = 0; |
// Encode data: 3 bytes of data are encoded into 4 bytes, |
// where the 2 most significant bits are both 0. |
while(currentBufferIndex != lengthOfCurrentBuffer) { |
if (!shift) txd_buffer[txd_bufferIndex] = 0; |
txd_buffer[txd_bufferIndex] |= currentBuffer[currentBufferIndex] >> (shift + 2); |
txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111; |
shift += 2; |
if (shift == 6) { shift=0; txd_bufferIndex++; } |
currentBufferIndex++; |
} |
} |
// If the number of data bytes was not divisible by 3, stuff |
// with 0 pseudodata until length is again divisible by 3. |
if (shift == 2) { |
// We need to stuff with zero bytes at the end. |
txd_buffer[txd_bufferIndex] &= 0b00110000; |
txd_buffer[++txd_bufferIndex] = 0; |
shift = 4; |
} |
if (shift == 4) { |
// We need to stuff with zero bytes at the end. |
txd_buffer[txd_bufferIndex++] &= 0b00111100; |
txd_buffer[txd_bufferIndex] = 0; |
} |
va_end(ap); |
AddCRC(pt); // add checksum after data block and initates the transmission |
} |
/branches/dongfang_FC_rewrite/backup/sensors.h |
---|
0,0 → 1,29 |
#ifndef _SENSORS_H |
#define _SENSORS_H |
#include <inttypes.h> |
/* |
* Whether (pitch, roll, yaw) gyros are reversed (see analog.h). |
*/ |
extern const uint8_t GYRO_REVERSED[3]; |
/* |
* Whether (pitch, roll, Z) acc. meters are reversed(see analog.h). |
*/ |
extern const uint8_t ACC_REVERSED[3]; |
/* |
* Common procedures for all gyro types. |
* 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); |
/* |
* Set some default FC parameters, depending on gyro type: Drift correction etc. |
*/ |
void gyro_setDefaults(void); |
#endif |
/branches/dongfang_FC_rewrite/backup/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/backup/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/backup/spi.c |
---|
0,0 → 1,421 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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.IntegralPitch = 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.IntegralPitch = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
toNaviCtrl.IntegralRoll = (int16_t)((10 * angle[ROLL]) / 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.GyroPitch = rate_ATT[PITCH]; |
toNaviCtrl.GyroRoll = rate_ATT[ROLL]; |
toNaviCtrl.GyroYaw = yawRate; |
toNaviCtrl.AccPitch = (10 * getAngleEstimateFromAcc(PITCH)) / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
toNaviCtrl.AccRoll = (10 * getAngleEstimateFromAcc(ROLL)) / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
// TODO: What are these little bastards? |
averageAcc[PITCH] = averageAcc[ROLL] = averageAccCount = 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; |
// TODO: Height and in the correct scaling... |
toNaviCtrl.Param.Int[1] = 0; //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.GPSStickPitch) < 512 && abs(fromNaviCtrl.GPSStickRoll) < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) { |
GPSStickPitch = fromNaviCtrl.GPSStickPitch; |
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]; |
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; |
} else { // checksum does not match |
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/backup/spi.h |
---|
0,0 → 1,86 |
// ######################## 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 IntegralPitch; |
int16_t IntegralRoll; |
int16_t AccPitch; |
int16_t AccRoll; |
int16_t GyroHeading; |
int16_t GyroPitch; |
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 GPSStickPitch; |
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; |
// unsigned char Hardware; |
} __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); |
// new: |
// extern void UpdateSPI_Buffer(void); |
#endif //_SPI_H |
/branches/dongfang_FC_rewrite/backup/timer0.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 <inttypes.h> |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "eeprom.h" |
#include "analog.h" |
// for degugging! |
#include "rc.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^=1; |
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/backup/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/backup/timer2.c |
---|
0,0 → 1,296 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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; |
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; |
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/backup/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/backup/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/backup/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/backup/uart0.c |
---|
0,0 → 1,734 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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/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" |
#include "output.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 request_variables = 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(PID) ", |
"GyroRoll(PID) ", |
"GyroYaw ", //5 |
"GyroPitch(AC) ", |
"GyroRoll(AC) ", |
"GyroYaw(AC) ", |
"AccPitch (angle)", |
"AccRoll (angle) ", //10 |
"UBat ", |
"RC Pitch ", |
"RC Roll ", |
"Corr 0/00 pitch ", |
"Corr 0/00 roll ", //15 |
"Corr pitch ", |
"Corr roll ", |
"Acc pitch filter", |
"Acc roll filter ", |
"ADPitchGyroOffs ", //20 |
"ADRollGyroOffs ", |
"M1 ", |
"M2 ", |
"M3 ", |
"M4 ", //25 |
"Pitch acc noise ", |
"Roll acc noise ", |
"DriftCompPitch ", |
"DriftCompRoll ", |
"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; |
DebugOut.Digital[0] ^= DEBUG_SERIAL; |
DebugOut.Digital[1] ^= DEBUG_SERIAL; |
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) |
} |
// -------------------------------------------------------------------------- |
// application example: |
// SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16); |
/* |
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
va_list ap; |
uint16_t txd_bufferIndex = 0; |
uint8_t *currentBuffer; |
uint8_t currentBufferIndex; |
uint16_t lengthOfCurrentBuffer; |
uint8_t shift = 0; |
txd_buffer[txd_bufferIndex++] = '#'; // Start character |
txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...) |
txd_buffer[txd_bufferIndex++] = cmd; // Command |
va_start(ap, numofbuffers); |
while(numofbuffers) { |
currentBuffer = va_arg(ap, uint8_t*); |
lengthOfCurrentBuffer = va_arg(ap, int); |
currentBufferIndex = 0; |
// Encode data: 3 bytes of data are encoded into 4 bytes, |
// where the 2 most significant bits are both 0. |
while(currentBufferIndex != lengthOfCurrentBuffer) { |
if (!shift) txd_buffer[txd_bufferIndex] = 0; |
txd_buffer[txd_bufferIndex] |= currentBuffer[currentBufferIndex] >> (shift + 2); |
txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111; |
shift += 2; |
if (shift == 6) { shift=0; txd_bufferIndex++; } |
currentBufferIndex++; |
} |
} |
// If the number of data bytes was not divisible by 3, stuff |
// with 0 pseudodata until length is again divisible by 3. |
if (shift == 2) { |
// We need to stuff with zero bytes at the end. |
txd_buffer[txd_bufferIndex] &= 0b00110000; |
txd_buffer[++txd_bufferIndex] = 0; |
shift = 4; |
} |
if (shift == 4) { |
// We need to stuff with zero bytes at the end. |
txd_buffer[txd_bufferIndex++] &= 0b00111100; |
txd_buffer[txd_bufferIndex] = 0; |
} |
va_end(ap); |
AddCRC(pt); // add checksum after data block and initates the transmission |
} |
*/ |
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) { |
// We control the motorTestActive var from here: Count it down. |
if (motorTestActive) motorTestActive--; |
// 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); |
} |
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 'x': |
request_variables = 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 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
Data3D.AngleRoll = (int16_t)((10 * angle[ROLL]) / 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 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // 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; |
} |
if (request_variables && txd_complete) { |
SendOutData('X', FC_ADDRESS, 1, (uint8_t *)&variables, sizeof(variables)); |
request_variables = FALSE; |
} |
} |
/branches/dongfang_FC_rewrite/backup/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/backup/uart1.c |
---|
0,0 → 1,163 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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/backup/uart1.h |
---|
0,0 → 1,6 |
#ifndef _UART1_H |
#define _UART1_H |
extern void usart1_Init (void); |
#endif //_UART1_H |
/branches/dongfang_FC_rewrite/backup/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/backup/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/backup/userparams.txt |
---|
0,0 → 1,36 |
Userparam 3: Projection throttle effect. |
Userparam 4: Throttle stick D value. Recommended: 12-15. Good fun to fly with. |
Userparam 5: Filter control. |
bits 0-1: Gyro signal for the flight control PID (filter constants 1-4) |
bits 2-3: Gyro signal for the attitude angle integration (ATT) (filter constants 1-4) |
bits 4-5: GyroD (filter constants 1-4) |
bits 6-7: Acc. (filter constants 1-4) |
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 (may be nasty to fly!). Recommended: ca. 100. |
Userparam8: Acc integral correction Z axis limit. Default 0 (!) |
Other params: |
GyroAccFactor = 0'th order integral drift correction - acceleration sensor part as 1/1000s. |
Default 5 (ENC-03) |
GyroAccTrim = -1'th order integral drift correction - |
Offset corrections are divided by this before added to offsets. Default 2. |
DriftComp = Max offset correction per iteration (=per 1/2 second). |
This limits the value _before_ division by GyroAccTrim. Default 32 (ENC-03) |
Rotary rate limiter flag ON = The gyro signals for flight control are extrapolated with |
a sharp rise in "virtual" rotation rates when the gyros are near saturation. This |
prevents (via the flight control) the copter from turning fast enough that the gyros |
saturate. Saturation will have the effect that stick controls overwhelm the (now clipped) |
gyro feedback in the flight control, and the copter flips VERY fast. Saturation is |
normally undesirable but it provides for a very entertaining stunt! |
The rotary rate limiter (beginner feature to make the copter _very_ irresponsive to |
stick controls) was removed. |
/branches/dongfang_FC_rewrite/backup/yaw-analysis.txt |
---|
0,0 → 1,353 |
// gyro readings |
int16_t GyroNick, GyroRoll, GyroYaw; |
int16_t TrimNick, TrimRoll; |
int32_t IntegralGyroYaw = 0; |
int32_t ReadingIntegralGyroYaw = 0; |
// compass course |
int16_t CompassHeading = -1; // negative angle indicates invalid data. |
int16_t CompassCourse = -1; |
int16_t CompassOffCourse = 0; |
uint8_t CompassCalState = 0; |
uint16_t BadCompassHeading = 500; |
int32_t YawGyroHeading; // Yaw Gyro Integral supported by compass |
int16_t YawGyroDrift; |
uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control |
int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0; |
int16_t GPSStickNick = 0, GPSStickRoll = 0; |
// stick values derived by uart inputs |
int16_t ExternStickNick = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20; |
/************************************************************************/ |
/* Neutral Readings */ |
/************************************************************************/ |
void SetNeutral(uint8_t AccAdjustment) |
{ |
... |
AdBiasGyroYaw = (int16_t)((Sum_3 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE); |
GyroYaw = 0; |
CompassCourse = CompassHeading; |
// Inititialize YawGyroIntegral value with current compass heading |
YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR; |
YawGyroDrift = 0; |
// Something completely different: Here's why the turn-over's were vars. |
TurnOver180Nick = ((int32_t) ParamSet.AngleTurnOverNick * 2500L) +15000L; |
TurnOver180Roll = ((int32_t) ParamSet.AngleTurnOverRoll * 2500L) +15000L; |
} |
/************************************************************************/ |
/* Averaging Measurement Readings */ |
/************************************************************************/ |
void Mean(void) |
{ |
GyroYaw = AdBiasGyroYaw - AdValueGyroYaw; |
// Yaw |
// calculate yaw gyro integral (~ to rotation angle) |
YawGyroHeading += GyroYaw; |
ReadingIntegralGyroYaw += GyroYaw; |
// Coupling fraction |
if(! LoopingNick && !LoopingRoll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) |
{ |
.... |
YawGyroHeading += tmp14; |
if(!FCParam.AxisCouplingYawCorrection) ReadingIntegralGyroYaw -= tmp14 / 2; // force yaw |
if(abs(GyroYaw > 64)) |
{ |
if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1; |
} |
} |
// Yaw |
// limit YawGyroHeading proportional to 0? to 360? |
if(YawGyroHeading >= (360L * GYRO_DEG_FACTOR)) YawGyroHeading -= 360L * GYRO_DEG_FACTOR; // 360? Wrap |
if(YawGyroHeading < 0) YawGyroHeading += 360L * GYRO_DEG_FACTOR; |
IntegralGyroYaw = ReadingIntegralGyroYaw; |
} |
void SetCompassCalState(void) |
{ |
static uint8_t stick = 1; |
// if nick is centered or top set stick to zero |
if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -20) stick = 0; |
// if nick is down trigger to next cal state |
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) && !stick) |
{ |
stick = 1; |
CompassCalState++; |
if(CompassCalState < 5) Beep(CompassCalState); |
else BeepTime = 1000; |
} |
} |
/************************************************************************/ |
/* MotorControl */ |
/************************************************************************/ |
void MotorControl(void) |
{ |
int16_t h, tmp_int; |
// Mixer Fractions that are combined for Motor Control |
int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction; |
int16_t PDPartYaw; |
static int32_t SetPointYaw = 0; |
static uint16_t UpdateCompassCourse = 0; |
Mean(); |
GRN_ON; |
if(RC_Quality > 140) |
{ |
if(ModelIsFlying < 256) |
{ |
StickYaw = 0; |
if(ModelIsFlying == 250) |
{ |
UpdateCompassCourse = 1; |
ReadingIntegralGyroYaw = 0; |
SetPointYaw = 0; |
} |
} |
else MKFlags |= (MKFLAG_FLY); // set fly flag |
...... |
if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) |
{ |
// if roll stick is centered and nick stick is down |
if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 30 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) |
{ |
CompassCalState = 1; |
BeepTime = 1000; |
} |
(R/C data): |
// mapping of yaw |
StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
// (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction) |
if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) |
{ |
if (StickYaw > 2) StickYaw-= 2; |
else if (StickYaw< -2) StickYaw += 2; |
else StickYaw = 0; |
} |
if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128) |
{ |
StickYaw += ExternControl.Yaw; |
// disable I part of gyro control feedback |
if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) GyroIFactor = 0; |
// MeasurementCounter is incremented in the isr of analog.c |
if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached |
{ |
if(ParamSet.DriftComp) |
{ |
if(YawGyroDrift > BALANCE_NUMBER/2) AdBiasGyroYaw++; |
if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--; |
} |
YawGyroDrift = 0; |
// Yawing |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(abs(StickYaw) > 15 ) // yaw stick is activated |
{ |
BadCompassHeading = 1000; |
if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) |
{ |
UpdateCompassCourse = 1; |
} |
} |
// exponential stick sensitivity in yawring rate |
tmp_int = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx? |
tmp_int += (ParamSet.StickYawP * StickYaw) / 4; |
SetPointYaw = tmp_int; |
// trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw) |
ReadingIntegralGyroYaw -= tmp_int; |
// limit the effect |
CHECK_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Compass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// compass code is used if Compass option is selected |
if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) |
{ |
int16_t w, v, r,correction, error; |
if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) ) |
{ |
SetCompassCalState(); |
#ifdef USE_KILLAGREG |
MM3_Calibrate(); |
#endif |
} |
else |
{ |
#ifdef USE_KILLAGREG |
static uint8_t updCompass = 0; |
if (!updCompass--) |
{ |
updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
MM3_Heading(); |
} |
#endif |
// get maximum attitude angle |
w = abs(IntegralGyroNick / 512); |
v = abs(IntegralGyroRoll / 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)) % 360) - 180; |
if(abs(GyroYaw) > 128) // spinning fast |
{ |
error = 0; |
} |
if(!BadCompassHeading && w < 25) |
{ |
YawGyroDrift += error; |
if(UpdateCompassCourse) |
{ |
BeepTime = 200; |
YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR; |
CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR); |
UpdateCompassCourse = 0; |
} |
} |
YawGyroHeading += (error * 8) / correction; |
w = (w * FCParam.CompassYawEffect) / 32; |
w = FCParam.CompassYawEffect - w; |
if(w >= 0) |
{ |
if(!BadCompassHeading) |
{ |
v = 64 + (MaxStickNick + MaxStickRoll) / 8; |
// calc course deviation |
r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180; |
v = (r * w) / v; // align to compass course |
// limit yaw rate |
w = 3 * FCParam.CompassYawEffect; |
if (v > w) v = w; |
else if (v < -w) v = -w; |
ReadingIntegralGyroYaw += v; |
} |
else |
{ // wait a while |
BadCompassHeading--; |
} |
} |
else |
{ // ignore compass at extreme attitudes for a while |
BadCompassHeading = 500; |
} |
} |
} |
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / CONTROL_SCALING) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING; // yaw controller |
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
// limit YawMixFraction |
if(GasMixFraction > MIN_YAWGAS) |
{ |
CHECK_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2)); |
} |
else |
{ |
CHECK_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
} |
tmp_int = ParamSet.GasMax * CONTROL_SCALING; |
CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction)); |
--------------- |
anal-lyse: |
YawMixFraction = PDPartYaw - SetPointYaw * CONTROL_SCALING; |
OK |
SetPointYaw->setPointYaw: |
- static |
- Set to 0 at take off |
- Set to yaw stick val (StickYawP/512 * StickYaw^2 + StickYawP/4 * StickYaw) |
OK |
PDPartYaw: |
- nonstatic nonglobal |
- = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) |
/ |
(256L / CONTROL_SCALING) |
+ (int32_t)(IntegralGyroYaw * GyroYawIFactor) |
/ |
(2 * (44000 / CONTROL_SCALING)); |
OK |
GyroYaw: |
- global |
- = AdBiasGyroYaw - AdValueGyroYaw (pretty much the raw offset gyro) |
OK |
YawGyroHeading->yawGyroHeading: |
- GyroYaw is summed to it at each iteration |
- It is wrapped around at <0 and >=360. |
- It is used -- where???? To adjust CompassCourse... |
OK |
IntegralGyroYaw->yawAngle: Deviation of heading from desired??? |
- GyroYaw is summed to it at each iteration |
- SetPointYaw is subtracted from it (!) at each iteration. |
- It is NOT wrapped, but just limited to +/- 50000 |
- It is corrected / pulled in axis coupling and by the compass. |
OK (Except that with the compass) |
BadCompassHeading: Need to update the "CompassCourse". |
CompassHeading: Opdateret fra mm3mag. |
CompassOffCourse: CompassHeading - CompassCourse. Opdateret fra mm3mag. |
UpdateCompassCourse: Set CompassCourse to the value of CompassHeading and set YawgyroHeading = compassHeading * GYRO_DEG_FACTOR |