/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/FlightControl.pnproj |
---|
0,0 → 1,0 |
<Project name="FlightControl"><Folder name="Sources"><File path="ubx.c"></File><File path="analog.c"></File><File path="dsl.c"></File><File path="eeprom.c"></File><File path="fc.c"></File><File path="gps.c"></File><File path="led.c"></File><File path="main.c"></File><File path="menu.c"></File><File path="mk3mag.c"></File><File path="mm3.c"></File><File path="mymath.c"></File><File path="printf_P.c"></File><File path="rc.c"></File><File path="spectrum.c"></File><File path="spi.c"></File><File path="timer0.c"></File><File path="timer2.c"></File><File path="twimaster.c"></File><File path="uart0.c"></File><File path="uart1.c"></File></Folder><Folder name="Header"><File path="ubx.h"></File><File path="analog.h"></File><File path="dsl.h"></File><File path="eeprom.h"></File><File path="fc.h"></File><File path="gps.h"></File><File path="led.h"></File><File path="main.h"></File><File path="menu.h"></File><File path="mk3mag.h"></File><File path="mm3.h"></File><File path="mymath.h"></File><File path="old_macros.h"></File><File path="printf_P.h"></File><File path="rc.h"></File><File path="spectrum.h"></File><File path="spi.h"></File><File path="timer0.h"></File><File path="timer2.h"></File><File path="twimaster.h"></File><File path="uart0.h"></File><File path="uart1.h"></File></Folder><File path="License.txt"></File><File path="makefile"></File><File path="version.txt"></File></Project> |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/FlightControl.pnps |
---|
0,0 → 1,0 |
<pd><ViewState><e p="FlightControl" x="true"></e><e p="FlightControl\Header" x="true"></e><e p="FlightControl\Sources" x="true"></e></ViewState></pd> |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/analog.c |
---|
0,0 → 1,355 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "analog.h" |
#include "main.h" |
#include "timer0.h" |
#include "fc.h" |
#include "printf_P.h" |
#include "eeprom.h" |
#include "twimaster.h" |
volatile uint16_t Test = 0; |
volatile int16_t UBat = 100; |
volatile int16_t AdValueGyroNick = 0, AdValueGyroRoll = 0, AdValueGyroYaw = 0; |
volatile int16_t FilterHiResGyroNick = 0, FilterHiResGyroRoll = 0; |
volatile int16_t HiResGyroNick = 2500, HiResGyroRoll = 2500; |
volatile int16_t AdValueAccRoll = 0, AdValueAccNick = 0, AdValueAccTop = 0, AdValueAccZ = 0; |
volatile int32_t AirPressure = 32000; |
volatile uint8_t average_pressure = 0; |
volatile int16_t StartAirPressure; |
volatile uint16_t ReadingAirPressure = 1023; |
volatile int16_t HeightD = 0; |
volatile uint16_t MeasurementCounter = 0; |
volatile uint8_t ADReady = 1; |
uint8_t DacOffsetGyroNick = 115, DacOffsetGyroRoll = 115, DacOffsetGyroYaw = 115; |
uint8_t GyroDefectNick = 0, GyroDefectRoll = 0, GyroDefectYaw = 0; |
int8_t ExpandBaro = 0; |
uint8_t PressureSensorOffset; |
/*****************************************************/ |
/* Initialize Analog Digital Converter */ |
/*****************************************************/ |
void ADC_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) | 0x00; |
//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 |
ADC_Enable(); |
// restore global interrupt flags |
SREG = sreg; |
} |
void SearchAirPressureOffset(void) |
{ |
uint8_t off; |
off = GetParamByte(PID_PRESSURE_OFFSET); |
if(off > 20) off -= 10; |
OCR0A = off; |
ExpandBaro = 0; |
Delay_ms_Mess(100); |
if(ReadingAirPressure < 850) off = 0; |
for(; off < 250;off++) |
{ |
OCR0A = off; |
Delay_ms_Mess(50); |
printf("."); |
if(ReadingAirPressure < 850) break; |
} |
SetParamByte(PID_PRESSURE_OFFSET, off); |
PressureSensorOffset = off; |
Delay_ms_Mess(300); |
} |
void SearchDacGyroOffset(void) |
{ |
uint8_t i, ready = 0; |
uint16_t timeout ; |
GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
timeout = SetDelay(2000); |
if(BoardRelease == 13) // the auto offset calibration is available only at board release 1.3 |
{ |
for(i = 140; i != 0; i--) |
{ |
if(ready == 3 && i > 10) i = 9; |
ready = 0; |
if(AdValueGyroNick < 1020) DacOffsetGyroNick--; else if(AdValueGyroNick > 1030) DacOffsetGyroNick++; else ready++; |
if(AdValueGyroRoll < 1020) DacOffsetGyroRoll--; else if(AdValueGyroRoll > 1030) DacOffsetGyroRoll++; else ready++; |
if(AdValueGyroYaw < 1020) DacOffsetGyroYaw-- ; else if(AdValueGyroYaw > 1030) DacOffsetGyroYaw++ ; else ready++; |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission |
if(DacOffsetGyroNick < 10) { GyroDefectNick = 1; DacOffsetGyroNick = 10;}; if(DacOffsetGyroNick > 245) { GyroDefectNick = 1; DacOffsetGyroNick = 245;}; |
if(DacOffsetGyroRoll < 10) { GyroDefectRoll = 1; DacOffsetGyroRoll = 10;}; if(DacOffsetGyroRoll > 245) { GyroDefectRoll = 1; DacOffsetGyroRoll = 245;}; |
if(DacOffsetGyroYaw < 10) { GyroDefectYaw = 1; DacOffsetGyroYaw = 10;}; if(DacOffsetGyroYaw > 245) { GyroDefectYaw = 1; DacOffsetGyroYaw = 245;}; |
while(twi_state) |
{ |
if(CheckDelay(timeout)) |
{ |
printf("\r\n DAC or I2C Error1 check I2C, 3Vref, DAC, and BL-Ctrl"); |
break; |
} |
} // wait for end of data transmission |
average_pressure = 0; |
ADC_Enable(); |
while(average_pressure == 0); |
if(i < 10) Delay_ms_Mess(10); |
} |
Delay_ms_Mess(70); |
} |
} |
/*****************************************************/ |
/* Interrupt Service Routine for ADC */ |
/*****************************************************/ |
// runs at 312.5 kHz or 3.2 µs |
// if after (60.8µs) all 19 states are processed the interrupt is disabled |
// and the update of further ads is stopped |
/* |
0 nickgyro |
1 rollgyro |
2 yawgyro |
3 accroll |
4 accnick |
5 nickgyro |
6 rollgyro |
7 ubat |
8 acctop |
9 air pressure |
10 nickgyro |
11 rollgyro |
12 yawgyro |
13 accroll |
14 accnick |
15 gyronick |
16 gyroroll |
17 airpressure |
*/ |
#define AD_GYRO_YAW 0 |
#define AD_GYRO_ROLL 1 |
#define AD_GYRO_NICK 2 |
#define AD_AIRPRESS 3 |
#define AD_UBAT 4 |
#define AD_ACC_TOP 5 |
#define AD_ACC_ROLL 6 |
#define AD_ACC_NICK 7 |
ISR(ADC_vect) |
{ |
static uint8_t ad_channel = AD_GYRO_NICK, state = 0; |
static uint16_t gyroyaw, gyroroll, gyronick, accroll, accnick; |
static int32_t filtergyronick, filtergyroroll; |
static int16_t tmpAirPressure = 0; |
// state machine |
switch(state++) |
{ |
case 0: |
gyronick = ADC; // get nick gyro voltage 1st sample |
ad_channel = AD_GYRO_ROLL; |
break; |
case 1: |
gyroroll = ADC; // get roll gyro voltage 1st sample |
ad_channel = AD_GYRO_YAW; |
break; |
case 2: |
gyroyaw = ADC; // get yaw gyro voltage 1st sample |
ad_channel = AD_ACC_ROLL; |
break; |
case 3: |
accroll = ADC; // get roll acc voltage 1st sample |
ad_channel = AD_ACC_NICK; |
break; |
case 4: |
accnick = ADC; // get nick acc voltage 1st sample |
ad_channel = AD_GYRO_NICK; |
break; |
case 5: |
gyronick += ADC; // get nick gyro voltage 2nd sample |
ad_channel = AD_GYRO_ROLL; |
break; |
case 6: |
gyroroll += ADC; // get roll gyro voltage 2nd sample |
ad_channel = AD_UBAT; |
break; |
case 7: |
// get actual UBat (Volts*10) is ADC*30V/1024*10 = ADC/3 |
UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value |
ad_channel = AD_ACC_TOP; |
break; |
case 8: |
AdValueAccZ = ADC; // get plain acceleration in Z direction |
AdValueAccTop = (int16_t)ADC - AdBiasAccTop; // get acceleration in Z direction |
if(AdValueAccTop > 1) |
{ |
if(AdBiasAccTop < 750) |
{ |
AdBiasAccTop += 0.02; |
if(ModelIsFlying < 500) AdBiasAccTop += 0.1; |
} |
} |
else if(AdValueAccTop < -1) |
{ |
if(AdBiasAccTop > 550) |
{ |
AdBiasAccTop -= 0.02; |
if(ModelIsFlying < 500) AdBiasAccTop -= 0.1; |
} |
} |
ReadingIntegralTop += AdValueAccTop; // load |
ReadingIntegralTop -= ReadingIntegralTop / 1024; // discharge |
ad_channel = AD_AIRPRESS; |
break; |
// case 9 is moved to the end |
case 10: |
gyronick += ADC; // get nick gyro voltage 3rd sample |
ad_channel = AD_GYRO_ROLL; |
break; |
case 11: |
gyroroll += ADC; // get roll gyro voltage 3rd sample |
ad_channel = AD_GYRO_YAW; |
break; |
case 12: |
gyroyaw += ADC; // get yaw gyro voltage 2nd sample |
if(BoardRelease == 10) AdValueGyroYaw = (gyroyaw + 1) / 2; // analog gain on board 1.0 is 2 times higher |
else |
if(BoardRelease == 20) AdValueGyroYaw = 2047 - gyroyaw; // 2 times higher than a single sample |
else AdValueGyroYaw = gyroyaw; // 2 times higher than a single sample |
ad_channel = AD_ACC_ROLL; |
break; |
case 13: |
accroll += ADC; // get roll acc voltage 2nd sample |
AdValueAccRoll = AdBiasAccRoll - accroll; // subtract bias |
ad_channel = AD_ACC_NICK; |
break; |
case 14: |
accnick += ADC; // get nick acc voltage 2nd sample |
AdValueAccNick = accnick - AdBiasAccNick; // subtract bias |
ad_channel = AD_GYRO_NICK; |
break; |
case 15: |
gyronick += ADC; // get nick gyro voltage 4th sample |
if(BoardRelease == 10) gyronick *= 2; // 8 times higer than a single sample, HW gain x2 |
else gyronick *= 4; // 16 times higer than a single sample |
AdValueGyroNick = gyronick / 8; // 2 times higher than a single sample |
filtergyronick = (filtergyronick + gyronick) / 2; //(16 samples)/2 results in a factor of 8 higher than a single sample) see HIRES_GYRO_AMPLIFY |
HiResGyroNick = filtergyronick - BiasHiResGyroNick; |
FilterHiResGyroNick = (FilterHiResGyroNick + HiResGyroNick) / 2; |
ad_channel = AD_GYRO_ROLL; |
break; |
case 16: |
gyroroll += ADC; // get roll gyro voltage 4th sample |
if(BoardRelease == 10) gyroroll *= 2; // 8 times higer than a single sample, HW gain x2 |
else gyroroll *= 4; // 16 times higer than a single sample |
AdValueGyroRoll = gyroroll / 8; // 2 times higher than a single sample |
filtergyroroll = (filtergyroroll + gyroroll) / 2; //(16 samples)/2 results in a factor of 8 higher than a single sample) see HIRES_GYRO_AMPLIFY |
HiResGyroRoll = filtergyroroll - BiasHiResGyroRoll; |
FilterHiResGyroRoll = (FilterHiResGyroRoll + HiResGyroRoll) / 2; |
ad_channel = AD_AIRPRESS; |
break; |
case 17: |
state = 0; // restart sequence from beginning |
ADReady = 1; // mark |
MeasurementCounter++; // increment total measurement counter |
// "break;" is missing to enable fall thru case 9 at the end of the sequence |
case 9: |
tmpAirPressure += ADC; // sum adc values |
if(++average_pressure >= 5) // if 5 values are summerized for averaging |
{ |
tmpAirPressure /= 2; |
ReadingAirPressure = ADC; // update meassured air pressure |
HeightD = (31 * HeightD + (int16_t)FCParam.HeightD * (int16_t)(255 * ExpandBaro + StartAirPressure - tmpAirPressure - ReadingHeight)) / 32; // D-Part = CurrentValue - OldValue |
AirPressure = (tmpAirPressure + 7 * AirPressure + 4) / 8; // averaging using history |
ReadingHeight = 255 * ExpandBaro + StartAirPressure - AirPressure; |
average_pressure = 0; // reset air pressure measurement counter |
tmpAirPressure /= 2; |
} |
ad_channel = AD_GYRO_NICK; |
break; |
default: |
ad_channel = AD_GYRO_NICK; |
state = 0; |
break; |
} |
// set adc muxer to next ad_channel |
ADMUX = (ADMUX & 0xE0) | ad_channel; |
// after full cycle stop further interrupts |
if(state != 0) ADC_Enable(); |
} |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/analog.h |
---|
0,0 → 1,37 |
#ifndef _ANALOG_H |
#define _ANALOG_H |
#include <inttypes.h> |
extern volatile uint16_t MeasurementCounter; |
extern volatile int16_t UBat; |
extern volatile int16_t AdValueGyroNick, AdValueGyroRoll, AdValueGyroYaw; |
#define HIRES_GYRO_AMPLIFY 8 // the offset corrected HiResGyro values are a factor of 8 scaled to the AdValues |
extern volatile int16_t HiResGyroNick, HiResGyroRoll; |
extern volatile int16_t FilterHiResGyroNick, FilterHiResGyroRoll; |
extern volatile int16_t AdValueAccRoll, AdValueAccNick, AdValueAccTop, AdValueAccZ; |
extern volatile int32_t AirPressure; |
extern volatile int16_t HeightD; |
extern volatile uint16_t ReadingAirPressure; |
extern volatile int16_t StartAirPressure; |
extern volatile uint8_t ADReady; |
extern uint8_t DacOffsetGyroNick, DacOffsetGyroRoll, DacOffsetGyroYaw; |
extern uint8_t PressureSensorOffset; |
extern int8_t ExpandBaro; |
void SearchAirPressureOffset(void); |
void SearchDacGyroOffset(void); |
void ADC_Init(void); |
// clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
#define ADC_Disable() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE))) |
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
#define ADC_Enable() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE)) |
#endif //_ANALOG_H |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/eeprom.c |
---|
0,0 → 1,582 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + Nur für den privaten Gebrauch |
// + www.MikroKopter.com |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zulässig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Contant Values |
// + 0-250 -> normale Values |
// + 251 -> Poti1 |
// + 252 -> Poti2 |
// + 253 -> Poti3 |
// + 254 -> Poti4 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#ifndef EEMEM |
#define EEMEM __attribute__ ((section (".eeprom"))) |
#endif |
#include <avr/eeprom.h> |
#include <string.h> |
#include "eeprom.h" |
#include "printf_P.h" |
#include "led.h" |
#include "main.h" |
#include "fc.h" |
// byte array in eeprom |
uint8_t EEPromArray[E2END+1] EEMEM; |
paramset_t ParamSet; |
MixerTable_t Mixer; |
/***************************************************/ |
/* Default Values for parameter set 1 */ |
/***************************************************/ |
void ParamSet_DefaultSet1(void) // sport |
{ |
if(BoardRelease >= 20) |
{ |
ParamSet.GyroD = 5; |
ParamSet.DriftComp = 0; |
ParamSet.GyroAccFactor = 27; |
ParamSet.AngleTurnOverNick = 78; |
ParamSet.AngleTurnOverRoll = 78; |
} |
else |
{ |
ParamSet.GyroD = 3; |
ParamSet.DriftComp = 32; |
ParamSet.GyroAccFactor = 30; |
ParamSet.AngleTurnOverNick = 85; |
ParamSet.AngleTurnOverRoll = 85; |
} |
ParamSet.ChannelAssignment[CH_NICK] = 1; |
ParamSet.ChannelAssignment[CH_ROLL] = 2; |
ParamSet.ChannelAssignment[CH_GAS] = 3; |
ParamSet.ChannelAssignment[CH_YAW] = 4; |
ParamSet.ChannelAssignment[CH_POTI1] = 5; |
ParamSet.ChannelAssignment[CH_POTI2] = 6; |
ParamSet.ChannelAssignment[CH_POTI3] = 7; |
ParamSet.ChannelAssignment[CH_POTI4] = 8; |
ParamSet.GlobalConfig = CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX; |
ParamSet.HeightMinGas = 30; |
ParamSet.MaxHeight = 251; |
ParamSet.HeightP = 10; |
ParamSet.HeightD = 30; |
ParamSet.Height_ACC_Effect = 30; |
ParamSet.Height_Gain = 4; |
ParamSet.StickP = 15; |
ParamSet.StickD = 30; |
ParamSet.StickYawP = 12; |
ParamSet.GasMin = 8; |
ParamSet.GasMax = 230; |
ParamSet.CompassYawEffect = 128; |
ParamSet.GyroP = 80; |
ParamSet.GyroI = 150; |
ParamSet.LowVoltageWarning = 94; |
ParamSet.EmergencyGas = 35; |
ParamSet.EmergencyGasDuration = 30; |
ParamSet.UfoArrangement = 0; |
ParamSet.IFactor = 32; |
ParamSet.UserParam1 = 0; |
ParamSet.UserParam2 = 0; |
ParamSet.UserParam3 = 0; |
ParamSet.UserParam4 = 0; |
ParamSet.UserParam5 = 0; |
ParamSet.UserParam6 = 0; |
ParamSet.UserParam7 = 0; |
ParamSet.UserParam8 = 0; |
ParamSet.ServoNickControl = 100; |
ParamSet.ServoNickComp = 40; |
ParamSet.ServoNickCompInvert = 0; |
ParamSet.ServoNickMin = 50; |
ParamSet.ServoNickMax = 150; |
ParamSet.ServoRefresh = 5; |
ParamSet.LoopGasLimit = 50; |
ParamSet.LoopThreshold = 90; |
ParamSet.LoopHysteresis = 50; |
ParamSet.BitConfig = 0; |
ParamSet.AxisCoupling1 = 90; |
ParamSet.AxisCoupling2 = 67; |
ParamSet.AxisCouplingYawCorrection = 0; |
ParamSet.GyroAccTrim = 16; |
ParamSet.DynamicStability = 100; |
ParamSet.J16Bitmask = 95; |
ParamSet.J17Bitmask = 243; |
ParamSet.J16Timing = 15; |
ParamSet.J17Timing = 15; |
ParamSet.NaviGpsModeControl = 253; |
ParamSet.NaviGpsGain = 100; |
ParamSet.NaviGpsP = 90; |
ParamSet.NaviGpsI = 90; |
ParamSet.NaviGpsD = 90; |
ParamSet.NaviGpsPLimit = 25; |
ParamSet.NaviGpsILimit = 75; |
ParamSet.NaviGpsDLimit = 75; |
ParamSet.NaviGpsACC = 0; |
ParamSet.NaviGpsMinSat = 6; |
ParamSet.NaviStickThreshold = 8; |
ParamSet.NaviWindCorrection = 90; |
ParamSet.NaviSpeedCompensation = 30; |
ParamSet.NaviOperatingRadius = 100; |
ParamSet.NaviAngleLimitation = 45; |
ParamSet.NaviPHLoginTime = 4; |
memcpy(ParamSet.Name, "Sport\0",6); |
} |
/***************************************************/ |
/* Default Values for parameter set 2 */ |
/***************************************************/ |
void ParamSet_DefaultSet2(void) // normal |
{ |
if(BoardRelease >= 20) |
{ |
ParamSet.GyroD = 5; |
ParamSet.DriftComp = 0; |
ParamSet.GyroAccFactor = 27; |
ParamSet.AngleTurnOverNick = 78; |
ParamSet.AngleTurnOverRoll = 78; |
} |
else |
{ |
ParamSet.GyroD = 3; |
ParamSet.DriftComp = 32; |
ParamSet.GyroAccFactor = 30; |
ParamSet.AngleTurnOverNick = 85; |
ParamSet.AngleTurnOverRoll = 85; |
} |
ParamSet.ChannelAssignment[CH_NICK] = 1; |
ParamSet.ChannelAssignment[CH_ROLL] = 2; |
ParamSet.ChannelAssignment[CH_GAS] = 3; |
ParamSet.ChannelAssignment[CH_YAW] = 4; |
ParamSet.ChannelAssignment[CH_POTI1] = 5; |
ParamSet.ChannelAssignment[CH_POTI2] = 6; |
ParamSet.ChannelAssignment[CH_POTI3] = 7; |
ParamSet.ChannelAssignment[CH_POTI4] = 8; |
ParamSet.GlobalConfig = CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX; |
ParamSet.HeightMinGas = 30; |
ParamSet.MaxHeight = 251; |
ParamSet.HeightP = 10; |
ParamSet.HeightD = 30; |
ParamSet.Height_ACC_Effect = 30; |
ParamSet.Height_Gain = 3; |
ParamSet.StickP = 12; |
ParamSet.StickD = 16; |
ParamSet.StickYawP = 6; |
ParamSet.GasMin = 8; |
ParamSet.GasMax = 230; |
ParamSet.CompassYawEffect = 128; |
ParamSet.GyroP = 80; |
ParamSet.GyroI = 120; |
ParamSet.LowVoltageWarning = 94; |
ParamSet.EmergencyGas = 35; |
ParamSet.EmergencyGasDuration = 30; |
ParamSet.UfoArrangement = 0; |
ParamSet.IFactor = 32; |
ParamSet.UserParam1 = 0; |
ParamSet.UserParam2 = 0; |
ParamSet.UserParam3 = 0; |
ParamSet.UserParam4 = 0; |
ParamSet.UserParam5 = 0; |
ParamSet.UserParam6 = 0; |
ParamSet.UserParam7 = 0; |
ParamSet.UserParam8 = 0; |
ParamSet.ServoNickControl = 100; |
ParamSet.ServoNickComp = 40; |
ParamSet.ServoNickCompInvert = 0; |
ParamSet.ServoNickMin = 50; |
ParamSet.ServoNickMax = 150; |
ParamSet.ServoRefresh = 5; |
ParamSet.LoopGasLimit = 50; |
ParamSet.LoopThreshold = 90; |
ParamSet.LoopHysteresis = 50; |
ParamSet.BitConfig = 0; |
ParamSet.AxisCoupling1 = 90; |
ParamSet.AxisCoupling2 = 67; |
ParamSet.AxisCouplingYawCorrection = 60; |
ParamSet.GyroAccTrim = 32; |
ParamSet.DynamicStability = 75; |
ParamSet.J16Bitmask = 95; |
ParamSet.J17Bitmask = 243; |
ParamSet.J16Timing = 20; |
ParamSet.J17Timing = 20; |
ParamSet.NaviGpsModeControl = 253; |
ParamSet.NaviGpsGain = 100; |
ParamSet.NaviGpsP = 90; |
ParamSet.NaviGpsI = 90; |
ParamSet.NaviGpsD = 90; |
ParamSet.NaviGpsPLimit = 25; |
ParamSet.NaviGpsILimit = 75; |
ParamSet.NaviGpsDLimit = 75; |
ParamSet.NaviGpsACC = 0; |
ParamSet.NaviGpsMinSat = 6; |
ParamSet.NaviStickThreshold = 8; |
ParamSet.NaviWindCorrection = 90; |
ParamSet.NaviSpeedCompensation = 30; |
ParamSet.NaviOperatingRadius = 100; |
ParamSet.NaviAngleLimitation = 45; |
ParamSet.NaviPHLoginTime = 4; |
memcpy(ParamSet.Name, "Normal\0", 7); |
} |
/***************************************************/ |
/* Default Values for parameter set 3 */ |
/***************************************************/ |
void ParamSet_DefaultSet3(void) // beginner |
{ |
if(BoardRelease >= 20) |
{ |
ParamSet.GyroD = 5; |
ParamSet.DriftComp = 0; |
ParamSet.GyroAccFactor = 27; // Wert : 1-64 |
ParamSet.AngleTurnOverNick = 78; |
ParamSet.AngleTurnOverRoll = 78; |
} |
else |
{ |
ParamSet.GyroD = 3; |
ParamSet.DriftComp = 32; |
ParamSet.GyroAccFactor = 30; // Wert : 1-64 |
ParamSet.AngleTurnOverNick = 85; |
ParamSet.AngleTurnOverRoll = 85; |
} |
ParamSet.ChannelAssignment[CH_NICK] = 1; |
ParamSet.ChannelAssignment[CH_ROLL] = 2; |
ParamSet.ChannelAssignment[CH_GAS] = 3; |
ParamSet.ChannelAssignment[CH_YAW] = 4; |
ParamSet.ChannelAssignment[CH_POTI1] = 5; |
ParamSet.ChannelAssignment[CH_POTI2] = 6; |
ParamSet.ChannelAssignment[CH_POTI3] = 7; |
ParamSet.ChannelAssignment[CH_POTI4] = 8; |
ParamSet.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX; |
ParamSet.HeightMinGas = 30; |
ParamSet.MaxHeight = 251; |
ParamSet.HeightP = 10; |
ParamSet.HeightD = 30; |
ParamSet.Height_ACC_Effect = 30; |
ParamSet.Height_Gain = 3; |
ParamSet.StickP = 8; |
ParamSet.StickD = 16; |
ParamSet.StickYawP = 6; |
ParamSet.GasMin = 8; |
ParamSet.GasMax = 230; |
ParamSet.CompassYawEffect = 128; |
ParamSet.GyroP = 100; |
ParamSet.GyroI = 120; |
ParamSet.LowVoltageWarning = 94; |
ParamSet.EmergencyGas = 35; |
ParamSet.EmergencyGasDuration = 20; |
ParamSet.UfoArrangement = 0; |
ParamSet.IFactor = 16; |
ParamSet.UserParam1 = 0; |
ParamSet.UserParam2 = 0; |
ParamSet.UserParam3 = 0; |
ParamSet.UserParam4 = 0; |
ParamSet.UserParam5 = 0; |
ParamSet.UserParam6 = 0; |
ParamSet.UserParam7 = 0; |
ParamSet.UserParam8 = 0; |
ParamSet.ServoNickControl = 100; |
ParamSet.ServoNickComp = 40; |
ParamSet.ServoNickCompInvert = 0; |
ParamSet.ServoNickMin = 50; |
ParamSet.ServoNickMax = 150; |
ParamSet.ServoRefresh = 5; |
ParamSet.LoopGasLimit = 50; |
ParamSet.LoopThreshold = 90; |
ParamSet.LoopHysteresis = 50; |
ParamSet.BitConfig = 0; |
ParamSet.AxisCoupling1 = 90; |
ParamSet.AxisCoupling2 = 67; |
ParamSet.AxisCouplingYawCorrection = 70; |
ParamSet.GyroAccTrim = 32; |
ParamSet.DynamicStability = 50; |
ParamSet.J16Bitmask = 95; |
ParamSet.J17Bitmask = 243; |
ParamSet.J16Timing = 30; |
ParamSet.J17Timing = 30; |
ParamSet.NaviGpsModeControl = 253; |
ParamSet.NaviGpsGain = 100; |
ParamSet.NaviGpsP = 90; |
ParamSet.NaviGpsI = 90; |
ParamSet.NaviGpsD = 90; |
ParamSet.NaviGpsPLimit = 25; |
ParamSet.NaviGpsILimit = 75; |
ParamSet.NaviGpsDLimit = 75; |
ParamSet.NaviGpsACC = 0; |
ParamSet.NaviGpsMinSat = 6; |
ParamSet.NaviStickThreshold = 8; |
ParamSet.NaviWindCorrection = 90; |
ParamSet.NaviSpeedCompensation = 30; |
ParamSet.NaviOperatingRadius = 100; |
ParamSet.NaviAngleLimitation = 45; |
ParamSet.NaviPHLoginTime = 4; |
memcpy(ParamSet.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 *) &ParamSet.ChannelAssignment[0], &EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber - 1)], PARAMSET_STRUCT_LEN); |
LED_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 *) &ParamSet.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( &ParamSet.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); |
LED_Init(); |
} |
/***************************************************/ |
/* 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 |
for(i = 0; i < 16; i++) |
{ |
Mixer.Motor[i][MIX_GAS] = 0; |
Mixer.Motor[i][MIX_NICK] = 0; |
Mixer.Motor[i][MIX_ROLL] = 0; |
Mixer.Motor[i][MIX_YAW] = 0; |
} |
// default = Quadro |
Mixer.Motor[0][MIX_GAS] = 64; Mixer.Motor[0][MIX_NICK] = +64; Mixer.Motor[0][MIX_ROLL] = 0; Mixer.Motor[0][MIX_YAW] = +64; |
Mixer.Motor[1][MIX_GAS] = 64; Mixer.Motor[1][MIX_NICK] = -64; Mixer.Motor[1][MIX_ROLL] = 0; Mixer.Motor[1][MIX_YAW] = +64; |
Mixer.Motor[2][MIX_GAS] = 64; Mixer.Motor[2][MIX_NICK] = 0; Mixer.Motor[2][MIX_ROLL] = -64; Mixer.Motor[2][MIX_YAW] = -64; |
Mixer.Motor[3][MIX_GAS] = 64; Mixer.Motor[3][MIX_NICK] = 0; Mixer.Motor[3][MIX_ROLL] = +64; Mixer.Motor[3][MIX_YAW] = -64; |
memcpy(Mixer.Name, "Quadro\0", 7); |
} |
/***************************************************/ |
/* 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); |
} |
/***************************************************/ |
/* Initialize EEPROM Parameter Sets */ |
/***************************************************/ |
void ParamSet_Init(void) |
{ |
uint8_t Channel_Backup = 0, i; |
// parameter version check |
if(eeprom_read_byte(&EEPromArray[PID_PARAM_REVISION]) != EEPARAM_REVISION) |
{ |
// if version check faild |
printf("\n\rInit Parameter in EEPROM"); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE], 0xFF); // reset also mixer table |
// check if channel mapping backup is valid |
if( (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+0]) < 12) |
&& (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+1]) < 12) |
&& (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+2]) < 12) |
&& (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]) < 12) |
) |
{ |
Channel_Backup = 1; |
} |
// fill all 5 parameter settings |
for (i = 1;i < 6; i++) |
{ |
switch(i) |
{ |
case 1: |
ParamSet_DefaultSet1(); // Fill ParamSet 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 |
ParamSet.ChannelAssignment[0] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+0]); |
ParamSet.ChannelAssignment[1] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+1]); |
ParamSet.ChannelAssignment[2] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+2]); |
ParamSet.ChannelAssignment[3] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]); |
ParamSet.ChannelAssignment[4] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+4]); |
ParamSet.ChannelAssignment[5] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+5]); |
ParamSet.ChannelAssignment[6] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+6]); |
ParamSet.ChannelAssignment[7] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+7]); |
} |
ParamSet_WriteToEEProm(i); |
} |
// default-Setting is parameter set 3 |
SetActiveParamSet(3); |
// update version info |
SetParamByte(PID_PARAM_REVISION, EEPARAM_REVISION); |
} |
// read active parameter set to ParamSet 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_GAS] > 0) RequiredMotors++; |
} |
printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.Name, RequiredMotors); |
printf("\n\r=============================="); |
} |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/eeprom.h |
---|
0,0 → 1,176 |
#ifndef _EEPROM_H |
#define _EEPROM_H |
#include <inttypes.h> |
#define EEPROM_ADR_PARAM_BEGIN 0 |
#define PID_PARAM_REVISION 1 // byte |
#define PID_ACTIVE_SET 2 // byte |
#define PID_PRESSURE_OFFSET 3 // byte |
#define PID_ACC_NICK 4 // word |
#define PID_ACC_ROLL 6 // word |
#define PID_ACC_TOP 8 // word |
#ifdef USE_KILLAGREG |
#define PID_MM3_X_OFF 11 // byte |
#define PID_MM3_Y_OFF 12 // byte |
#define PID_MM3_Z_OFF 13 // byte |
#define PID_MM3_X_RANGE 14 // word |
#define PID_MM3_Y_RANGE 16 // word |
#define PID_MM3_Z_RANGE 18 // word |
#endif |
#define EEPROM_ADR_CHANNELS 80 // 8 bytes |
#define EEPROM_ADR_PARAMSET_LENGTH 98 // word |
#define EEPROM_ADR_PARAMSET_BEGIN 100 |
#define EEPROM_ADR_MIXER_TABLE 1000 // 1000 - 1076 |
#define MIX_GAS 0 |
#define MIX_NICK 1 |
#define MIX_ROLL 2 |
#define MIX_YAW 3 |
typedef struct |
{ |
uint8_t Revision; |
int8_t Name[12]; |
int8_t Motor[16][4]; |
} __attribute__((packed)) MixerTable_t; |
extern MixerTable_t Mixer; |
// bit mask for ParamSet.GlobalConfig |
#define CFG_HEIGHT_CONTROL 0x01 |
#define CFG_HEIGHT_SWITCH 0x02 |
#define CFG_HEADING_HOLD 0x04 |
#define CFG_COMPASS_ACTIVE 0x08 |
#define CFG_COMPASS_FIX 0x10 |
#define CFG_GPS_ACTIVE 0x20 |
#define CFG_AXIS_COUPLING_ACTIVE 0x40 |
#define CFG_ROTARY_RATE_LIMITER 0x80 |
// bit mask for ParamSet.BitConfig |
#define CFG_LOOP_UP 0x01 |
#define CFG_LOOP_DOWN 0x02 |
#define CFG_LOOP_LEFT 0x04 |
#define CFG_LOOP_RIGHT 0x08 |
#define CFG_HEIGHT_3SWITCH 0x10 |
// defines for lookup ParamSet.ChannelAssignment |
#define CH_NICK 0 |
#define CH_ROLL 1 |
#define CH_GAS 2 |
#define CH_YAW 3 |
#define CH_POTI1 4 |
#define CH_POTI2 5 |
#define CH_POTI3 6 |
#define CH_POTI4 7 |
#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) |
// 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; // Wert : 0-100 |
uint8_t HeightD; // Wert : 0-250 |
uint8_t MaxHeight; // Wert : 0-32 |
uint8_t HeightP; // Wert : 0-32 |
uint8_t Height_Gain; // Wert : 0-50 |
uint8_t Height_ACC_Effect; // Wert : 0-250 |
uint8_t StickP; // Wert : 1-6 |
uint8_t StickD; // Wert : 0-64 |
uint8_t StickYawP; // Wert : 1-20 |
uint8_t GasMin; // Wert : 0-32 |
uint8_t GasMax; // Wert : 33-250 |
uint8_t GyroAccFactor; // Wert : 1-64 |
uint8_t CompassYawEffect; // Wert : 0-32 |
uint8_t GyroP; // Wert : 10-250 |
uint8_t GyroI; // Wert : 0-250 |
uint8_t GyroD; // Wert : 0-250 |
uint8_t LowVoltageWarning; // Wert : 0-250 |
uint8_t EmergencyGas; // Wert : 0-250 //Gaswert bei Empängsverlust |
uint8_t EmergencyGasDuration; // Wert : 0-250 // Zeitbis auf EmergencyGas geschaltet wird, wg. Rx-Problemen |
uint8_t UfoArrangement; // x oder + Formation |
uint8_t IFactor; // Wert : 0-250 |
uint8_t UserParam1; // Wert : 0-250 |
uint8_t UserParam2; // Wert : 0-250 |
uint8_t UserParam3; // Wert : 0-250 |
uint8_t UserParam4; // Wert : 0-250 |
uint8_t ServoNickControl; // Wert : 0-250 // Stellung des Servos |
uint8_t ServoNickComp; // Wert : 0-250 // Einfluss Gyro/Servo |
uint8_t ServoNickMin; // Wert : 0-250 // Anschlag |
uint8_t ServoNickMax; // Wert : 0-250 // Anschlag |
uint8_t ServoRefresh; // Wert: 0-250 // Refreshrate of servo pwm output |
uint8_t LoopGasLimit; // Wert: 0-250 max. Gas während Looping |
uint8_t LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag |
uint8_t LoopHysteresis; // Wert: 0-250 Hysterese für Stickausschlag |
uint8_t AxisCoupling1; // Wert: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
uint8_t AxisCoupling2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
uint8_t AxisCouplingYawCorrection;// Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
uint8_t AngleTurnOverNick; // Wert: 0-250 180°-Punkt |
uint8_t AngleTurnOverRoll; // Wert: 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 UserParam5; // Wert : 0-250 |
uint8_t UserParam6; // Wert : 0-250 |
uint8_t UserParam7; // Wert : 0-250 |
uint8_t UserParam8; // Wert : 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 ServoNickCompInvert; // Wert : 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 ParamSet; |
extern void ParamSet_Init(void); |
extern void ParamSet_ReadFromEEProm(uint8_t setnumber); |
extern void ParamSet_WriteToEEProm(uint8_t setnumber); |
extern uint8_t GetActiveParamSet(void); |
extern void SetActiveParamSet(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); |
#endif //_EEPROM_H |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/fc.c |
---|
0,0 → 1,1665 |
/*####################################################################################### |
Flight Control |
#######################################################################################*/ |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + Nur für den privaten Gebrauch |
// + www.MikroKopter.com |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zulässig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <stdlib.h> |
#include <avr/io.h> |
#include "main.h" |
#include "eeprom.h" |
#include "timer0.h" |
#include "analog.h" |
#include "fc.h" |
#include "uart0.h" |
#include "rc.h" |
#include "twimaster.h" |
#include "timer2.h" |
#ifdef USE_KILLAGREG |
#include "mm3.h" |
#include "gps.h" |
#endif |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#include "gps.h" |
#endif |
#include "led.h" |
#ifdef USE_NAVICTRL |
#include "spi.h" |
#endif |
#define STICK_GAIN 4 |
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
// gyro readings |
int16_t GyroNick, GyroRoll, GyroYaw; |
// gyro bias |
int16_t BiasHiResGyroNick = 0, BiasHiResGyroRoll = 0, AdBiasGyroYaw = 0; |
// accelerations |
int16_t AccNick, AccRoll, AccTop; |
// neutral acceleration readings |
int16_t AdBiasAccNick = 0, AdBiasAccRoll = 0; |
volatile float AdBiasAccTop = 0; |
// the additive gyro rate corrections according to the axis coupling |
int16_t TrimNick, TrimRoll; |
// attitude gyro integrals |
int32_t IntegralGyroNick = 0,IntegralGyroNick2 = 0; |
int32_t IntegralGyroRoll = 0,IntegralGyroRoll2 = 0; |
int32_t IntegralGyroYaw = 0; |
int32_t ReadingIntegralGyroNick = 0, ReadingIntegralGyroNick2 = 0; |
int32_t ReadingIntegralGyroRoll = 0, ReadingIntegralGyroRoll2 = 0; |
int32_t ReadingIntegralGyroYaw = 0; |
int32_t MeanIntegralGyroNick; |
int32_t MeanIntegralGyroRoll; |
// attitude acceleration integrals |
int32_t MeanAccNick = 0, MeanAccRoll = 0; |
volatile int32_t ReadingIntegralTop = 0; |
// compass course |
int16_t CompassHeading = -1; // negative angle indicates invalid data. |
int16_t CompassCourse = -1; |
int16_t CompassOffCourse = 0; |
uint8_t CompassCalState = 0; |
uint8_t FunnelCourse = 0; |
uint16_t BadCompassHeading = 500; |
int32_t YawGyroHeading; // Yaw Gyro Integral supported by compass |
int16_t YawGyroDrift; |
int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0; |
// MK flags |
uint16_t ModelIsFlying = 0; |
uint8_t volatile MKFlags = 0; |
int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L; |
uint8_t GyroPFactor, GyroIFactor; // the PD factors for the attitude control |
uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control |
int16_t Ki = 10300 / 33; |
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
uint8_t RequiredMotors = 0; |
// stick values derived by rc channels readings |
int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0; |
int16_t GPSStickNick = 0, GPSStickRoll = 0; |
int16_t MaxStickNick = 0, MaxStickRoll = 0; |
// stick values derived by uart inputs |
int16_t ExternStickNick = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20; |
int16_t ReadingHeight = 0; |
int16_t SetPointHeight = 0; |
int16_t AttitudeCorrectionRoll = 0, AttitudeCorrectionNick = 0; |
uint8_t LoopingNick = 0, LoopingRoll = 0; |
uint8_t LoopingLeft = 0, LoopingRight = 0, LoopingDown = 0, LoopingTop = 0; |
fc_param_t FCParam = {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}; |
/************************************************************************/ |
/* Filter for motor value smoothing */ |
/************************************************************************/ |
int16_t MotorSmoothing(int16_t newvalue, int16_t oldvalue) |
{ |
int16_t motor; |
if(newvalue > oldvalue) motor = (1 * (int16_t)oldvalue + newvalue) / 2; //mean of old and new |
else motor = newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
return(motor); |
} |
/************************************************************************/ |
/* Creates numbeeps beeps at the speaker */ |
/************************************************************************/ |
void Beep(uint8_t numbeeps) |
{ |
while(numbeeps--) |
{ |
if(MKFlags & MKFLAG_MOTOR_RUN) return; //auf keinen Fall bei laufenden Motoren! |
BeepTime = 100; // 0.1 second |
Delay_ms(250); // blocks 250 ms as pause to next beep, |
// this will block the flight control loop, |
// therefore do not use this function if motors are running |
} |
} |
/************************************************************************/ |
/* Neutral Readings */ |
/************************************************************************/ |
void SetNeutral(uint8_t AccAdjustment) |
{ |
uint8_t i; |
int32_t Sum_1, Sum_2 = 0, Sum_3; |
Servo_Off(); // disable servo output |
AdBiasAccNick = 0; |
AdBiasAccRoll = 0; |
AdBiasAccTop = 0; |
BiasHiResGyroNick = 0; |
BiasHiResGyroRoll = 0; |
AdBiasGyroYaw = 0; |
FCParam.AxisCoupling1 = 0; |
FCParam.AxisCoupling2 = 0; |
ExpandBaro = 0; |
// sample values with bias set to zero |
Delay_ms_Mess(100); |
if(BoardRelease == 13) SearchDacGyroOffset(); |
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Height Control activated? |
{ |
if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset(); |
} |
// determine gyro bias by averaging (require no rotation movement) |
#define GYRO_BIAS_AVERAGE 32 |
Sum_1 = 0; |
Sum_2 = 0; |
Sum_3 = 0; |
for(i=0; i < GYRO_BIAS_AVERAGE; i++) |
{ |
Delay_ms_Mess(10); |
Sum_1 += AdValueGyroNick * HIRES_GYRO_AMPLIFY; |
Sum_2 += AdValueGyroRoll * HIRES_GYRO_AMPLIFY; |
Sum_3 += AdValueGyroYaw; |
} |
BiasHiResGyroNick = (int16_t)((Sum_1 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE); |
BiasHiResGyroRoll = (int16_t)((Sum_2 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE); |
AdBiasGyroYaw = (int16_t)((Sum_3 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE); |
if(AccAdjustment) |
{ |
// determine acc bias by averaging (require horizontal adjustment in nick and roll attitude) |
#define ACC_BIAS_AVERAGE 10 |
Sum_1 = 0; |
Sum_2 = 0; |
Sum_3 = 0; |
for(i=0; i < ACC_BIAS_AVERAGE; i++) |
{ |
Delay_ms_Mess(10); |
Sum_1 += AdValueAccNick; |
Sum_2 += AdValueAccRoll; |
Sum_3 += AdValueAccZ; |
} |
// use abs() to avoid negative bias settings because of adc sign flip in adc.c |
AdBiasAccNick = (int16_t)((abs(Sum_1) + ACC_BIAS_AVERAGE / 2) / ACC_BIAS_AVERAGE); |
AdBiasAccRoll = (int16_t)((abs(Sum_2) + ACC_BIAS_AVERAGE / 2) / ACC_BIAS_AVERAGE); |
AdBiasAccTop = (int16_t)((abs(Sum_3) + ACC_BIAS_AVERAGE / 2) / ACC_BIAS_AVERAGE); |
// Save ACC neutral settings to eeprom |
SetParamWord(PID_ACC_NICK, (uint16_t)AdBiasAccNick); |
SetParamWord(PID_ACC_ROLL, (uint16_t)AdBiasAccRoll); |
SetParamWord(PID_ACC_TOP, (uint16_t)AdBiasAccTop); |
} |
else // restore from eeprom |
{ |
AdBiasAccNick = (int16_t)GetParamWord(PID_ACC_NICK); |
AdBiasAccRoll = (int16_t)GetParamWord(PID_ACC_ROLL); |
AdBiasAccTop = (int16_t)GetParamWord(PID_ACC_TOP); |
} |
// setting acc bias values has an influence in the analog.c ISR |
// therefore run measurement for 100ms to achive stable readings |
Delay_ms_Mess(100); |
// reset acc averaging and integrals |
AccNick = ACC_AMPLIFY * (int32_t)AdValueAccNick; |
AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll; |
AccTop = AdValueAccTop; |
ReadingIntegralTop = AdValueAccTop; |
// and gyro readings |
GyroNick = 0; |
GyroRoll = 0; |
GyroYaw = 0; |
// reset gyro integrals to acc guessing |
IntegralGyroNick = ParamSet.GyroAccFactor * (int32_t)AccNick; |
IntegralGyroRoll = ParamSet.GyroAccFactor * (int32_t)AccRoll; |
//ReadingIntegralGyroNick = IntegralGyroNick; |
//ReadingIntegralGyroRoll = IntegralGyroRoll; |
ReadingIntegralGyroNick2 = IntegralGyroNick; |
ReadingIntegralGyroRoll2 = IntegralGyroRoll; |
ReadingIntegralGyroYaw = 0; |
StartAirPressure = AirPressure; |
HeightD = 0; |
// update compass course to current heading |
CompassCourse = CompassHeading; |
// Inititialize YawGyroIntegral value with current compass heading |
YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR; |
YawGyroDrift = 0; |
BeepTime = 50; |
TurnOver180Nick = ((int32_t) ParamSet.AngleTurnOverNick * 2500L) +15000L; |
TurnOver180Roll = ((int32_t) ParamSet.AngleTurnOverRoll * 2500L) +15000L; |
ExternHeightValue = 0; |
GPSStickNick = 0; |
GPSStickRoll = 0; |
MKFlags |= MKFLAG_CALIBRATE; |
FCParam.KalmanK = -1; |
FCParam.KalmanMaxDrift = 0; |
FCParam.KalmanMaxFusion = 32; |
Poti1 = PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110; |
Poti2 = PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110; |
Poti3 = PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110; |
Poti4 = PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110; |
Servo_On(); //enable servo output |
RC_Quality = 100; |
} |
/************************************************************************/ |
/* Averaging Measurement Readings */ |
/************************************************************************/ |
void Mean(void) |
{ |
int32_t tmpl = 0, tmpl2 = 0, tmp13 = 0, tmp14 = 0; |
int16_t FilterGyroNick, FilterGyroRoll; |
static int16_t Last_GyroRoll = 0, Last_GyroNick = 0; |
int16_t d2Nick, d2Roll; |
int32_t AngleNick, AngleRoll; |
int16_t CouplingNickRoll = 0, CouplingRollNick = 0; |
// Get bias free gyro readings |
GyroNick = HiResGyroNick / HIRES_GYRO_AMPLIFY; // unfiltered gyro rate |
FilterGyroNick = FilterHiResGyroNick / HIRES_GYRO_AMPLIFY; // use filtered gyro rate |
// handle rotation rates that violate adc ranges |
if(AdValueGyroNick < 15) GyroNick = -1000; |
if(AdValueGyroNick < 7) GyroNick = -2000; |
if(BoardRelease == 10) |
{ |
if(AdValueGyroNick > 1010) GyroNick = +1000; |
if(AdValueGyroNick > 1017) GyroNick = +2000; |
} |
else |
{ |
if(AdValueGyroNick > 2000) GyroNick = +1000; |
if(AdValueGyroNick > 2015) GyroNick = +2000; |
} |
GyroRoll = HiResGyroRoll / HIRES_GYRO_AMPLIFY; // unfiltered gyro rate |
FilterGyroRoll = FilterHiResGyroRoll / HIRES_GYRO_AMPLIFY; // use filtered gyro rate |
// handle rotation rates that violate adc ranges |
if(AdValueGyroRoll < 15) GyroRoll = -1000; |
if(AdValueGyroRoll < 7) GyroRoll = -2000; |
if(BoardRelease == 10) |
{ |
if(AdValueGyroRoll > 1010) GyroRoll = +1000; |
if(AdValueGyroRoll > 1017) GyroRoll = +2000; |
} |
else |
{ |
if(AdValueGyroRoll > 2000) GyroRoll = +1000; |
if(AdValueGyroRoll > 2015) GyroRoll = +2000; |
} |
GyroYaw = AdBiasGyroYaw - AdValueGyroYaw; |
// Acceleration Sensor |
// lowpass acc measurement and scale AccNick/AccRoll by a factor of ACC_AMPLIFY to have a better resolution |
AccNick = ((int32_t)AccNick * 3 + ((ACC_AMPLIFY * (int32_t)AdValueAccNick))) / 4L; |
AccRoll = ((int32_t)AccRoll * 3 + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 4L; |
AccTop = ((int32_t)AccTop * 3 + ((int32_t)AdValueAccTop)) / 4L; |
// sum acc sensor readings for later averaging |
MeanAccNick += ACC_AMPLIFY * AdValueAccNick; |
MeanAccRoll += ACC_AMPLIFY * AdValueAccRoll; |
NaviAccNick += AdValueAccNick; |
NaviAccRoll += AdValueAccRoll; |
NaviCntAcc++; |
// enable ADC to meassure next readings, before that point all variables should be read that are written by the ADC ISR |
ADC_Enable(); |
ADReady = 0; |
// limit angle readings for axis coupling calculations |
#define ANGLE_LIMIT 93000L // aprox. 93000/GYRO_DEG_FACTOR = 82 deg |
AngleNick = ReadingIntegralGyroNick; |
CHECK_MIN_MAX(AngleNick, -ANGLE_LIMIT, ANGLE_LIMIT); |
AngleRoll = ReadingIntegralGyroRoll; |
CHECK_MIN_MAX(AngleRoll, -ANGLE_LIMIT, ANGLE_LIMIT); |
// Yaw |
// calculate yaw gyro integral (~ to rotation angle) |
YawGyroHeading += GyroYaw; |
ReadingIntegralGyroYaw += GyroYaw; |
// Coupling fraction |
if(! LoopingNick && !LoopingRoll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) |
{ |
tmp13 = (FilterGyroRoll * AngleNick) / 2048L; |
tmp13 *= FCParam.AxisCoupling2; // 65 |
tmp13 /= 4096L; |
CouplingNickRoll = tmp13; |
tmp14 = (FilterGyroNick * AngleRoll) / 2048L; |
tmp14 *= FCParam.AxisCoupling2; // 65 |
tmp14 /= 4096L; |
CouplingRollNick = tmp14; |
tmp14 -= tmp13; |
YawGyroHeading += tmp14; |
if(!FCParam.AxisCouplingYawCorrection) ReadingIntegralGyroYaw -= tmp14 / 2; // force yaw |
tmpl = ((GyroYaw + tmp14) * AngleNick) / 2048L; |
tmpl *= FCParam.AxisCoupling1; |
tmpl /= 4096L; |
tmpl2 = ((GyroYaw + tmp14) * AngleRoll) / 2048L; |
tmpl2 *= FCParam.AxisCoupling1; |
tmpl2 /= 4096L; |
if(abs(GyroYaw > 64)) |
{ |
if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1; |
} |
TrimNick = -tmpl2 + tmpl / 100L; |
TrimRoll = tmpl - tmpl2 / 100L; |
} |
else |
{ |
CouplingNickRoll = 0; |
CouplingRollNick = 0; |
TrimNick = 0; |
TrimRoll = 0; |
} |
// 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; |
// Roll |
ReadingIntegralGyroRoll2 += FilterGyroRoll + TrimRoll; |
ReadingIntegralGyroRoll += FilterGyroRoll + TrimRoll- AttitudeCorrectionRoll; |
if(ReadingIntegralGyroRoll > TurnOver180Roll) |
{ |
ReadingIntegralGyroRoll = -(TurnOver180Roll - 10000L); |
ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll; |
} |
if(ReadingIntegralGyroRoll < -TurnOver180Roll) |
{ |
ReadingIntegralGyroRoll = (TurnOver180Roll - 10000L); |
ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll; |
} |
// Nick |
ReadingIntegralGyroNick2 += FilterGyroNick + TrimNick; |
ReadingIntegralGyroNick += FilterGyroNick + TrimNick - AttitudeCorrectionNick; |
if(ReadingIntegralGyroNick > TurnOver180Nick) |
{ |
ReadingIntegralGyroNick = -(TurnOver180Nick - 25000L); |
ReadingIntegralGyroNick2 = ReadingIntegralGyroNick; |
} |
if(ReadingIntegralGyroNick < -TurnOver180Nick) |
{ |
ReadingIntegralGyroNick = (TurnOver180Nick - 25000L); |
ReadingIntegralGyroNick2 = ReadingIntegralGyroNick; |
} |
IntegralGyroYaw = ReadingIntegralGyroYaw; |
IntegralGyroNick = ReadingIntegralGyroNick; |
IntegralGyroRoll = ReadingIntegralGyroRoll; |
IntegralGyroNick2 = ReadingIntegralGyroNick2; |
IntegralGyroRoll2 = ReadingIntegralGyroRoll2; |
#define D_LIMIT 128 |
if(FCParam.GyroD) |
{ |
d2Nick = (HiResGyroNick - Last_GyroNick); // change of gyro rate |
Last_GyroNick = (Last_GyroNick + HiResGyroNick) / 2; |
CHECK_MIN_MAX(d2Nick, -D_LIMIT, D_LIMIT); |
GyroNick += (d2Nick * (int16_t)FCParam.GyroD) / 16; |
d2Roll = (HiResGyroRoll - Last_GyroRoll); // change of gyro rate |
Last_GyroRoll = (Last_GyroRoll + HiResGyroRoll) / 2; |
CHECK_MIN_MAX(d2Roll, -D_LIMIT, D_LIMIT); |
GyroRoll += (d2Roll * (int16_t)FCParam.GyroD) / 16; |
HiResGyroNick += (d2Nick * (int16_t)FCParam.GyroD); |
HiResGyroRoll += (d2Roll * (int16_t)FCParam.GyroD); |
} |
// Increase the roll/nick rate virtually proportional to the coupling to suppress a faster rotation |
if(FilterGyroNick > 0) TrimNick += ((int32_t)abs(CouplingRollNick) * FCParam.AxisCouplingYawCorrection) / 64L; |
else TrimNick -= ((int32_t)abs(CouplingRollNick) * FCParam.AxisCouplingYawCorrection) / 64L; |
if(FilterGyroRoll > 0) TrimRoll += ((int32_t)abs(CouplingNickRoll) * FCParam.AxisCouplingYawCorrection) / 64L; |
else TrimRoll -= ((int32_t)abs(CouplingNickRoll) * FCParam.AxisCouplingYawCorrection) / 64L; |
// increase the nick/roll rates virtually from the threshold of 245 to slow down higher rotation rates |
if((ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER) && ! LoopingNick && !LoopingRoll) |
{ |
if(FilterGyroNick > 256) GyroNick += 1 * (FilterGyroNick - 256); |
else if(FilterGyroNick < -256) GyroNick += 1 * (FilterGyroNick + 256); |
if(FilterGyroRoll > 256) GyroRoll += 1 * (FilterGyroRoll - 256); |
else if(FilterGyroRoll < -256) GyroRoll += 1 * (FilterGyroRoll + 256); |
} |
} |
/************************************************************************/ |
/* Transmit Motor Data via I2C */ |
/************************************************************************/ |
void SendMotorData(void) |
{ |
uint8_t i; |
if(!(MKFlags & MKFLAG_MOTOR_RUN)) |
{ |
MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
if(!MotorTest_Active) Motor[i].SetPoint = 0; |
else Motor[i].SetPoint = MotorTest[i]; |
} |
if(MotorTest_Active) MotorTest_Active--; |
} |
DebugOut.Analog[12] = Motor[0].SetPoint; // Front |
DebugOut.Analog[13] = Motor[1].SetPoint; // Rear |
DebugOut.Analog[14] = Motor[3].SetPoint; // Left |
DebugOut.Analog[15] = Motor[2].SetPoint; // Right |
//Start I2C Interrupt Mode |
I2C_Start(TWI_STATE_MOTOR_TX); |
} |
/************************************************************************/ |
/* Map the parameter to poti values */ |
/************************************************************************/ |
void ParameterMapping(void) |
{ |
if(RC_Quality > 160) // do the mapping of RC-Potis only if the rc-signal is ok |
// else the last updated values are used |
{ |
//update poti values by rc-signals |
#define CHK_POTI_MM(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;} |
#define CHK_POTI(b,a) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a;} |
CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight); |
CHK_POTI_MM(FCParam.HeightD,ParamSet.HeightD,0,100); |
CHK_POTI_MM(FCParam.HeightP,ParamSet.HeightP,0,100); |
CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect); |
CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect); |
CHK_POTI_MM(FCParam.GyroP,ParamSet.GyroP,10,255); |
CHK_POTI(FCParam.GyroI,ParamSet.GyroI); |
CHK_POTI(FCParam.GyroD,ParamSet.GyroD); |
CHK_POTI(FCParam.IFactor,ParamSet.IFactor); |
CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1); |
CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2); |
CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3); |
CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4); |
CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5); |
CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6); |
CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7); |
CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8); |
CHK_POTI(FCParam.ServoNickControl,ParamSet.ServoNickControl); |
CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit); |
CHK_POTI(FCParam.AxisCoupling1,ParamSet.AxisCoupling1); |
CHK_POTI(FCParam.AxisCoupling2,ParamSet.AxisCoupling2); |
CHK_POTI(FCParam.AxisCouplingYawCorrection,ParamSet.AxisCouplingYawCorrection); |
CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability); |
CHK_POTI_MM(FCParam.J16Timing,ParamSet.J16Timing,1,255); |
CHK_POTI_MM(FCParam.J17Timing,ParamSet.J17Timing,1,255); |
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
CHK_POTI(FCParam.NaviGpsModeControl,ParamSet.NaviGpsModeControl); |
CHK_POTI(FCParam.NaviGpsGain,ParamSet.NaviGpsGain); |
CHK_POTI(FCParam.NaviGpsP,ParamSet.NaviGpsP); |
CHK_POTI(FCParam.NaviGpsI,ParamSet.NaviGpsI); |
CHK_POTI(FCParam.NaviGpsD,ParamSet.NaviGpsD); |
CHK_POTI(FCParam.NaviGpsACC,ParamSet.NaviGpsACC); |
CHK_POTI_MM(FCParam.NaviOperatingRadius,ParamSet.NaviOperatingRadius,10, 255); |
CHK_POTI(FCParam.NaviWindCorrection,ParamSet.NaviWindCorrection); |
CHK_POTI(FCParam.NaviSpeedCompensation,ParamSet.NaviSpeedCompensation); |
#endif |
CHK_POTI(FCParam.ExternalControl,ParamSet.ExternalControl); |
Ki = 10300 / ( FCParam.IFactor + 1 ); |
} |
} |
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; |
// PID controller variables |
int16_t DiffNick, DiffRoll; |
int16_t PDPartNick, PDPartRoll, PDPartYaw, PPartNick, PPartRoll; |
static int32_t IPartNick = 0, IPartRoll = 0; |
static int32_t SetPointYaw = 0; |
static int32_t IntegralGyroNickError = 0, IntegralGyroRollError = 0; |
static int32_t CorrectionNick, CorrectionRoll; |
static uint16_t RcLostTimer; |
static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0; |
static uint8_t HeightControlActive = 0; |
static int16_t HeightControlGas = 0; |
static int8_t TimerDebugOut = 0; |
static uint16_t UpdateCompassCourse = 0; |
// high resolution motor values for smoothing of PID motor outputs |
static int16_t MotorValue[MAX_MOTORS]; |
uint8_t i; |
Mean(); |
GRN_ON; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// determine gas value |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMixFraction = StickGas; |
if(GasMixFraction < ParamSet.GasMin + 10) GasMixFraction = ParamSet.GasMin + 10; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// RC-signal is bad |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(RC_Quality < 120) // the rc-frame signal is not reveived or noisy |
{ |
if(!PcAccess) // if also no PC-Access via UART |
{ |
if(BeepModulation == 0xFFFF) |
{ |
BeepTime = 15000; // 1.5 seconds |
BeepModulation = 0x0C00; |
} |
} |
if(RcLostTimer) RcLostTimer--; // decremtent timer after rc sigal lost |
else // rc lost countdown finished |
{ |
MKFlags &= ~(MKFLAG_MOTOR_RUN|MKFLAG_EMERGENCY_LANDING); // clear motor run flag that stop the motors in SendMotorData() |
} |
RED_ON; // set red led |
if(ModelIsFlying > 1000) // wahrscheinlich in der Luft --> langsam absenken |
{ |
GasMixFraction = ParamSet.EmergencyGas; // set emergency gas |
MKFlags |= (MKFLAG_EMERGENCY_LANDING); // ser flag fpr emergency landing |
// set neutral rc inputs |
PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] = 0; |
PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
PPM_diff[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
PPM_in[ParamSet.ChannelAssignment[CH_NICK]] = 0; |
PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
} |
else MKFlags &= ~(MKFLAG_MOTOR_RUN); // clear motor run flag that stop the motors in SendMotorData() |
} // eof RC_Quality < 120 |
else |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// RC-signal is good |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(RC_Quality > 140) |
{ |
MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing |
// reset emergency timer |
RcLostTimer = ParamSet.EmergencyGasDuration * 50; |
if(GasMixFraction > 40 && (MKFlags & MKFLAG_MOTOR_RUN) ) |
{ |
if(ModelIsFlying < 0xFFFF) ModelIsFlying++; |
} |
if(ModelIsFlying < 256) |
{ |
IPartNick = 0; |
IPartRoll = 0; |
StickYaw = 0; |
if(ModelIsFlying == 250) |
{ |
UpdateCompassCourse = 1; |
ReadingIntegralGyroYaw = 0; |
SetPointYaw = 0; |
} |
} |
else MKFlags |= (MKFLAG_FLY); // set fly flag |
if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--; |
if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--; |
if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--; |
if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--; |
//PPM24-Extension |
if(Poti5 < PPM_in[9] + 110) Poti5++; else if(Poti5 > PPM_in[9] + 110 && Poti5) Poti5--; |
if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--; |
if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--; |
if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--; |
//limit poti values |
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
//PPM24-Extension |
if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255; |
if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255; |
if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255; |
if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255; |
// if motors are off and the gas stick is in the upper position |
if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && !(MKFlags & MKFLAG_MOTOR_RUN) ) |
{ |
// and if the yaw stick is in the leftmost position |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// calibrate the neutral readings of all attitude sensors |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
{ |
// gas/yaw joystick is top left |
// _________ |
// |x | |
// | | |
// | | |
// | | |
// | | |
// ¯¯¯¯¯¯¯¯¯ |
if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_neutral = 0; |
GRN_OFF; |
ModelIsFlying = 0; |
// check roll/nick stick position |
// if nick stick is top or roll stick is left or right --> change parameter setting |
// according to roll/nick stick position |
if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70) |
{ |
uint8_t setting = 1; // default |
// nick/roll joystick |
// _________ |
// |2 3 4| |
// | | |
// |1 5| |
// | | |
// | | |
// ¯¯¯¯¯¯¯¯¯ |
// roll stick leftmost and nick stick centered --> setting 1 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 1; |
// roll stick leftmost and nick stick topmost --> setting 2 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 2; |
// roll stick centered an nick stick topmost --> setting 3 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 3; |
// roll stick rightmost and nick stick topmost --> setting 4 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > 70) setting = 4; |
// roll stick rightmost and nick stick centered --> setting 5 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < 70) setting = 5; |
// update active parameter set in eeprom |
SetActiveParamSet(setting); |
ParamSet_ReadFromEEProm(GetActiveParamSet()); |
SetNeutral(NO_ACC_CALIB); |
Beep(GetActiveParamSet()); |
} |
else |
{ |
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) |
{ |
// nick/roll joystick |
// _________ |
// | | |
// | | |
// | | |
// | | |
// | x | |
// ¯¯¯¯¯¯¯¯¯ |
// enable calibration state of compass |
CompassCalState = 1; |
BeepTime = 1000; |
} |
else // nick and roll are centered |
{ |
ParamSet_ReadFromEEProm(GetActiveParamSet()); |
SetNeutral(NO_ACC_CALIB); |
Beep(GetActiveParamSet()); |
} |
} |
else // nick and roll are centered |
{ |
ParamSet_ReadFromEEProm(GetActiveParamSet()); |
SetNeutral(NO_ACC_CALIB); |
Beep(GetActiveParamSet()); |
} |
} |
} |
} |
// and if the yaw stick is in the rightmost position |
// save the ACC neutral setting to eeprom |
else if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
{ |
// gas/yaw joystick is top right |
// _________ |
// | x| |
// | | |
// | | |
// | | |
// | | |
// ¯¯¯¯¯¯¯¯¯ |
if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_neutral = 0; |
GRN_OFF; |
ModelIsFlying = 0; |
SetNeutral(ACC_CALIB); |
Beep(GetActiveParamSet()); |
} |
} |
else delay_neutral = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// gas stick is down |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < -85) |
{ |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
{ |
// gas/yaw joystick is bottom right |
// _________ |
// | | |
// | | |
// | | |
// | | |
// | x| |
// ¯¯¯¯¯¯¯¯¯ |
// Start Motors |
if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_startmotors = 200; // do not repeat if once executed |
ModelIsFlying = 1; |
MKFlags |= (MKFLAG_MOTOR_RUN|MKFLAG_START); // set flag RUN and START |
SetPointYaw = 0; |
ReadingIntegralGyroYaw = 0; |
ReadingIntegralGyroNick = ParamSet.GyroAccFactor * (int32_t)AccNick; |
ReadingIntegralGyroRoll = ParamSet.GyroAccFactor * (int32_t)AccRoll; |
ReadingIntegralGyroNick2 = IntegralGyroNick; |
ReadingIntegralGyroRoll2 = IntegralGyroRoll; |
IPartNick = 0; |
IPartRoll = 0; |
} |
} |
else delay_startmotors = 0; // reset delay timer if sticks are not in this position |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
{ |
// gas/yaw joystick is bottom left |
// _________ |
// | | |
// | | |
// | | |
// | | |
// |x | |
// ¯¯¯¯¯¯¯¯¯ |
// Stop Motors |
if(++delay_stopmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_stopmotors = 200; // do not repeat if once executed |
ModelIsFlying = 0; |
MKFlags &= ~(MKFLAG_MOTOR_RUN); |
} |
} |
else delay_stopmotors = 0; // reset delay timer if sticks are not in this position |
} |
// remapping of paameters only if the signal rc-sigbnal conditions are good |
} // eof RC_Quality > 150 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// new values from RC |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!NewPpmData-- || (MKFlags & MKFLAG_EMERGENCY_LANDING) ) // NewData = 0 means new data from RC |
{ |
static int16_t stick_nick = 0, stick_roll = 0; |
ParameterMapping(); // remapping params (online poti replacement) |
// calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
stick_nick = (stick_nick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.StickP) / 4; |
stick_nick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.StickD; |
StickNick = stick_nick - GPSStickNick; |
stick_roll = (stick_roll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.StickP) / 4; |
stick_roll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.StickD; |
StickRoll = stick_roll - GPSStickRoll; |
// 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; |
} |
// mapping of gas |
StickGas = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers |
// update gyro control loop factors |
GyroPFactor = FCParam.GyroP + 10; |
GyroIFactor = FCParam.GyroI; |
GyroYawPFactor = FCParam.GyroP + 10; |
GyroYawIFactor = FCParam.GyroI; |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Analog control via serial communication |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128) |
{ |
StickNick += (int16_t) ExternControl.Nick * (int16_t) ParamSet.StickP; |
StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.StickP; |
StickYaw += ExternControl.Yaw; |
ExternHeightValue = (int16_t) ExternControl.Height * (int16_t)ParamSet.Height_Gain; |
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
} |
if(StickGas < 0) StickGas = 0; |
// disable I part of gyro control feedback |
if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) GyroIFactor = 0; |
// update max stick positions for nick and roll |
if(abs(StickNick / STICK_GAIN) > MaxStickNick) |
{ |
MaxStickNick = abs(StickNick)/STICK_GAIN; |
if(MaxStickNick > 100) MaxStickNick = 100; |
} |
else MaxStickNick--; |
if(abs(StickRoll / STICK_GAIN) > MaxStickRoll) |
{ |
MaxStickRoll = abs(StickRoll)/STICK_GAIN; |
if(MaxStickRoll > 100) MaxStickRoll = 100; |
} |
else MaxStickRoll--; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Looping? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_LEFT) LoopingLeft = 1; |
else |
{ |
if(LoopingLeft) // Hysteresis |
{ |
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) LoopingLeft = 0; |
} |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_RIGHT) LoopingRight = 1; |
else |
{ |
if(LoopingRight) // Hysteresis |
{ |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) LoopingRight = 0; |
} |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_UP) LoopingTop = 1; |
else |
{ |
if(LoopingTop) // Hysteresis |
{ |
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) LoopingTop = 0; |
} |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_DOWN) LoopingDown = 1; |
else |
{ |
if(LoopingDown) // Hysteresis |
{ |
if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) LoopingDown = 0; |
} |
} |
if(LoopingLeft || LoopingRight) LoopingRoll = 1; else LoopingRoll = 0; |
if(LoopingTop || LoopingDown) { LoopingNick = 1; LoopingRoll = 0; LoopingLeft = 0; LoopingRight = 0;} else LoopingNick = 0; |
} // End of new RC-Values or Emergency Landing |
if(LoopingRoll || LoopingNick) |
{ |
if(GasMixFraction > ParamSet.LoopGasLimit) GasMixFraction = ParamSet.LoopGasLimit; |
FunnelCourse = 1; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// in case of emergency landing |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// set all inputs to save values |
if(MKFlags & MKFLAG_EMERGENCY_LANDING) |
{ |
StickYaw = 0; |
StickNick = 0; |
StickRoll = 0; |
GyroPFactor = 90; |
GyroIFactor = 120; |
GyroYawPFactor = 90; |
GyroYawIFactor = 120; |
LoopingRoll = 0; |
LoopingNick = 0; |
MaxStickNick = 0; |
MaxStickRoll = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Trim Gyro-Integrals to ACC-Signals |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define BALANCE_NUMBER 256L |
// sum for averaging |
MeanIntegralGyroNick += IntegralGyroNick; |
MeanIntegralGyroRoll += IntegralGyroRoll; |
if( LoopingNick || LoopingRoll) // if looping in any direction |
{ |
// reset averaging for acc and gyro integral as well as gyro integral acc correction |
MeasurementCounter = 0; |
MeanAccNick = 0; |
MeanAccRoll = 0; |
MeanIntegralGyroNick = 0; |
MeanIntegralGyroRoll = 0; |
ReadingIntegralGyroNick2 = ReadingIntegralGyroNick; |
ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll; |
AttitudeCorrectionNick = 0; |
AttitudeCorrectionRoll = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(! LoopingNick && !LoopingRoll && ( (AdValueAccZ > 512) || (MKFlags & MKFLAG_MOTOR_RUN) ) ) // if not lopping in any direction |
{ |
int32_t tmp_long, tmp_long2; |
if( FCParam.KalmanK != -1) |
{ |
// determine the deviation of gyro integral from averaged acceleration sensor |
tmp_long = (int32_t)(IntegralGyroNick / ParamSet.GyroAccFactor - (int32_t)AccNick); |
tmp_long = (tmp_long * FCParam.KalmanK) / (32 * 16); |
tmp_long2 = (int32_t)(IntegralGyroRoll / ParamSet.GyroAccFactor - (int32_t)AccRoll); |
tmp_long2 = (tmp_long2 * FCParam.KalmanK) / (32 * 16); |
if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands |
{ |
tmp_long /= 2; |
tmp_long2 /= 2; |
} |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
// limit correction effect |
if(tmp_long > (int32_t)FCParam.KalmanMaxFusion) tmp_long = (int32_t)FCParam.KalmanMaxFusion; |
if(tmp_long < -(int32_t)FCParam.KalmanMaxFusion) tmp_long =-(int32_t)FCParam.KalmanMaxFusion; |
if(tmp_long2 > (int32_t)FCParam.KalmanMaxFusion) tmp_long2 = (int32_t)FCParam.KalmanMaxFusion; |
if(tmp_long2 <-(int32_t)FCParam.KalmanMaxFusion) tmp_long2 =-(int32_t)FCParam.KalmanMaxFusion; |
} |
else |
{ |
// determine the deviation of gyro integral from acceleration sensor |
tmp_long = (int32_t)(IntegralGyroNick / ParamSet.GyroAccFactor - (int32_t)AccNick); |
tmp_long /= 16; |
tmp_long2 = (int32_t)(IntegralGyroRoll / ParamSet.GyroAccFactor - (int32_t)AccRoll); |
tmp_long2 /= 16; |
if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
#define BALANCE 32 |
// limit correction effect |
CHECK_MIN_MAX(tmp_long, -BALANCE, BALANCE); |
CHECK_MIN_MAX(tmp_long2, -BALANCE, BALANCE); |
} |
// correct current readings |
ReadingIntegralGyroNick -= tmp_long; |
ReadingIntegralGyroRoll -= tmp_long2; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// MeasurementCounter is incremented in the isr of analog.c |
if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached |
{ |
static int16_t cnt = 0; |
static int8_t last_n_p, last_n_n, last_r_p, last_r_n; |
static int32_t MeanIntegralGyroNick_old, MeanIntegralGyroRoll_old; |
// if not lopping in any direction (this should be always the case, |
// because the Measurement counter is reset to 0 if looping in any direction is active.) |
if(! LoopingNick && !LoopingRoll && !FunnelCourse && ParamSet.DriftComp) |
{ |
// Calculate mean value of the gyro integrals |
MeanIntegralGyroNick /= BALANCE_NUMBER; |
MeanIntegralGyroRoll /= BALANCE_NUMBER; |
// Calculate mean of the acceleration values scaled to the gyro integrals |
MeanAccNick = (ParamSet.GyroAccFactor * MeanAccNick) / BALANCE_NUMBER; |
MeanAccRoll = (ParamSet.GyroAccFactor * MeanAccRoll) / BALANCE_NUMBER; |
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
IntegralGyroNickError = (int32_t)(MeanIntegralGyroNick - (int32_t)MeanAccNick); |
CorrectionNick = IntegralGyroNickError / ParamSet.GyroAccTrim; |
AttitudeCorrectionNick = CorrectionNick / BALANCE_NUMBER; |
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
IntegralGyroRollError = (int32_t)(MeanIntegralGyroRoll - (int32_t)MeanAccRoll); |
CorrectionRoll = IntegralGyroRollError / ParamSet.GyroAccTrim; |
AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER; |
if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FCParam.KalmanK == -1) ) |
{ |
AttitudeCorrectionNick /= 2; |
AttitudeCorrectionRoll /= 2; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gyro-Drift ermitteln |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// deviation of gyro nick integral (IntegralGyroNick is corrected by averaged acc sensor) |
IntegralGyroNickError = IntegralGyroNick2 - IntegralGyroNick; |
ReadingIntegralGyroNick2 -= IntegralGyroNickError; |
// deviation of gyro nick integral (IntegralGyroNick is corrected by averaged acc sensor) |
IntegralGyroRollError = IntegralGyroRoll2 - IntegralGyroRoll; |
ReadingIntegralGyroRoll2 -= IntegralGyroRollError; |
if(ParamSet.DriftComp) |
{ |
if(YawGyroDrift > BALANCE_NUMBER/2) AdBiasGyroYaw++; |
if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--; |
} |
YawGyroDrift = 0; |
#define ERROR_LIMIT0 (BALANCE_NUMBER / 2) |
#define ERROR_LIMIT1 (BALANCE_NUMBER * 2) |
#define ERROR_LIMIT2 (BALANCE_NUMBER * 16) |
#define MOVEMENT_LIMIT 20000 |
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1; |
if(IntegralGyroNickError > ERROR_LIMIT1) cnt = 4; |
CorrectionNick = 0; |
if((labs(MeanIntegralGyroNick_old - MeanIntegralGyroNick) < MOVEMENT_LIMIT) || (FCParam.KalmanMaxDrift > 3 * 8)) |
{ |
if(IntegralGyroNickError > ERROR_LIMIT2) |
{ |
if(last_n_p) |
{ |
cnt += labs(IntegralGyroNickError) / (ERROR_LIMIT2 / 8); |
CorrectionNick = IntegralGyroNickError / 8; |
if(CorrectionNick > 5000) CorrectionNick = 5000; |
AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER; |
} |
else last_n_p = 1; |
} |
else last_n_p = 0; |
if(IntegralGyroNickError < -ERROR_LIMIT2) |
{ |
if(last_n_n) |
{ |
cnt += labs(IntegralGyroNickError) / (ERROR_LIMIT2 / 8); |
CorrectionNick = IntegralGyroNickError / 8; |
if(CorrectionNick < -5000) CorrectionNick = -5000; |
AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER; |
} |
else last_n_n = 1; |
} |
else last_n_n = 0; |
} |
else |
{ |
cnt = 0; |
BadCompassHeading = 1000; |
} |
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp; |
if(FCParam.KalmanMaxDrift) if(cnt > FCParam.KalmanMaxDrift) cnt = FCParam.KalmanMaxDrift; |
// correct Gyro Offsets |
if(IntegralGyroNickError > ERROR_LIMIT0) BiasHiResGyroNick += cnt; |
if(IntegralGyroNickError < -ERROR_LIMIT0) BiasHiResGyroNick -= cnt; |
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1; |
if(IntegralGyroRollError > ERROR_LIMIT1) cnt = 4; |
CorrectionRoll = 0; |
if((labs(MeanIntegralGyroRoll_old - MeanIntegralGyroRoll) < MOVEMENT_LIMIT) || (FCParam.KalmanMaxDrift > 3 * 8)) |
{ |
if(IntegralGyroRollError > ERROR_LIMIT2) |
{ |
if(last_r_p) |
{ |
cnt += labs(IntegralGyroRollError) / (ERROR_LIMIT2 / 8); |
CorrectionRoll = IntegralGyroRollError / 8; |
if(CorrectionRoll > 5000) CorrectionRoll = 5000; |
AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER; |
} |
else last_r_p = 1; |
} |
else last_r_p = 0; |
if(IntegralGyroRollError < -ERROR_LIMIT2) |
{ |
if(last_r_n) |
{ |
cnt += labs(IntegralGyroRollError) / (ERROR_LIMIT2 / 8); |
CorrectionRoll = IntegralGyroRollError / 8; |
if(CorrectionRoll < -5000) CorrectionRoll = -5000; |
AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER; |
} |
else last_r_n = 1; |
} |
else last_r_n = 0; |
} |
else |
{ |
cnt = 0; |
BadCompassHeading = 1000; |
} |
// correct Gyro Offsets |
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp; |
if(FCParam.KalmanMaxDrift) if(cnt > FCParam.KalmanMaxDrift) cnt = FCParam.KalmanMaxDrift; |
if(IntegralGyroRollError > ERROR_LIMIT0) BiasHiResGyroRoll += cnt; |
if(IntegralGyroRollError < -ERROR_LIMIT0) BiasHiResGyroRoll -= cnt; |
} |
else // looping is active |
{ |
AttitudeCorrectionRoll = 0; |
AttitudeCorrectionNick = 0; |
FunnelCourse = 0; |
} |
// if GyroIFactor == 0 , for example at Heading Hold, ignore attitude correction |
if(!GyroIFactor) |
{ |
AttitudeCorrectionRoll = 0; |
AttitudeCorrectionNick = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
MeanIntegralGyroNick_old = MeanIntegralGyroNick; |
MeanIntegralGyroRoll_old = MeanIntegralGyroRoll; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// reset variables used for next averaging |
MeanAccNick = 0; |
MeanAccRoll = 0; |
MeanIntegralGyroNick = 0; |
MeanIntegralGyroRoll = 0; |
MeasurementCounter = 0; |
} // end of averaging |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// 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)) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// GPS |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE) |
{ |
GPS_Main(); |
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
} |
else |
{ |
GPSStickNick = 0; |
GPSStickRoll = 0; |
} |
#endif |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugwerte zuordnen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!TimerDebugOut--) |
{ |
TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz) |
DebugOut.Analog[0] = (10 * IntegralGyroNick) / GYRO_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[1] = (10 * IntegralGyroRoll) / GYRO_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[2] = (10 * AccNick) / ACC_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[3] = (10 * AccRoll) / ACC_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[4] = GyroYaw; |
DebugOut.Analog[5] = ReadingHeight; |
DebugOut.Analog[6] = (ReadingIntegralTop / 512); |
DebugOut.Analog[8] = CompassHeading; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = RC_Quality; |
DebugOut.Analog[11] = YawGyroHeading / GYRO_DEG_FACTOR; |
DebugOut.Analog[19] = CompassCalState; |
// DebugOut.Analog[24] = GyroNick/2; |
// DebugOut.Analog[25] = GyroRoll/2; |
DebugOut.Analog[27] = (int16_t)FCParam.KalmanMaxDrift; |
// DebugOut.Analog[28] = (int16_t)FCParam.KalmanMaxFusion; |
DebugOut.Analog[30] = GPSStickNick; |
DebugOut.Analog[31] = GPSStickRoll; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// calculate control feedback from angle (gyro integral) and agular velocity (gyro signal) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define TRIM_LIMIT 200 |
CHECK_MIN_MAX(TrimNick, -TRIM_LIMIT, TRIM_LIMIT); |
CHECK_MIN_MAX(TrimRoll, -TRIM_LIMIT, TRIM_LIMIT); |
if(FunnelCourse) |
{ |
IPartNick = 0; |
IPartRoll = 0; |
} |
if(! LoopingNick) |
{ |
PPartNick = (IntegralGyroNick * GyroIFactor) / (44000 / STICK_GAIN); // P-Part |
} |
else |
{ |
PPartNick = 0; |
} |
PDPartNick = PPartNick + (int32_t)((int32_t)GyroNick * GyroPFactor + (int32_t)TrimNick * 128L) / (256L / STICK_GAIN); // +D-Part |
if(!LoopingRoll) |
{ |
PPartRoll = (IntegralGyroRoll * GyroIFactor) / (44000 / STICK_GAIN); // P-Part |
} |
else |
{ |
PPartRoll = 0; |
} |
PDPartRoll = PPartRoll + (int32_t)((int32_t)GyroRoll * GyroPFactor + (int32_t)TrimRoll * 128L) / (256L / STICK_GAIN); // +D-Part |
PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / STICK_GAIN)); |
//DebugOut.Analog[21] = PDPartNick; |
//DebugOut.Analog[22] = PDPartRoll; |
// limit control feedback |
#define SENSOR_LIMIT (4096 * 4) |
CHECK_MIN_MAX(PDPartNick, -SENSOR_LIMIT, SENSOR_LIMIT); |
CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT); |
CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// all BL-Ctrl connected? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(MissingMotor) |
{ |
// if we are in the lift off condition |
if( (ModelIsFlying > 1) && (ModelIsFlying < 50) && (GasMixFraction > 0) ) |
ModelIsFlying = 1; // keep within lift off condition |
GasMixFraction = ParamSet.GasMin; // reduce gas to min to avoid lift of |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Height Control |
// The height control algorithm reduces the gas but does not increase the gas. |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMixFraction *= STICK_GAIN; |
// if height control is activated and no emergency landing is active |
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && !(MKFlags & MKFLAG_EMERGENCY_LANDING) ) |
{ |
int tmp_int; |
static uint8_t delay = 100; |
// if height control is activated by an rc channel |
if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH) |
{ // check if parameter is less than activation threshold |
if( |
( (ParamSet.BitConfig & CFG_HEIGHT_3SWITCH) && ( (FCParam.MaxHeight > 80) && (FCParam.MaxHeight < 140) ) )|| // for 3-state switch height control is only disabled in center position |
(!(ParamSet.BitConfig & CFG_HEIGHT_3SWITCH) && (FCParam.MaxHeight < 50) ) // for 2-State switch height control is disabled in lower position |
) |
{ //hight control not active |
if(!delay--) |
{ |
// measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
if( (ReadingAirPressure > 1000) && (OCR0A < 255) ) |
{ // increase offset |
if(OCR0A < 244) |
{ |
ExpandBaro -= 10; |
OCR0A = PressureSensorOffset - ExpandBaro; |
} |
else |
{ |
OCR0A = 254; |
} |
BeepTime = 300; |
delay = 250; |
} |
// measurement of air pressure close to lower limit and |
else if( (ReadingAirPressure < 100) && (OCR0A > 1) ) |
{ // decrease offset |
if(OCR0A > 10) |
{ |
ExpandBaro += 10; |
OCR0A = PressureSensorOffset - ExpandBaro; |
} |
else |
{ |
OCR0A = 1; |
} |
BeepTime = 300; |
delay = 250; |
} |
else |
{ |
SetPointHeight = ReadingHeight - 20; // update SetPoint with current reading |
HeightControlActive = 0; // disable height control |
delay = 1; |
} |
} |
} |
else |
{ //hight control not active |
HeightControlActive = 1; // enable height control |
delay = 200; |
} |
} |
else // no switchable height control |
{ |
SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20; |
HeightControlActive = 1; |
} |
// get current height |
h = ReadingHeight; |
// if current height is above the setpoint reduce gas |
if((h > SetPointHeight) && HeightControlActive) |
{ |
// height difference -> P control part |
h = ((h - SetPointHeight) * (int16_t) FCParam.HeightP) / (16 / STICK_GAIN); |
h = GasMixFraction - h; // reduce gas |
// height gradient --> D control part |
//h -= (HeightD * FCParam.HeightD) / (8 / STICK_GAIN); // D control part |
h -= (HeightD) / (8 / STICK_GAIN); // D control part |
// acceleration sensor effect |
tmp_int = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN); |
if(tmp_int > 70 * STICK_GAIN) tmp_int = 70 * STICK_GAIN; |
else if(tmp_int < -(70 * STICK_GAIN)) tmp_int = -(70 * STICK_GAIN); |
h -= tmp_int; |
// update height control gas |
HeightControlGas = (HeightControlGas*15 + h) / 16; |
// limit gas reduction |
if(HeightControlGas < ParamSet.HeightMinGas * STICK_GAIN) |
{ |
if(GasMixFraction >= ParamSet.HeightMinGas * STICK_GAIN) HeightControlGas = ParamSet.HeightMinGas * STICK_GAIN; |
// allows landing also if gas stick is reduced below min gas on height control |
if(GasMixFraction < ParamSet.HeightMinGas * STICK_GAIN) HeightControlGas = GasMixFraction; |
} |
// limit gas to stick setting |
if(HeightControlGas > GasMixFraction) HeightControlGas = GasMixFraction; |
GasMixFraction = HeightControlGas; |
} |
} |
// limit gas to parameter setting |
if(GasMixFraction > (ParamSet.GasMax - 20) * STICK_GAIN) GasMixFraction = (ParamSet.GasMax - 20) * STICK_GAIN; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Mixer and PI-Controller |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DebugOut.Analog[7] = GasMixFraction; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Yaw-Fraction |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
YawMixFraction = PDPartYaw - SetPointYaw * STICK_GAIN; // yaw controller |
#define MIN_YAWGAS (40 * STICK_GAIN) // 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 * STICK_GAIN; |
CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction)); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Nick-Axis |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffNick = PDPartNick - StickNick; // get difference |
if(GyroIFactor) IPartNick += PPartNick - StickNick; // I-part for attitude control |
else IPartNick += DiffNick; // I-part for head holding |
CHECK_MIN_MAX(IPartNick, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L)); |
NickMixFraction = DiffNick + (IPartNick / Ki); // PID-controller for nick |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Roll-Axis |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffRoll = PDPartRoll - StickRoll; // get difference |
if(GyroIFactor) IPartRoll += PPartRoll - StickRoll; // I-part for attitude control |
else IPartRoll += DiffRoll; // I-part for head holding |
CHECK_MIN_MAX(IPartRoll, -(STICK_GAIN * 16000L), (STICK_GAIN * 16000L)); |
RollMixFraction = DiffRoll + (IPartRoll / Ki); // PID-controller for roll |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Limiter |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction) / 2)) / 64; |
CHECK_MIN_MAX(NickMixFraction, -tmp_int, tmp_int); |
CHECK_MIN_MAX(RollMixFraction, -tmp_int, tmp_int); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Universal Mixer |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
int16_t tmp; |
if(Mixer.Motor[i][MIX_GAS] > 0) // if gas then mixer |
{ |
tmp = ((int32_t)GasMixFraction * Mixer.Motor[i][MIX_GAS] ) / 64L; |
tmp += ((int32_t)NickMixFraction * Mixer.Motor[i][MIX_NICK]) / 64L; |
tmp += ((int32_t)RollMixFraction * Mixer.Motor[i][MIX_ROLL]) / 64L; |
tmp += ((int32_t)YawMixFraction * Mixer.Motor[i][MIX_YAW] ) / 64L; |
MotorValue[i] = MotorSmoothing(tmp, MotorValue[i]); // Spike Filter |
tmp = MotorValue[i] / STICK_GAIN; |
CHECK_MIN_MAX(tmp, ParamSet.GasMin, ParamSet.GasMax); |
Motor[i].SetPoint = tmp; |
} |
else Motor[i].SetPoint = 0; |
} |
} |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/fc.h |
---|
0,0 → 1,157 |
/*####################################################################################### |
Flight Control |
#######################################################################################*/ |
#ifndef _FC_H |
#define _FC_H |
#include <inttypes.h> |
// scaling from AdAccNick, AdAccRoll -> AccNick, AccRoll |
// i.e. AccNick = ACC_AMPLIFY * AdAccNick |
#define ACC_AMPLIFY 6 |
// scaling from AccNick, AccRoll -> Attitude in deg (approx sin(x) = x), |
// i.e. Nick Angle in deg = AccNick / ACC_DEG_FACTOR |
// the value is derived from the datasheet of the ACC sensor where 5g are scaled to vref |
// therefore 1g is 1024/5 = 205 counts. the adc isr combines 2 acc samples to AdValueAcc |
// and 1g yields to AdValueAcc = 2* 205 * 410 wich is again scaled by ACC_DEG_FACTOR |
// that results in 1g --> Acc = 205 * 12 = 2460. the linear approx of the arcsin and the scaling |
// of Acc gives the factor below. sin(20deg) * 2460 = 841 --> 841 / 20 = 42 |
#define ACC_DEG_FACTOR 42 |
// scaling from IntegralGyroNick, IntegralGyroRoll, IntegralGyroYaw -> Attitude in deg |
// i.e. Nick Angle in deg = IntegralGyroNick / GYRO_DEG_FACTOR |
#define GYRO_DEG_FACTOR ((int16_t)(ParamSet.GyroAccFactor) * ACC_DEG_FACTOR) |
extern uint8_t RequiredMotors; |
typedef struct |
{ |
uint8_t HeightD; |
uint8_t MaxHeight; |
uint8_t HeightP; |
uint8_t Height_ACC_Effect; |
uint8_t CompassYawEffect; |
uint8_t GyroD; |
uint8_t GyroP; |
uint8_t GyroI; |
uint8_t StickYawP; |
uint8_t IFactor; |
uint8_t UserParam1; |
uint8_t UserParam2; |
uint8_t UserParam3; |
uint8_t UserParam4; |
uint8_t UserParam5; |
uint8_t UserParam6; |
uint8_t UserParam7; |
uint8_t UserParam8; |
uint8_t ServoNickControl; |
uint8_t LoopGasLimit; |
uint8_t AxisCoupling1; |
uint8_t AxisCoupling2; |
uint8_t AxisCouplingYawCorrection; |
uint8_t DynamicStability; |
uint8_t ExternalControl; |
uint8_t J16Timing; |
uint8_t J17Timing; |
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
uint8_t NaviGpsModeControl; |
uint8_t NaviGpsGain; |
uint8_t NaviGpsP; |
uint8_t NaviGpsI; |
uint8_t NaviGpsD; |
uint8_t NaviGpsACC; |
uint8_t NaviOperatingRadius; |
uint8_t NaviWindCorrection; |
uint8_t NaviSpeedCompensation; |
#endif |
int8_t KalmanK; |
int8_t KalmanMaxDrift; |
int8_t KalmanMaxFusion; |
} fc_param_t; |
extern fc_param_t FCParam; |
// rotation rates |
extern int16_t GyroNick, GyroRoll, GyroYaw; |
// attitude calcualted by temporal integral of gyro rates |
extern int32_t IntegralGyroNick, IntegralGyroRoll, IntegralGyroYaw; |
// bias values |
extern int16_t BiasHiResGyroNick, BiasHiResGyroRoll, AdBiasGyroYaw; |
extern int16_t AdBiasAccNick, AdBiasAccRoll; |
extern volatile float AdBiasAccTop; |
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; |
// hight control |
extern int ReadingHeight; |
extern int SetPointHeight; |
// accelerations |
extern int16_t AccNick, AccRoll, AccTop; |
// acceleration send to navi board |
extern int16_t NaviAccNick, NaviAccRoll, NaviCntAcc; |
// looping params |
extern long TurnOver180Nick, TurnOver180Roll; |
// external control |
extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw; |
#define ACC_CALIB 1 |
#define NO_ACC_CALIB 0 |
void MotorControl(void); |
void SendMotorData(void); |
void SetNeutral(uint8_t AccAdjustment); |
void Beep(uint8_t numbeeps); |
extern int16_t Poti1, Poti2, Poti3, Poti4, Poti5, Poti6, Poti7, Poti8; |
// current stick values |
extern int16_t StickNick; |
extern int16_t StickRoll; |
extern int16_t StickYaw; |
// current GPS-stick values |
extern int16_t GPSStickNick; |
extern int16_t GPSStickRoll; |
// current stick elongations |
extern int16_t MaxStickNick, MaxStickRoll, MaxStickYaw; |
extern uint16_t ModelIsFlying; |
// MKFlags |
#define MKFLAG_MOTOR_RUN 0x01 |
#define MKFLAG_FLY 0x02 |
#define MKFLAG_CALIBRATE 0x04 |
#define MKFLAG_START 0x08 |
#define MKFLAG_EMERGENCY_LANDING 0x10 |
#define MKFLAG_RESERVE1 0x20 |
#define MKFLAG_RESERVE2 0x40 |
#define MKFLAG_RESERVE3 0x80 |
extern volatile uint8_t MKFlags; |
#endif //_FC_H |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/gps.c |
---|
0,0 → 1,466 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "fc.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 (FCParam.NaviGpsModeControl < 50) FlightMode = GPS_FLIGHT_MODE_AID; |
else if(FCParam.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 >= ParamSet.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[ParamSet.ChannelAssignment[CH_NICK]]) < ParamSet.NaviStickThreshold) && (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < ParamSet.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)FCParam.NaviGpsD * GPSInfo.velnorth)/512; |
D_East = ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast)/512; |
// P-Part |
P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/2048; |
P_East = ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/2048; |
// I-Part |
I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/8192; |
I_East = ((int32_t)FCParam.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, ParamSet.NaviGpsPLimit)) |
{ |
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)FCParam.NaviGpsGain) / 100; |
PID_East = (PID_East * (int32_t)FCParam.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 fc.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, ParamSet.NaviAngleLimitation); |
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 < ParamSet.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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/gps.h |
---|
0,0 → 1,9 |
#ifndef _GPS_H |
#define _GPS_H |
#include <inttypes.h> |
extern void GPS_Main(void); |
#endif //_GPS_H |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/led.c |
---|
0,0 → 1,115 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "led.h" |
#include "fc.h" |
#include "eeprom.h" |
uint8_t J16Blinkcount = 0, J16Mask = 1; |
uint8_t J17Blinkcount = 0, J17Mask = 1; |
// initializes the LED control outputs J16, J17 |
void LED_Init(void) |
{ |
// set PC2 & PC3 as output (control of J16 & J17) |
DDRC |= (1<<DDC2)|(1<<DDC3); |
J16_OFF; |
J17_OFF; |
J16Blinkcount = 0; J16Mask = 128; |
J17Blinkcount = 0; J17Mask = 128; |
} |
// called in main loop every 2ms |
void LED_Update(void) |
{ |
static int8_t delay = 0; |
if(!delay--) // 10 ms intervall |
{ |
delay = 4; |
if ((ParamSet.J16Timing > 250) && (FCParam.J16Timing > 230)) |
{ |
if(ParamSet.J16Bitmask & 128) J16_ON; |
else J16_OFF; |
} |
else if ((ParamSet.J16Timing > 250) && (FCParam.J16Timing < 10)) |
{ |
if(ParamSet.J16Bitmask & 128) J16_OFF; |
else J16_ON; |
} |
else if(!J16Blinkcount--) |
{ |
J16Blinkcount = FCParam.J16Timing - 1; |
if(J16Mask == 1) J16Mask = 128; else J16Mask /= 2; |
if(J16Mask & ParamSet.J16Bitmask) J16_ON; else J16_OFF; |
} |
if ((ParamSet.J17Timing > 250) && (FCParam.J17Timing > 230)) |
{ |
if(ParamSet.J17Bitmask & 128) J17_ON; |
else J17_OFF; |
} |
else if ((ParamSet.J17Timing > 250) && (FCParam.J17Timing < 10)) |
{ |
if(ParamSet.J17Bitmask & 128) J17_OFF; |
else J17_ON; |
} |
else if(!J17Blinkcount--) |
{ |
J17Blinkcount = FCParam.J17Timing - 1; |
if(J17Mask == 1) J17Mask = 128; else J17Mask /= 2; |
if(J17Mask & ParamSet.J17Bitmask) J17_ON; else J17_OFF; |
} |
} |
} |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/led.h |
---|
0,0 → 1,19 |
#ifndef _LED_H |
#define _LED_H |
#include <avr/io.h> |
#define J16_ON PORTC |= (1<<PORTC2) |
#define J16_OFF PORTC &= ~(1<<PORTC2) |
#define J16_TOGGLE PORTC ^= (1<<PORTC2) |
#define J17_ON PORTC |= (1<<PORTC3) |
#define J17_OFF PORTC &= ~(1<<PORTC3) |
#define J17_TOGGLE PORTC ^= (1<<PORTC3) |
void LED_Init(void); |
void LED_Update(void); |
#endif //_LED_H |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/main.c |
---|
0,0 → 1,367 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "main.h" |
#include "timer0.h" |
#include "timer2.h" |
#include "uart0.h" |
#include "uart1.h" |
#include "led.h" |
#include "menu.h" |
#include "fc.h" |
#include "rc.h" |
#include "analog.h" |
#include "printf_P.h" |
#ifdef USE_KILLAGREG |
#include "mm3.h" |
#endif |
#ifdef USE_NAVICTRL |
#include "spi.h" |
#endif |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#endif |
#include "twimaster.h" |
#include "eeprom.h" |
uint8_t BoardRelease = 10; |
uint8_t CPUType = ATMEGA644; |
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; |
} |
uint8_t GetBoardRelease(void) |
{ |
uint8_t BoardRelease = 10; |
// 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; |
} |
int16_t main (void) |
{ |
uint16_t timer; |
uint8_t i; |
// disable interrupts global |
cli(); |
// analyze hardware environment |
CPUType = GetCPUType(); |
BoardRelease = GetBoardRelease(); |
// disable watchdog |
MCUSR &=~(1<<WDRF); |
WDTCSR |= (1<<WDCE)|(1<<WDE); |
WDTCSR = 0; |
BeepTime = 2000; |
PPM_in[CH_GAS] = 0; |
StickYaw = 0; |
StickRoll = 0; |
StickNick = 0; |
RED_OFF; |
// initalize modules |
LED_Init(); |
TIMER0_Init(); |
TIMER2_Init(); |
USART0_Init(); |
if(CPUType == ATMEGA644P) USART1_Init(); |
RC_Init(); |
ADC_Init(); |
I2C_Init(); |
#ifdef USE_NAVICTRL |
SPI_MasterInit(); |
#endif |
#ifdef USE_KILLAGREG |
MM3_Init(); |
#endif |
#ifdef USE_MK3MAG |
MK3MAG_Init(); |
#endif |
// enable interrupts global |
sei(); |
printf("\n\r==================================="); |
printf("\n\rFlightControl"); |
printf("\n\rHardware: %d.%d", BoardRelease/10, BoardRelease%10); |
if(CPUType == ATMEGA644P) |
printf("\r\n CPU: Atmega644p"); |
else |
printf("\r\n CPU: Atmega644"); |
printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
printf("\n\r==================================="); |
GRN_ON; |
// Parameter Set handling |
ParamSet_Init(); |
// Check connected BL-Ctrls |
printf("\n\rFound BL-Ctrl: "); |
motor_read = 0; |
UpdateMotor = 0; |
SendMotorData(); |
while(!UpdateMotor); |
motor_read = 0; // read the first I2C-Data |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
UpdateMotor = 0; |
SendMotorData(); |
while(!UpdateMotor); |
if(Motor[i].Present) printf("%d ",i+1); |
} |
for(i = 0; i < MAX_MOTORS; i++) |
{ |
if(!Motor[i].Present && Mixer.Motor[i][MIX_GAS] > 0) printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i+1); |
Motor[i].Error = 0; |
} |
printf("\n\r==================================="); |
if(GetParamWord(PID_ACC_NICK) > 2048) |
{ |
printf("\n\rACC not calibrated!"); |
} |
//wait for a short time (otherwise the RC channel check won't work below) |
timer = SetDelay(500); |
while(!CheckDelay(timer)); |
if(ParamSet.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_KILLAGREG |
printf("\n\rSupport for MicroMag3 Compass"); |
#endif |
#ifdef USE_MK3MAG |
printf("\n\rSupport for MK3MAG Compass"); |
#endif |
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART"); |
else printf("\n\rSupport for GPS at 1st UART"); |
#endif |
SetNeutral(NO_ACC_CALIB); |
RED_OFF; |
BeepTime = 2000; |
ExternControl.Digital[0] = 0x55; |
printf("\n\rControl: "); |
if (ParamSet.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold"); |
else printf("Neutral (ACC-Mode)"); |
printf("\n\n\r"); |
LCD_Clear(); |
I2CTimeout = 5000; |
while (1) |
{ |
if(UpdateMotor && ADReady) // control interval |
{ |
UpdateMotor = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
//J4HIGH; |
MotorControl(); |
//J4LOW; |
SendMotorData(); // the flight control code |
RED_OFF; |
if(PcAccess) PcAccess--; |
else |
{ |
ExternControl.Config = 0; |
ExternStickNick= 0; |
ExternStickRoll = 0; |
ExternStickYaw = 0; |
} |
if(RC_Quality) RC_Quality--; |
#ifdef USE_NAVICTRL |
if(NCDataOkay) |
{ |
if(--NCDataOkay == 0) // no data from NC |
{ // set gps control sticks neutral |
GPSStickNick = 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; |
} |
if((BeepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN) ) |
{ |
BeepTime = 10000; // 1 second |
BeepModulation = 0x0080; |
} |
} |
else |
{ |
RED_OFF; |
} |
// allow Serial Data Transmit if motors must not updated or motors are not running |
if( !UpdateMotor || !(MKFlags & MKFLAG_MOTOR_RUN) ) |
{ |
USART0_TransmitTxData(); |
} |
USART0_ProcessRxData(); |
if(CheckDelay(timer)) |
{ |
if(UBat < ParamSet.LowVoltageWarning) |
{ |
BeepModulation = 0x0300; |
if(!BeepTime ) |
{ |
BeepTime = 6000; // 0.6 seconds |
} |
} |
#ifdef USE_NAVICTRL |
SPI_StartTransmitPacket(); |
SendSPI = 4; |
#endif |
timer = SetDelay(20); // every 20 ms |
} |
LED_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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/main.h |
---|
0,0 → 1,31 |
#ifndef _MAIN_H |
#define _MAIN_H |
#include <avr/io.h> |
#define ATMEGA644 0 |
#define ATMEGA644P 1 |
#define SYSCLK F_CPU |
// neue Hardware |
#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) |
#include <inttypes.h> |
extern uint8_t BoardRelease; |
extern uint8_t CPUType; |
#endif //_MAIN_H |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/makefile |
---|
0,0 → 1,508 |
#-------------------------------------------------------------------- |
# MCU name |
MCU = atmega644p |
F_CPU = 20000000 |
#------------------------------------------------------------------- |
VERSION_MAJOR = 0 |
VERSION_MINOR = 74 |
VERSION_PATCH = 3 |
VERSION_SERIAL_MAJOR = 10 # Serial Protocol Major Version |
VERSION_SERIAL_MINOR = 1 # Serial Protocol Minor Version |
NC_SPI_COMPATIBLE = 6 # SPI Protocol Version |
#------------------------------------------------------------------- |
#OPTIONS |
# Use one of the extensions for a gps solution |
#EXT = KILLAGREG |
#EXT = NAVICTRL |
EXT = MK3MAG |
# Use optional one the RCs if EXT = NAVICTRL has been used |
#RC = DSL |
#RC = SPECTRUM |
#------------------------------------------------------------------- |
# get SVN revision |
REV := $(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) |
endif |
ifeq ($(MCU), atmega644p) |
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m |
HEX_NAME = MEGA644p_$(EXT)_$(RC) |
endif |
ifeq ($(F_CPU), 16000000) |
QUARZ = 16MHZ |
endif |
ifeq ($(F_CPU), 20000000) |
QUARZ = 20MHZ |
endif |
# Output format. (can be srec, ihex, binary) |
FORMAT = ihex |
# Target file name (without extension). |
ifeq ($(VERSION_PATCH), 0) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)a_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 1) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)b_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 2) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)c_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 3) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)d_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 4) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)e_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 5) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)f_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 6) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)g_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 7) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)h_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 8) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)i_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 9) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)j_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 10) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)k_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 11) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)l_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 12) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)m_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 13) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)n_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 14) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)o_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 15) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)p_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 16) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)q_SVN$(REV) |
endif |
ifeq ($(VERSION_PATCH), 17) |
TARGET = Flight-Ctrl_$(HEX_NAME)_V$(VERSION_MAJOR)_$(VERSION_MINOR)r_SVN$(REV) |
endif |
# Optimization level, can be [0, 1, 2, 3, s]. 0 turns off optimization. |
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.) |
OPT = 2 |
#OPT = s |
########################################################################################################## |
# List C source files here. (C dependencies are automatically generated.) |
SRC = main.c uart0.c printf_P.c timer0.c timer2.c analog.c menu.c led.c |
SRC += twimaster.c rc.c fc.c eeprom.c uart1.c |
ifeq ($(EXT), KILLAGREG) |
SRC += mm3.c mymath.c gps.c ubx.c |
endif |
ifeq ($(EXT), MK3MAG) |
SRC += mk3mag.c mymath.c gps.c ubx.c |
endif |
ifeq ($(EXT), NAVICTRL) |
SRC += spi.c |
ifeq ($(RC), DSL) |
SRC += dsl.c |
endif |
ifeq ($(RC), SPECTRUM) |
SRC += spectrum.c |
endif |
endif |
########################################################################################################## |
# List Assembler source files here. |
# Make them always end in a capital .S. Files ending in a lowercase .s |
# will not be considered source files but generated files (assembler |
# output from the compiler), and will be deleted upon "make clean"! |
# Even though the DOS/Win* filesystem matches both .s and .S the same, |
# it will preserve the spelling of the filenames, and gcc itself does |
# care about how the name is spelled on its command-line. |
ASRC = |
# List any extra directories to look for include files here. |
# Each directory must be seperated by a space. |
EXTRAINCDIRS = |
# Optional compiler flags. |
# -g: generate debugging information (for GDB, or for COFF conversion) |
# -O*: optimization level |
# -f...: tuning, see gcc manual and avr-libc documentation |
# -Wall...: warning level |
# -Wa,...: tell GCC to pass this to the assembler. |
# -ahlms: create assembler listing |
CFLAGS = -O$(OPT) \ |
-funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums \ |
-Wall -Wstrict-prototypes \ |
-Wa,-adhlns=$(<:.c=.lst) \ |
$(patsubst %,-I%,$(EXTRAINCDIRS)) |
# Set a "language standard" compiler flag. |
# Unremark just one line below to set the language standard to use. |
# gnu99 = C99 + GNU extensions. See GCC manual for more information. |
#CFLAGS += -std=c89 |
#CFLAGS += -std=gnu89 |
#CFLAGS += -std=c99 |
CFLAGS += -std=gnu99 |
CFLAGS += -DF_CPU=$(F_CPU) -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_PATCH=$(VERSION_PATCH) -DVERSION_SERIAL_MAJOR=$(VERSION_SERIAL_MAJOR) -DVERSION_SERIAL_MINOR=$(VERSION_SERIAL_MINOR) -DNC_SPI_COMPATIBLE=$(NC_SPI_COMPATIBLE) |
ifeq ($(EXT), KILLAGREG) |
CFLAGS += -DUSE_KILLAGREG |
endif |
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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/menu.c |
---|
0,0 → 1,325 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "main.h" |
#include "eeprom.h" |
#include "timer2.h" |
#include "fc.h" |
#include "rc.h" |
#include "uart0.h" |
#include "printf_P.h" |
#include "analog.h" |
#include "twimaster.h" |
#ifdef USE_KILLAGREG |
#include "mm3.h" |
#endif |
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
#include "ubx.h" |
#endif |
#if (!defined (USE_KILLAGREG) && !defined (USE_MK3MAG)) |
uint8_t MaxMenuItem = 13; |
#else |
#ifdef USE_MK3MAG |
uint8_t MaxMenuItem = 14; |
#endif |
#ifdef USE_KILLAGREG |
uint8_t MaxMenuItem = 16; |
#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(ParamSet.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",ReadingAirPressure); |
LCD_printfxy(0,3,"Offset :%5i",PressureSensorOffset); |
} |
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",IntegralGyroNick/1024); |
LCD_printfxy(0,2,"Roll: %5i",IntegralGyroRoll/1024); |
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[ParamSet.ChannelAssignment[CH_NICK]],PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); |
LCD_printfxy(0,1,"Gs:%4i Ya:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_GAS]],PPM_in[ParamSet.ChannelAssignment[CH_YAW]]); |
LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_POTI1]],PPM_in[ParamSet.ChannelAssignment[CH_POTI2]]); |
LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_POTI3]],PPM_in[ParamSet.ChannelAssignment[CH_POTI4]]); |
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 - BiasHiResGyroNick / HIRES_GYRO_AMPLIFY, BiasHiResGyroNick / HIRES_GYRO_AMPLIFY, BiasHiResGyroNick % HIRES_GYRO_AMPLIFY); |
LCD_printfxy(0,2,"Roll %4i (%3i.%i)",AdValueGyroRoll - BiasHiResGyroRoll / HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll / HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll % HIRES_GYRO_AMPLIFY); |
LCD_printfxy(0,3,"Yaw %4i (%3i)",AdBiasGyroYaw - AdValueGyroYaw , AdBiasGyroYaw); |
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)",AdValueGyroNick - BiasHiResGyroNick/HIRES_GYRO_AMPLIFY, BiasHiResGyroNick / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroNick % (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)",AdValueGyroRoll - BiasHiResGyroRoll/HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroRoll % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7) |
LCD_printfxy(0,3,"Yaw %4i (%3i)",AdBiasGyroYaw - AdValueGyroYaw , AdBiasGyroYaw/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)",AdValueGyroNick - BiasHiResGyroNick/HIRES_GYRO_AMPLIFY, BiasHiResGyroNick / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroNick % (HIRES_GYRO_AMPLIFY * 2))/2, DacOffsetGyroNick); // division by 2 to push the reminder below 10 (15/2 = 7) |
LCD_printfxy(0,2,"Roll %4i (%3i.%i)(%3i)",AdValueGyroRoll - BiasHiResGyroRoll/HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroRoll % (HIRES_GYRO_AMPLIFY * 2))/2, DacOffsetGyroRoll); // division by 2 to push the reminder below 10 (15/2 = 7) |
LCD_printfxy(0,3,"Yaw %4i (%3i)(%3i)",AdBiasGyroYaw - AdValueGyroYaw , AdBiasGyroYaw/2, DacOffsetGyroYaw ); |
break; |
} |
break; |
case 6:// Acceleration Sensor Menu Item |
LCD_printfxy(0,0,"ACC - Sensor"); |
LCD_printfxy(0,1,"Nick %4i (%3i)",AdValueAccNick/2, AdBiasAccNick/2); // factor 2 because of adding 2 samples in ADC ISR |
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueAccRoll/2, AdBiasAccRoll/2); // factor 2 because of adding 2 samples in ADC ISR |
LCD_printfxy(0,3,"Height %4i (%3i)",AdValueAccTop, (int16_t)AdBiasAccTop); |
break; |
case 7:// Accumulator 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" ,Poti1,Poti5); //PPM24-Extesion |
LCD_printfxy(0,1,"Po2: %3i Po6: %3i" ,Poti2,Poti6); //PPM24-Extesion |
LCD_printfxy(0,2,"Po3: %3i Po7: %3i" ,Poti3,Poti7); //PPM24-Extesion |
LCD_printfxy(0,3,"Po4: %3i Po8: %3i" ,Poti4,Poti8); //PPM24-Extesion |
break; |
case 10:// Servo Menu Item |
LCD_printfxy(0,0,"Servo " ); |
LCD_printfxy(0,1,"Setpoint %3i",FCParam.ServoNickControl); |
LCD_printfxy(0,2,"Position: %3i",ServoNickValue); |
LCD_printfxy(0,3,"Range:%3i-%3i",ParamSet.ServoNickMin, ParamSet.ServoNickMax); |
break; |
case 11://Extern Control |
LCD_printfxy(0,0,"ExternControl " ); |
LCD_printfxy(0,1,"Ni:%4i Ro:%4i ",ExternControl.Nick, ExternControl.Roll); |
LCD_printfxy(0,2,"Gs:%4i Ya:%4i ",ExternControl.Gas, ExternControl.Yaw); |
LCD_printfxy(0,3,"Hi:%4i Cf:%4i ",ExternControl.Height, ExternControl.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_KILLAGREG) || 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 |
#ifdef USE_KILLAGREG |
case 15:// MM3 Kompass |
LCD_printfxy(0,0,"MM3 Offset"); |
LCD_printfxy(0,1,"X_Offset: %3i",MM3_calib.X_off); |
LCD_printfxy(0,2,"Y_Offset: %3i",MM3_calib.Y_off); |
LCD_printfxy(0,3,"Z_Offset: %3i",MM3_calib.Z_off); |
break; |
case 16://MM3 Range |
LCD_printfxy(0,0,"MM3 Range"); |
LCD_printfxy(0,1,"X_Range: %4i",MM3_calib.X_range); |
LCD_printfxy(0,2,"Y_Range: %4i",MM3_calib.Y_range); |
LCD_printfxy(0,3,"Z_Range: %4i",MM3_calib.Z_range); |
break; |
#endif |
default: |
MaxMenuItem = MenuItem - 1; |
MenuItem = 0; |
break; |
} |
RemoteKeys = 0; |
} |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/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 "fc.h" |
#include "rc.h" |
#include "eeprom.h" |
#include "mk3mag.h" |
uint8_t PWMTimeout = 12; |
ToMk3Mag_t ToMk3Mag; |
/*********************************************/ |
/* Initialize Interface to MK3MAG Compass */ |
/*********************************************/ |
void MK3MAG_Init(void) |
{ |
// Port PC4 connected to PWM output from compass module |
DDRC &= ~(1<<DDC4); // set as input |
PORTC |= (1<<PORTC4); // pull up to increase PWM counter also if nothing is connected |
PWMTimeout = 0; |
ToMk3Mag.CalState = 0; |
ToMk3Mag.Orientation = 1; |
} |
/*********************************************/ |
/* Get PWM from MK3MAG */ |
/*********************************************/ |
void MK3MAG_Update(void) // called every 102.4 us by timer 0 ISR |
{ |
static uint16_t PWMCount = 0; |
static uint16_t BeepDelay = 0; |
// The pulse width varies from 1ms (0°) to 36.99ms (359.9°) |
// in other words 100us/° with a +1ms offset. |
// The signal goes low for 65ms between pulses, |
// so the cycle time is 65mS + the pulse width. |
// pwm is high |
if(PINC & (1<<PINC4)) |
{ // If PWM signal is high increment PWM high counter |
// This counter is incremented by a periode of 102.4us, |
// i.e. the resoluton of pwm coded heading is approx. 1 deg. |
PWMCount++; |
// pwm overflow? |
if (PWMCount > 400) |
{ |
if(PWMTimeout) PWMTimeout--; // decrement timeout |
CompassHeading = -1; // unknown heading |
PWMCount = 0; // reset PWM Counter |
} |
} |
else // pwm is low |
{ // ignore pwm values values of 0 and higher than 37 ms; |
if((PWMCount) && (PWMCount < 362)) // 362 * 102.4us = 37.0688 ms |
{ |
if(PWMCount <10) CompassHeading = 0; |
else CompassHeading = ((uint32_t)(PWMCount - 10) * 1049L)/1024; // correct timebase and offset |
CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
// 12 * 362 counts * 102.4 us |
} |
PWMCount = 0; // reset pwm counter |
} |
if(!PWMTimeout) |
{ |
if(CheckDelay(BeepDelay)) |
{ |
if(!BeepTime) BeepTime = 100; // make noise with 10Hz to signal the compass problem |
BeepDelay = SetDelay(100); |
} |
} |
} |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/mk3mag.h |
---|
0,0 → 1,21 |
#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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/mm3.c |
---|
0,0 → 1,534 |
/* |
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 "main.h" |
#include "mymath.h" |
#include "fc.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[ParamSet.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[ParamSet.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)ParamSet.UserParam4 + 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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/mymath.c |
---|
0,0 → 1,100 |
#include <stdlib.h> |
#include <avr/pgmspace.h> |
#include "mymath.h" |
// discrete mathematics |
// Sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
const uint16_t pgm_sinlookup[91] PROGMEM = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192}; |
int16_t c_sin_8192(int16_t angle) |
{ |
int8_t m,n; |
int16_t sinus; |
// avoid negative angles |
if (angle < 0) |
{ |
m = -1; |
angle = abs(angle); |
} |
else m = +1; |
// fold angle to intervall 0 to 359 |
angle %= 360; |
// check quadrant |
if (angle <= 90) n=1; // first quadrant |
else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
else {angle = 360 - angle; n = -1;} //fourth quadrant |
// get lookup value |
sinus = pgm_read_word(&pgm_sinlookup[angle]); |
// calculate sinus value |
return (sinus * m * n); |
} |
// Cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
int16_t c_cos_8192(int16_t angle) |
{ |
return (c_sin_8192(90 - angle)); |
} |
// Arcustangens returns degree in a range of +/. 180 deg |
const uint8_t pgm_atanlookup[346] PROGMEM = {0,1,2,3,4,4,5,6,7,8,9,10,11,11,12,13,14,15,16,17,17,18,19,20,21,21,22,23,24,24,25,26,27,27,28,29,29,30,31,31,32,33,33,34,35,35,36,36,37,37,38,39,39,40,40,41,41,42,42,43,43,44,44,45,45,45,46,46,47,47,48,48,48,49,49,50,50,50,51,51,51,52,52,52,53,53,53,54,54,54,55,55,55,55,56,56,56,57,57,57,57,58,58,58,58,59,59,59,59,60,60,60,60,60,61,61,61,61,62,62,62,62,62,63,63,63,63,63,63,64,64,64,64,64,64,65,65,65,65,65,65,66,66,66,66,66,66,66,67,67,67,67,67,67,67,68,68,68,68,68,68,68,68,69,69,69,69,69,69,69,69,69,70,70,70,70,70,70,70,70,70,71,71,71,71,71,71,71,71,71,71,71,72,72,72,72,72,72,72,72,72,72,72,73,73,73,73,73,73,73,73,73,73,73,73,73,73,74,74,74,74,74,74,74,74,74,74,74,74,74,74,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,75,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,76,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,77,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79,79}; |
int16_t c_atan2(int16_t y, int16_t x) |
{ |
int16_t index, angle; |
int8_t m; |
if (!x && !y) return 0; //atan2(0, 0) is undefined |
if (y < 0) m = -1; |
else m = 1; |
if (!x) return (90 * m); // atan2(y,0) = +/- 90 deg |
index = (int16_t)(((int32_t)y * 64) / x);// calculate index for lookup table |
if (index < 0) index = -index; |
if (index < 346) angle = pgm_read_byte(&pgm_atanlookup[index]); // lookup for 0 deg to 79 deg |
else if (index > 7334) angle = 90; // limit is 90 deg |
else if (index > 2444) angle = 89; // 89 deg to 80 deg is mapped via intervalls |
else if (index > 1465) angle = 88; |
else if (index > 1046) angle = 87; |
else if (index > 813) angle = 86; |
else if (index > 664) angle = 85; |
else if (index > 561) angle = 84; |
else if (index > 486) angle = 83; |
else if (index > 428) angle = 82; |
else if (index > 382) angle = 81; |
else angle = 80; // (index>345) |
if (x > 0) return (angle * m); // 1st and 4th quadrant |
else if ((x < 0) && (m > 0)) return (180 - angle); // 2nd quadrant |
else return (angle - 180); // ( (x < 0) && (y < 0)) 3rd quadrant |
} |
// integer square root |
uint32_t c_sqrt(uint32_t number) |
{ |
if(!number) return 0; |
uint32_t s1, s2; |
uint8_t iter = 0; |
// initialization of iteration |
s2 = number; |
do // iterative formula to solve x^2 - n = 0 |
{ |
s1 = s2; |
s2 = number / s1; |
s2 += s1; |
s2 /= 2; |
iter++; |
//if(iter > 40) break; |
}while( ( (s1-s2) > 1) && (iter < 40)); |
return s2; |
} |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/mymath.h |
---|
0,0 → 1,11 |
#ifndef _MYMATH_H |
#define _MYMATH_H |
#include <inttypes.h> |
extern int16_t c_sin_8192(int16_t angle); |
extern int16_t c_cos_8192(int16_t angle); |
extern int16_t c_atan2(int16_t y, int16_t x); |
extern uint32_t c_sqrt(uint32_t number); |
#endif // _MYMATH_H |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/rc.c |
---|
0,0 → 1,209 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "main.h" |
volatile int16_t PPM_in[15]; //PPM24 supports 12 channels per frame |
volatile int16_t PPM_diff[15]; |
volatile uint8_t NewPpmData = 1; |
volatile int16_t RC_Quality = 0; |
/***************************************************************/ |
/* 16bit timer 1 is used to decode the PPM-Signal */ |
/***************************************************************/ |
void RC_Init (void) |
{ |
uint8_t sreg = SREG; |
// disable all interrupts before reconfiguration |
cli(); |
// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1 |
DDRD &= ~(1<<DDD6); |
PORTD |= (1<<PORTD6); |
// Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5) |
// set as output |
DDRD |= (1<<DDD5)|(1<<DDD4); |
// low level |
PORTD &= ~((1<<PORTD5)|(1<<PORTD4)); |
// PD3 can't be used if 2nd UART is activated |
// because TXD1 is at that port |
if(CPUType != ATMEGA644P) |
{ |
DDRD |= (1<<PORTD3); |
PORTD &= ~(1<<PORTD3); |
} |
// Timer/Counter1 Control Register A, B, C |
// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0) |
// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0) |
// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1) |
// Enable input capture noise cancler (bit: ICNC1=1) |
// Trigger on positive edge of the input capture pin (bit: ICES1=1), |
// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs |
// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s. |
TCCR1A &= ~((1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM11)|(1<<WGM10)); |
TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12)); |
TCCR1B |= (1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1); |
TCCR1C &= ~((1<<FOC1A)|(1<<FOC1B)); |
// Timer/Counter1 Interrupt Mask Register |
// Enable Input Capture Interrupt (bit: ICIE1=1) |
// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0) |
// Enable Overflow Interrupt (bit: TOIE1=0) |
TIMSK1 &= ~((1<<OCIE1B)|(1<<OCIE1A)|(1<<TOIE1)); |
TIMSK1 |= (1<<ICIE1); |
RC_Quality = 0; |
SREG = sreg; |
} |
/********************************************************************/ |
/* Every time a positive edge is detected at PD6 */ |
/********************************************************************/ |
/* t-Frame |
<-----------------------------------------------------------------------> |
____ ______ _____ ________ ______ sync gap ____ |
| | | | | | | | | | | |
| | | | | | | | | | | |
___| |_| |_| |_| |_.............| |________________| |
<-----><-------><------><--------> <------> <--- |
t0 t1 t2 t4 tn t0 |
The PPM-Frame length is 22.5 ms. |
Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse. |
The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms. |
The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms. |
The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms. |
The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms. |
The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is |
the syncronization gap. |
*/ |
ISR(TIMER1_CAPT_vect) // typical rate of 1 ms to 2 ms |
{ |
int16_t signal = 0, tmp; |
static int16_t index; |
static uint16_t oldICR1 = 0; |
// 16bit Input Capture Register ICR1 contains the timer value TCNT1 |
// at the time the edge was detected |
// calculate the time delay to the previous event time which is stored in oldICR1 |
// calculatiing the difference of the two uint16_t and converting the result to an int16_t |
// implicit handles a timer overflow 65535 -> 0 the right way. |
signal = (uint16_t) ICR1 - oldICR1; |
oldICR1 = ICR1; |
//sync gap? (3.52 ms < signal < 25.6 ms) |
if((signal > 1100) && (signal < 8000)) |
{ |
// if a sync gap happens and there where at least 4 channels decoded before |
// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array. |
if(index >= 4) |
{ |
NewPpmData = 0; // Null means NewData for the first 4 channels |
} |
// synchronize channel index |
index = 1; |
} |
else // within the PPM frame |
{ |
if(index < 14) // PPM24 supports 12 channels |
{ |
// check for valid signal length (0.8 ms < signal < 2.1984 ms) |
// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
if((signal > 250) && (signal < 687)) |
{ |
// shift signal to zero symmetric range -154 to 159 |
signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
// check for stable signal |
if(abs(signal - PPM_in[index]) < 6) |
{ |
if(RC_Quality < 200) RC_Quality +=10; |
else RC_Quality = 200; |
} |
// calculate exponential history for signal |
tmp = (3 * (PPM_in[index]) + signal) / 4; |
if(tmp > signal+1) tmp--; else |
if(tmp < signal-1) tmp++; |
// calculate signal difference on good signal level |
if(RC_Quality >= 195) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
else PPM_diff[index] = 0; |
PPM_in[index] = tmp; // update channel value |
} |
index++; // next channel |
// demux sum signal for channels 5 to 7 to J3, J4, J5 |
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; |
} |
} |
} |
} |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/rc.h |
---|
0,0 → 1,25 |
#ifndef _RC_H |
#define _RC_H |
#include <inttypes.h> |
#define J3HIGH PORTD |= (1<<PORTD5) |
#define J3LOW PORTD &= ~(1<<PORTD5) |
#define J3TOGGLE PORTD ^= (1<<PORTD5) |
#define J4HIGH PORTD |= (1<<PORTD4) |
#define J4LOW PORTD &= ~(1<<PORTD4) |
#define J4TOGGLE PORTD ^= (1<<PORTD4) |
#define J5HIGH PORTD |= (1<<PORTD3) |
#define J5LOW PORTD &= ~(1<<PORTD3) |
#define J5TOGGLE PORTD ^= (1<<PORTD3) |
extern void RC_Init (void); |
extern volatile int16_t PPM_in[15]; // the RC-Signal |
extern volatile int16_t PPM_diff[15]; // the differentiated RC-Signal |
extern volatile uint8_t NewPpmData; // 0 indicates a new recieved PPM Frame |
extern volatile int16_t RC_Quality; // rc signal quality indicator (0 to 200) |
#endif //_RC_H |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/spectrum.c |
---|
0,0 → 1,310 |
/*####################################################################################### |
Decodieren eines RC Summen Signals oder Spektrum Empfänger-Satellit |
#######################################################################################*/ |
#include "Spectrum.h" |
#include "main.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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/spi.c |
---|
0,0 → 1,458 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "main.h" |
#include "spi.h" |
#include "fc.h" |
#include "rc.h" |
#include "eeprom.h" |
#include "uart0.h" |
#include "timer0.h" |
#include "analog.h" |
//----------------------------------------- |
#define DDR_SPI DDRB |
#define DD_SS PB4 |
#define DD_SCK PB7 |
#define DD_MOSI PB5 |
#define DD_MISO PB6 |
// for compatibility reasons gcc3.x <-> gcc4.x |
#ifndef SPCR |
#define SPCR SPCR0 |
#endif |
#ifndef SPIE |
#define SPIE SPIE0 |
#endif |
#ifndef SPE |
#define SPE SPE0 |
#endif |
#ifndef DORD |
#define DORD DORD0 |
#endif |
#ifndef MSTR |
#define MSTR MSTR0 |
#endif |
#ifndef CPOL |
#define CPOL CPOL0 |
#endif |
#ifndef CPHA |
#define CPHA CPHA0 |
#endif |
#ifndef SPR1 |
#define SPR1 SPR01 |
#endif |
#ifndef SPR0 |
#define SPR0 SPR00 |
#endif |
#ifndef SPDR |
#define SPDR SPDR0 |
#endif |
#ifndef SPSR |
#define SPSR SPSR0 |
#endif |
#ifndef SPIF |
#define SPIF SPIF0 |
#endif |
#ifndef WCOL |
#define WCOL WCOL0 |
#endif |
#ifndef SPI2X |
#define SPI2X SPI2X0 |
#endif |
// ------------------------- |
#define SLAVE_SELECT_DDR_PORT DDRC |
#define SLAVE_SELECT_PORT PORTC |
#define SPI_SLAVE_SELECT PC5 |
#define SPI_TXSYNCBYTE1 0xAA |
#define SPI_TXSYNCBYTE2 0x83 |
#define SPI_RXSYNCBYTE1 0x81 |
#define SPI_RXSYNCBYTE2 0x55 |
typedef enum |
{ |
SPI_SYNC1, |
SPI_SYNC2, |
SPI_DATA |
} SPI_RXState_t; |
// data exchange packets to and From NaviCtrl |
ToNaviCtrl_t ToNaviCtrl; |
FromNaviCtrl_t FromNaviCtrl; |
SPI_VersionInfo_t SPI_VersionInfo; |
// rx packet buffer |
#define SPI_RXBUFFER_LEN sizeof(FromNaviCtrl) |
uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN]; |
uint8_t SPI_RxBufferIndex = 0; |
uint8_t SPI_RxBuffer_Request = 0; |
// tx packet buffer |
#define SPI_TXBUFFER_LEN sizeof(ToNaviCtrl) |
uint8_t *SPI_TxBuffer; |
uint8_t SPI_TxBufferIndex = 0; |
uint8_t SPITransferCompleted, SPI_ChkSum; |
uint8_t SPI_RxDataValid = 0; |
uint8_t NCDataOkay = 0; |
uint8_t NCSerialDataOkay = 0; |
uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION }; |
uint8_t SPI_CommandCounter = 0; |
/*********************************************/ |
/* Initialize SPI interface to NaviCtrl */ |
/*********************************************/ |
void SPI_MasterInit(void) |
{ |
DDR_SPI |= (1<<DD_MOSI)|(1<<DD_SCK); // Set MOSI and SCK output, all others input |
SLAVE_SELECT_DDR_PORT |= (1 << SPI_SLAVE_SELECT); // set Slave select port as output port |
SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1)|(0<<SPR0)|(0<<SPIE); // Enable SPI, Master, set clock rate fck/64 |
SPSR = 0;//(1<<SPI2X); |
SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // Deselect Slave |
SPI_TxBuffer = (uint8_t *) &ToNaviCtrl; // set pointer to tx-buffer |
SPITransferCompleted = 1; |
// initialize data packet to NaviControl |
ToNaviCtrl.Sync1 = SPI_TXSYNCBYTE1; |
ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2; |
ToNaviCtrl.Command = SPI_CMD_USER; |
ToNaviCtrl.IntegralNick = 0; |
ToNaviCtrl.IntegralRoll = 0; |
NCSerialDataOkay = 0; |
NCDataOkay = 0; |
SPI_RxDataValid = 0; |
SPI_VersionInfo.Major = VERSION_MAJOR; |
SPI_VersionInfo.Minor = VERSION_MINOR; |
SPI_VersionInfo.Patch = VERSION_PATCH; |
SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE; |
} |
/**********************************************************/ |
/* Update Data transferd by the SPI from/to NaviCtrl */ |
/**********************************************************/ |
void UpdateSPI_Buffer(void) |
{ |
int16_t tmp; |
cli(); // stop all interrupts to avoid writing of new data during update of that packet. |
// update content of packet to NaviCtrl |
ToNaviCtrl.IntegralNick = (int16_t)((10 * IntegralGyroNick) / GYRO_DEG_FACTOR); // convert to multiple of 0.1° |
ToNaviCtrl.IntegralRoll = (int16_t)((10 * IntegralGyroRoll) / GYRO_DEG_FACTOR); // convert to multiple of 0.1° |
ToNaviCtrl.GyroHeading = (int16_t)((10 * YawGyroHeading) / GYRO_DEG_FACTOR); // convert to multiple of 0.1° |
ToNaviCtrl.GyroNick = GyroNick; |
ToNaviCtrl.GyroRoll = GyroRoll; |
ToNaviCtrl.GyroYaw = GyroYaw; |
ToNaviCtrl.AccNick = ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
ToNaviCtrl.AccRoll = ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0; |
switch(ToNaviCtrl.Command) |
{ |
case SPI_CMD_USER: |
ToNaviCtrl.Param.Byte[0] = FCParam.UserParam1; |
ToNaviCtrl.Param.Byte[1] = FCParam.UserParam2; |
ToNaviCtrl.Param.Byte[2] = FCParam.UserParam3; |
ToNaviCtrl.Param.Byte[3] = FCParam.UserParam4; |
ToNaviCtrl.Param.Byte[4] = FCParam.UserParam5; |
ToNaviCtrl.Param.Byte[5] = FCParam.UserParam6; |
ToNaviCtrl.Param.Byte[6] = FCParam.UserParam7; |
ToNaviCtrl.Param.Byte[7] = FCParam.UserParam8; |
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] = ParamSet.LowVoltageWarning; |
ToNaviCtrl.Param.Byte[11] = GetActiveParamSet(); |
break; |
case SPI_CMD_PARAMETER1: |
ToNaviCtrl.Param.Byte[0] = ParamSet.NaviGpsModeControl; // Parameters for the Naviboard |
ToNaviCtrl.Param.Byte[1] = ParamSet.NaviGpsGain; |
ToNaviCtrl.Param.Byte[2] = ParamSet.NaviGpsP; |
ToNaviCtrl.Param.Byte[3] = ParamSet.NaviGpsI; |
ToNaviCtrl.Param.Byte[4] = ParamSet.NaviGpsD; |
ToNaviCtrl.Param.Byte[5] = ParamSet.NaviGpsACC; |
ToNaviCtrl.Param.Byte[6] = ParamSet.NaviGpsMinSat; |
ToNaviCtrl.Param.Byte[7] = ParamSet.NaviStickThreshold; |
ToNaviCtrl.Param.Byte[8] = ParamSet.NaviOperatingRadius; |
ToNaviCtrl.Param.Byte[9] = ParamSet.NaviWindCorrection; |
ToNaviCtrl.Param.Byte[10] = ParamSet.NaviSpeedCompensation; |
ToNaviCtrl.Param.Byte[11] = ParamSet.NaviAngleLimitation; |
break; |
case SPI_CMD_STICK: |
tmp = PPM_in[ParamSet.ChannelAssignment[CH_GAS]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
ToNaviCtrl.Param.Byte[0] = (int8_t) tmp; |
tmp = PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
ToNaviCtrl.Param.Byte[1] = (int8_t) tmp; |
tmp = PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
ToNaviCtrl.Param.Byte[2] = (int8_t) tmp; |
tmp = PPM_in[ParamSet.ChannelAssignment[CH_NICK]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
ToNaviCtrl.Param.Byte[3] = (int8_t) tmp; |
ToNaviCtrl.Param.Byte[4] = (uint8_t) Poti1; |
ToNaviCtrl.Param.Byte[5] = (uint8_t) Poti2; |
ToNaviCtrl.Param.Byte[6] = (uint8_t) Poti3; |
ToNaviCtrl.Param.Byte[7] = (uint8_t) Poti4; |
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] = ParamSet.NaviPHLoginTime; |
ToNaviCtrl.Param.Int[1] = ReadingHeight; // at address of Byte 2 and 3 |
ToNaviCtrl.Param.Byte[4] = ParamSet.NaviGpsPLimit; |
ToNaviCtrl.Param.Byte[5] = ParamSet.NaviGpsILimit; |
ToNaviCtrl.Param.Byte[6] = ParamSet.NaviGpsDLimit; |
break; |
case SPI_CMD_VERSION: |
ToNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major; |
ToNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor; |
ToNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch; |
ToNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible; |
ToNaviCtrl.Param.Byte[4] = BoardRelease; |
break; |
default: |
break; |
} |
sei(); // enable all interrupts |
// analyze content of packet from NaviCtrl if valid |
if (SPI_RxDataValid) |
{ |
// update gps controls |
if(abs(FromNaviCtrl.GPSStickNick) < 512 && abs(FromNaviCtrl.GPSStickRoll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE)) |
{ |
GPSStickNick = FromNaviCtrl.GPSStickNick; |
GPSStickRoll = FromNaviCtrl.GPSStickRoll; |
NCDataOkay = 250; |
} |
// update compass readings |
if(FromNaviCtrl.CompassHeading <= 360) |
{ |
CompassHeading = FromNaviCtrl.CompassHeading; |
} |
if(CompassHeading < 0) CompassOffCourse = 0; |
else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
// NaviCtrl wants to beep? |
if (FromNaviCtrl.BeepTime > BeepTime && !CompassCalState) BeepTime = FromNaviCtrl.BeepTime; |
switch (FromNaviCtrl.Command) |
{ |
case SPI_KALMAN: |
FCParam.KalmanK = FromNaviCtrl.Param.Byte[0]; |
FCParam.KalmanMaxFusion = FromNaviCtrl.Param.Byte[1]; |
FCParam.KalmanMaxDrift = FromNaviCtrl.Param.Byte[2]; |
NCSerialDataOkay = FromNaviCtrl.Param.Byte[3]; |
DebugOut.Analog[29] = NCSerialDataOkay; |
break; |
default: |
break; |
} |
} |
else // no valid data from NaviCtrl |
{ |
// disable GPS control |
GPSStickNick = 0; |
GPSStickRoll = 0; |
} |
} |
/*********************************************/ |
/* Start Transmission of packet to NaviCtrl */ |
/*********************************************/ |
void SPI_StartTransmitPacket(void) |
{ |
if (!SPITransferCompleted) return; // return immediately if transfer is in progress |
else // transmission was completed |
{ |
SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // Select slave |
// cyclic commands |
ToNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
SPITransferCompleted = 0; // transfer is in progress |
UpdateSPI_Buffer(); // update data in ToNaviCtrl |
SPI_TxBufferIndex = 1; //proceed with 2nd byte |
// -- Debug-Output --- |
//---- |
asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
ToNaviCtrl.Chksum = ToNaviCtrl.Sync1; // init checksum |
SPDR = ToNaviCtrl.Sync1; // send first byte |
} |
} |
//------------------------------------------------------ |
// This is the spi data transfer between FlightCtrl and NaviCtrl |
// Every time this routine is called within the mainloop one byte of the packet to |
// the NaviCtrl and one byte of the packet from the NaviCtrl is possible transfered |
void SPI_TransmitByte(void) |
{ |
static SPI_RXState_t SPI_RXState = SPI_SYNC1; |
uint8_t rxdata; |
static uint8_t rxchksum; |
if (SPITransferCompleted) return; // return immediatly if transfer was completed |
if (!(SPSR & (1 << SPIF))) return; // return if no SPI-IRQ pending |
SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop |
SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave |
rxdata = SPDR; // save spi data register |
switch (SPI_RXState) |
{ |
case SPI_SYNC1: // first sync byte |
SPI_RxBufferIndex = 0; // set pointer to start of rx buffer |
rxchksum = rxdata; // initialize checksum |
if (rxdata == SPI_RXSYNCBYTE1 ) |
{ // 1st Syncbyte found |
SPI_RXState = SPI_SYNC2; // trigger to state for second sync byte |
} |
break; |
case SPI_SYNC2: // second sync byte |
if (rxdata == SPI_RXSYNCBYTE2) |
{ // 2nd Syncbyte found |
rxchksum += rxdata; // update checksum |
SPI_RXState = SPI_DATA; // trigger to state for second sync byte |
} |
else // 2nd Syncbyte not found |
{ |
SPI_RXState = SPI_SYNC1; // jump back to 1st sync byte |
} |
break; |
case SPI_DATA: // data bytes |
SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata; // copy data byte to spi buffer |
// if all bytes are received of a packet from the NaviCtrl |
if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) |
{ // last byte transfered is the checksum of the packet |
if (rxdata == rxchksum) // checksum matching? |
{ |
// copy SPI_RxBuffer -> FromFlightCtrl |
uint8_t *ptr = (uint8_t *)&FromNaviCtrl; |
cli(); |
memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(FromNaviCtrl)); |
sei(); |
SPI_RxDataValid = 1; |
//DebugOut.Analog[18]++; |
} |
else |
{ // checksum does not match |
//DebugOut.Analog[17]++; |
SPI_RxDataValid = 0; // reset valid flag |
} |
SPI_RXState = SPI_SYNC1; // reset state sync |
} |
else // not all bytes transfered |
{ |
rxchksum += rxdata; // update checksum |
} |
break; |
}// eof switch(SPI_RXState) |
// if still some bytes left for transmission to NaviCtrl |
if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) |
{ |
SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // SelectSlave |
asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte |
ToNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent |
SPI_TxBufferIndex++; |
} |
else |
{ |
//Transfer of all bytes of the packet to NaviCtrl completed |
SPITransferCompleted = 1; |
} |
} |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/spi.h |
---|
0,0 → 1,100 |
// ######################## SPI - FlightCtrl ################### |
#ifndef _SPI_H |
#define _SPI_H |
//#include <util/delay.h> |
#include <inttypes.h> |
#define SPI_PROTOCOL_COMP 1 |
#define SPI_CMD_USER 10 |
#define SPI_CMD_STICK 11 |
#define SPI_CMD_MISC 12 |
#define SPI_CMD_PARAMETER1 13 |
#define SPI_CMD_VERSION 14 |
typedef struct |
{ |
uint8_t Sync1; |
uint8_t Sync2; |
uint8_t Command; |
int16_t IntegralNick; |
int16_t IntegralRoll; |
int16_t AccNick; |
int16_t AccRoll; |
int16_t GyroHeading; |
int16_t GyroNick; |
int16_t GyroRoll; |
int16_t GyroYaw; |
union |
{ |
int8_t sByte[12]; |
uint8_t Byte[12]; |
int16_t Int[6]; |
int32_t Long[3]; |
float Float[3]; |
} Param; |
uint8_t Chksum; |
} __attribute__((packed)) ToNaviCtrl_t; |
#define SPI_CMD_OSD_DATA 100 |
#define SPI_CMD_GPS_POS 101 |
#define SPI_CMD_GPS_TARGET 102 |
#define SPI_KALMAN 103 |
typedef struct |
{ |
uint8_t Command; |
int16_t GPSStickNick; |
int16_t GPSStickRoll; |
int16_t GPS_Yaw; |
int16_t CompassHeading; |
int16_t Status; |
uint16_t BeepTime; |
union |
{ |
int8_t Byte[12]; |
int16_t Int[6]; |
int32_t Long[3]; |
float Float[3]; |
} Param; |
uint8_t Chksum; |
} __attribute__((packed)) FromNaviCtrl_t; |
typedef struct |
{ |
uint8_t Major; |
uint8_t Minor; |
uint8_t Patch; |
uint8_t Compatible; |
} __attribute__((packed)) SPI_VersionInfo_t; |
extern ToNaviCtrl_t ToNaviCtrl; |
extern FromNaviCtrl_t FromNaviCtrl; |
typedef struct |
{ |
int8_t KalmanK; |
int8_t KalmanMaxDrift; |
int8_t KalmanMaxFusion; |
uint8_t SerialDataOkay; |
} __attribute__((packed)) NCData_t; |
extern uint8_t NCDataOkay; |
extern uint8_t NCSerialDataOkay; |
void SPI_MasterInit(void); |
void SPI_StartTransmitPacket(void); |
void SPI_TransmitByte(void); |
#endif //_SPI_H |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/timer0.c |
---|
0,0 → 1,239 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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" |
#include "main.h" |
#include "fc.h" |
#ifdef USE_KILLAGREG |
#include "mm3.h" |
#endif |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#endif |
volatile uint16_t CountMilliseconds = 0; |
volatile uint8_t UpdateMotor = 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) // 9.765 kHz |
{ |
static uint8_t cnt_1ms = 1,cnt = 0; |
uint8_t Beeper_On = 0; |
#ifdef USE_NAVICTRL |
if(SendSPI) SendSPI--; // if SendSPI is 0, the transmit of a byte via SPI bus to and from The Navicontrol is done |
#endif |
if(!cnt--) // every 10th run (9.765kHz/10 = 976Hz) |
{ |
cnt = 9; |
cnt_1ms++; |
cnt_1ms %= 2; |
if(!cnt_1ms) UpdateMotor = 1; // every 2nd run (976Hz/2 = 488 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(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) |
{ |
#ifdef USE_KILLAGREG |
MM3_Update(); // read out mm3 board |
#endif |
#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; |
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(ADReady) |
{ |
ADReady = 0; |
ADC_Enable(); |
} |
} |
} |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/timer0.h |
---|
0,0 → 1,21 |
#ifndef _TIMER0_H |
#define _TIMER0_H |
#include <inttypes.h> |
extern volatile uint16_t CountMilliseconds; |
extern volatile uint8_t UpdateMotor; |
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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/timer2.c |
---|
0,0 → 1,351 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "fc.h" |
#include "eeprom.h" |
#include "uart0.h" |
#include "main.h" |
#include "rc.h" |
volatile int16_t ServoNickValue = 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 nick compensation. |
void TIMER2_Init(void) |
{ |
uint8_t sreg = SREG; |
// disable all interrupts before reconfiguration |
cli(); |
// set PD7 as output of the PWM for nick 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 IRS_RUNTIME 127 |
#define PPM_STOPPULSE 188 |
//#define PPM_FRAMELEN 14063 |
#define PPM_FRAMELEN (1757 * ParamSet.ServoRefresh) // 22.5 ms / 8 Channels = 2.8125ms per Servo Channel |
#define MINSERVOPULSE 375 |
#define MAXSERVOPULSE 1500 |
#define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE) |
static uint8_t PulseOutput = 0; |
static uint16_t RemainingPulse = 0; |
static uint16_t ServoFrameTime = 0; |
static uint8_t ServoIndex = 0; |
#define MULTIPLYER 4 |
static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center position |
static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center position |
if(BoardRelease < 20) |
{ |
//--------------------------- |
// Nick servo state machine |
//--------------------------- |
if(!PulseOutput) // pulse output complete |
{ |
if(TCCR2A & (1<<COM2A0)) // we had a low pulse |
{ |
TCCR2A &= ~(1<<COM2A0);// make a high pulse |
RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
ServoNickOffset = (ServoNickOffset * 3 + (int16_t)FCParam.ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
if(ParamSet.ServoNickCompInvert & 0x01) |
{ // inverting movement of servo |
ServoNickValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
} |
else |
{ // non inverting movement of servo |
ServoNickValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
} |
// limit servo value to its parameter range definition |
if(ServoNickValue < ((int16_t)ParamSet.ServoNickMin * MULTIPLYER) ) |
{ |
ServoNickValue = (int16_t)ParamSet.ServoNickMin * MULTIPLYER; |
} |
else |
if(ServoNickValue > ((int16_t)ParamSet.ServoNickMax * MULTIPLYER) ) |
{ |
ServoNickValue = (int16_t)ParamSet.ServoNickMax * MULTIPLYER; |
} |
RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
ServoNickValue /= MULTIPLYER; |
DebugOut.Analog[20] = ServoNickValue; |
// range servo pulse width |
if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
// accumulate time for correct update rate |
ServoFrameTime = RemainingPulse; |
} |
else // we had a high pulse |
{ |
TCCR2A |= (1<<COM2A0); // make a low pulse |
RemainingPulse = PPM_FRAMELEN - ServoFrameTime; |
} |
// set pulse output active |
PulseOutput = 1; |
} |
} // EOF Nick servo state machine |
else |
{ |
//----------------------------------------------------- |
// PPM state machine, onboard demultiplexed by HEF4017 |
//----------------------------------------------------- |
if(!PulseOutput) // 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 |
{ |
RemainingPulse = 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 |
{ |
RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
switch(ServoIndex) // map servo channels |
{ |
case 1: // Nick Compensation Servo |
ServoNickOffset = (ServoNickOffset * 3 + (int16_t)FCParam.ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
if(ParamSet.ServoNickCompInvert & 0x01) |
{ // inverting movement of servo |
ServoNickValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
} |
else |
{ // non inverting movement of servo |
ServoNickValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
} |
// limit servo value to its parameter range definition |
if(ServoNickValue < ((int16_t)ParamSet.ServoNickMin * MULTIPLYER) ) |
{ |
ServoNickValue = (int16_t)ParamSet.ServoNickMin * MULTIPLYER; |
} |
else |
if(ServoNickValue > ((int16_t)ParamSet.ServoNickMax * MULTIPLYER) ) |
{ |
ServoNickValue = (int16_t)ParamSet.ServoNickMax * MULTIPLYER; |
} |
RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
ServoNickValue /= MULTIPLYER; |
DebugOut.Analog[20] = ServoNickValue; |
break; |
case 2: // Roll Compensation Servo |
ServoRollOffset = (ServoRollOffset * 3 + (int16_t)80 * MULTIPLYER) / 4; // lowpass offset |
ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
//if(ParamSet.ServoRollCompInvert & 0x01) |
{ // inverting movement of servo |
ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLYER * (IntegralGyroRoll / 128L ) ) / (256L) ); |
} |
/* else |
{ // non inverting movement of servo |
ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLYER * (IntegralGyroRoll / 128L ) ) / (256L) ); |
} |
*/ // limit servo value to its parameter range definition |
if(ServoRollValue < ((int16_t)ParamSet.ServoNickMin * MULTIPLYER) ) |
{ |
ServoRollValue = (int16_t)ParamSet.ServoNickMin * MULTIPLYER; |
} |
else |
if(ServoRollValue > ((int16_t)ParamSet.ServoNickMax * MULTIPLYER) ) |
{ |
ServoRollValue = (int16_t)ParamSet.ServoNickMax * MULTIPLYER; |
} |
RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoRollValue to center position |
ServoRollValue /= MULTIPLYER; |
//DebugOut.Analog[20] = ServoRollValue; |
break; |
default: // other servo channels |
RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs |
break; |
} |
// range servo pulse width |
if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
// substract stop pulse width |
RemainingPulse -= PPM_STOPPULSE; |
// accumulate time for correct sync gap |
ServoFrameTime += RemainingPulse; |
} |
} |
else // we had a high pulse |
{ |
TCCR2A |= (1<<COM2A0); // make a low pulse |
// set pulsewidth to stop pulse width |
RemainingPulse = PPM_STOPPULSE; |
// accumulate time for correct sync gap |
ServoFrameTime += RemainingPulse; |
if(ServoActive && RC_Quality > 180) HEF4017R_OFF; // disable HEF4017 reset |
ServoIndex++; // change to next servo channel |
if(ServoIndex > ParamSet.ServoRefresh) ServoIndex = 0; // reset to the sync gap |
} |
// set pulse output active |
PulseOutput = 1; |
} |
} // EOF PPM state machine |
// General pulse output generator |
if(RemainingPulse > (255 + IRS_RUNTIME)) |
{ |
OCR2A = 255; |
RemainingPulse -= 255; |
} |
else |
{ |
if(RemainingPulse > 255) // this is the 2nd last part |
{ |
if((RemainingPulse - 255) < IRS_RUNTIME) |
{ |
OCR2A = 255 - IRS_RUNTIME; |
RemainingPulse -= 255 - IRS_RUNTIME; |
} |
else // last part > ISR_RUNTIME |
{ |
OCR2A = 255; |
RemainingPulse -= 255; |
} |
} |
else // this is the last part |
{ |
OCR2A = RemainingPulse; |
RemainingPulse = 0; |
PulseOutput = 0; // trigger to stop pulse |
} |
} // EOF general pulse output generator |
} |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/twimaster.c |
---|
0,0 → 1,315 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "main.h" |
#include "eeprom.h" |
#include "twimaster.h" |
#include "fc.h" |
#include "analog.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]; |
#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_GAS] <= 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 Gyro-Offsets |
case 7: |
I2C_WriteByte(0x98); // Address the DAC |
break; |
case 8: |
I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C) |
break; |
case 9: |
switch(dac_channel) |
{ |
case 0: |
I2C_WriteByte(DacOffsetGyroNick); // 1st byte for Channel A |
break; |
case 1: |
I2C_WriteByte(DacOffsetGyroRoll); // 1st byte for Channel B |
break; |
case 2: |
I2C_WriteByte(DacOffsetGyroYaw ); // 1st byte for Channel C |
break; |
} |
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(dac_channel < 2) |
{ |
dac_channel ++; // jump to next channel |
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel |
} |
else |
{ // data to last motor send |
dac_channel = 0; // reset dac channel counter |
} |
break; |
default: |
I2C_Stop(TWI_STATE_MOTOR_TX); |
I2CTimeout = 10; |
motor_write = 0; |
motor_read = 0; |
} |
} |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/twimaster.h |
---|
0,0 → 1,37 |
#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 volatile uint8_t motor_write; |
+extern volatile uint8_t motor_read; |
+ |
+extern uint8_t MissingMotor; |
+ |
+#define MAX_MOTORS 12 |
+ |
+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; |
+ |
+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 |
+ |
+#endif |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/uart0.c |
---|
0,0 → 1,727 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + Nur für den privaten Gebrauch |
// + www.MikroKopter.com |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zulässig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/wdt.h> |
#include <avr/pgmspace.h> |
#include <stdarg.h> |
#include <string.h> |
#include "eeprom.h" |
#include "main.h" |
#include "menu.h" |
#include "timer0.h" |
#include "uart0.h" |
#include "fc.h" |
#include "rc.h" |
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG) |
#include "ubx.h" |
#endif |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#endif |
#define FC_ADDRESS 1 |
#define NC_ADDRESS 2 |
#define MK3MAG_ADDRESS 3 |
#define FALSE 0 |
#define TRUE 1 |
//int8_t test __attribute__ ((section (".noinit"))); |
uint8_t Request_VerInfo = FALSE; |
uint8_t Request_ExternalControl = FALSE; |
uint8_t Request_Display = FALSE; |
uint8_t Request_Display1 = FALSE; |
uint8_t Request_DebugData = FALSE; |
uint8_t Request_Data3D = FALSE; |
uint8_t Request_DebugLabel = 255; |
uint8_t Request_PPMChannels = FALSE; |
uint8_t Request_MotorTest = FALSE; |
uint8_t DisplayLine = 0; |
volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
volatile uint8_t rxd_buffer_locked = FALSE; |
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
volatile uint8_t txd_complete = TRUE; |
volatile uint8_t ReceivedBytes = 0; |
volatile uint8_t *pRxData = 0; |
volatile uint8_t RxDataLen = 0; |
uint8_t PcAccess = 100; |
uint8_t MotorTest_Active = 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; |
ExternControl_t ExternControl; |
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 |
"AngleNick ", //0 |
"AngleRoll ", |
"AccNick ", |
"AccRoll ", |
"YawGyro ", |
"Height Value ", //5 |
"AccZ ", |
"Gas ", |
"Compass Heading ", |
"Voltage ", |
"Receiver Level ", //10 |
"YawGyro Heading ", |
"Motor Front ", |
"Motor Rear ", |
"Motor Left ", |
"Motor Right ", //15 |
" ", |
" ", |
" ", |
"CompassCalState ", |
"NickServo ", //20 |
" ", |
" ", |
" ", |
" ", |
" ", //25 |
" ", |
"Kalman Max Drift", |
" ", |
"Navi Serial Data", |
"GPS Nick ", //30 |
"GPSS Roll " |
}; |
/****************************************************************/ |
/* Initialization of the USART0 */ |
/****************************************************************/ |
void USART0_Init (void) |
{ |
uint8_t sreg = SREG; |
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1); |
// disable all interrupts before configuration |
cli(); |
// disable RX-Interrupt |
UCSR0B &= ~(1 << RXCIE0); |
// disable TX-Interrupt |
UCSR0B &= ~(1 << TXCIE0); |
// set direction of RXD0 and TXD0 pins |
// set RXD0 (PD0) as an input pin |
PORTD |= (1 << PORTD0); |
DDRD &= ~(1 << DDD0); |
// set TXD0 (PD1) as an output pin |
PORTD |= (1 << PORTD1); |
DDRD |= (1 << DDD1); |
// USART0 Baud Rate Register |
// set clock divider |
UBRR0H = (uint8_t)(ubrr >> 8); |
UBRR0L = (uint8_t)ubrr; |
// USART0 Control and Status Register A, B, C |
// enable double speed operation in |
UCSR0A |= (1 << U2X0); |
// enable receiver and transmitter in |
UCSR0B = (1 << TXEN0) | (1 << RXEN0); |
// set asynchronous mode |
UCSR0C &= ~(1 << UMSEL01); |
UCSR0C &= ~(1 << UMSEL00); |
// no parity |
UCSR0C &= ~(1 << UPM01); |
UCSR0C &= ~(1 << UPM00); |
// 1 stop bit |
UCSR0C &= ~(1 << USBS0); |
// 8-bit |
UCSR0B &= ~(1 << UCSZ02); |
UCSR0C |= (1 << UCSZ01); |
UCSR0C |= (1 << UCSZ00); |
// flush receive buffer |
while ( UCSR0A & (1<<RXC0) ) UDR0; |
// enable interrupts at the end |
// enable RX-Interrupt |
UCSR0B |= (1 << RXCIE0); |
// enable TX-Interrupt |
UCSR0B |= (1 << TXCIE0); |
// initialize the debug timer |
DebugData_Timer = SetDelay(DebugData_Interval); |
// unlock rxd_buffer |
rxd_buffer_locked = FALSE; |
pRxData = 0; |
RxDataLen = 0; |
// no bytes to send |
txd_complete = TRUE; |
#ifdef USE_MK3MAG |
Compass_Timer = SetDelay(220); |
#endif |
UART_VersionInfo.SWMajor = VERSION_MAJOR; |
UART_VersionInfo.SWMinor = VERSION_MINOR; |
UART_VersionInfo.SWPatch = VERSION_PATCH; |
UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR; |
UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR; |
// restore global interrupt flags |
SREG = sreg; |
} |
/****************************************************************/ |
/* USART0 transmitter ISR */ |
/****************************************************************/ |
ISR(USART0_TX_vect) |
{ |
static uint16_t ptr_txd_buffer = 0; |
uint8_t tmp_tx; |
if(!txd_complete) // transmission not completed |
{ |
ptr_txd_buffer++; // die [0] wurde schon gesendet |
tmp_tx = txd_buffer[ptr_txd_buffer]; |
// if terminating character or end of txd buffer was reached |
if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN)) |
{ |
ptr_txd_buffer = 0; // reset txd pointer |
txd_complete = 1; // stop transmission |
} |
UDR0 = tmp_tx; // send current byte will trigger this ISR again |
} |
// transmission completed |
else ptr_txd_buffer = 0; |
} |
/****************************************************************/ |
/* USART0 receiver ISR */ |
/****************************************************************/ |
ISR(USART0_RX_vect) |
{ |
static uint16_t crc; |
static uint8_t ptr_rxd_buffer = 0; |
uint8_t crc1, crc2; |
uint8_t c; |
c = UDR0; // catch the received byte |
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
// If the cpu is not an Atmega644P the ublox module should be conneced to rxd of the 1st uart. |
if(CPUType != ATMEGA644P) ubx_parser(c); |
#endif |
if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return |
// the rxd buffer is unlocked |
if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received |
{ |
rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer |
crc = c; // init crc |
} |
#if 0 |
else if (ptr_rxd_buffer == 1) // handle address |
{ |
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
crc += c; // update crc |
} |
#endif |
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes |
{ |
if(c != '\r') // no termination character |
{ |
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
crc += c; // update crc |
} |
else // termination character was received |
{ |
// the last 2 bytes are no subject for checksum calculation |
// they are the checksum itself |
crc -= rxd_buffer[ptr_rxd_buffer-2]; |
crc -= rxd_buffer[ptr_rxd_buffer-1]; |
// calculate checksum from transmitted data |
crc %= 4096; |
crc1 = '=' + crc / 64; |
crc2 = '=' + crc % 64; |
// compare checksum to transmitted checksum bytes |
if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1])) |
{ // checksum valid |
rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes |
rxd_buffer_locked = TRUE; // lock the rxd buffer |
// if 2nd byte is an 'R' enable watchdog that will result in an reset |
if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando |
} |
else |
{ // checksum invalid |
rxd_buffer_locked = FALSE; // unlock rxd buffer |
} |
ptr_rxd_buffer = 0; // reset rxd buffer pointer |
} |
} |
else // rxd buffer overrun |
{ |
ptr_rxd_buffer = 0; // reset rxd buffer |
rxd_buffer_locked = FALSE; // unlock rxd buffer |
} |
} |
// -------------------------------------------------------------------------- |
void AddCRC(uint16_t datalen) |
{ |
uint16_t tmpCRC = 0, i; |
for(i = 0; i < datalen; i++) |
{ |
tmpCRC += txd_buffer[i]; |
} |
tmpCRC %= 4096; |
txd_buffer[i++] = '=' + tmpCRC / 64; |
txd_buffer[i++] = '=' + tmpCRC % 64; |
txd_buffer[i++] = '\r'; |
txd_complete = FALSE; |
UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
} |
// -------------------------------------------------------------------------- |
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ... |
{ |
va_list ap; |
uint16_t pt = 0; |
uint8_t a,b,c; |
uint8_t ptr = 0; |
uint8_t *pdata = 0; |
int len = 0; |
txd_buffer[pt++] = '#'; // Start character |
txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...) |
txd_buffer[pt++] = cmd; // Command |
va_start(ap, numofbuffers); |
if(numofbuffers) |
{ |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
while(len) |
{ |
if(len) |
{ |
a = pdata[ptr++]; |
len--; |
if((!len) && numofbuffers) |
{ |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} |
else a = 0; |
if(len) |
{ |
b = pdata[ptr++]; |
len--; |
if((!len) && numofbuffers) |
{ |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} |
else b = 0; |
if(len) |
{ |
c = pdata[ptr++]; |
len--; |
if((!len) && numofbuffers) |
{ |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} |
else c = 0; |
txd_buffer[pt++] = '=' + (a >> 2); |
txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
txd_buffer[pt++] = '=' + ( c & 0x3f); |
} |
va_end(ap); |
AddCRC(pt); // add checksum after data block and initates the transmission |
} |
// -------------------------------------------------------------------------- |
void Decode64(void) |
{ |
uint8_t a,b,c,d; |
uint8_t x,y,z; |
uint8_t ptrIn = 3; |
uint8_t ptrOut = 3; |
uint8_t len = ReceivedBytes - 6; |
while(len) |
{ |
a = rxd_buffer[ptrIn++] - '='; |
b = rxd_buffer[ptrIn++] - '='; |
c = rxd_buffer[ptrIn++] - '='; |
d = rxd_buffer[ptrIn++] - '='; |
//if(ptrIn > ReceivedBytes - 3) break; |
x = (a << 2) | (b >> 4); |
y = ((b & 0x0f) << 4) | (c >> 2); |
z = ((c & 0x03) << 6) | d; |
if(len--) rxd_buffer[ptrOut++] = x; else break; |
if(len--) rxd_buffer[ptrOut++] = y; else break; |
if(len--) rxd_buffer[ptrOut++] = z; else break; |
} |
pRxData = &rxd_buffer[3]; |
RxDataLen = ptrOut - 3; |
} |
// -------------------------------------------------------------------------- |
void USART0_ProcessRxData(void) |
{ |
// if data in the rxd buffer are not locked immediately return |
if(!rxd_buffer_locked) return; |
uint8_t tempchar1, tempchar2; |
Decode64(); // decode data block in rxd_buffer |
switch(rxd_buffer[1] - 'a') |
{ |
case FC_ADDRESS: |
switch(rxd_buffer[2]) |
{ |
#ifdef USE_MK3MAG |
case 'K':// compass value |
CompassHeading = ((Heading_t *)pRxData)->Heading; |
CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
break; |
#endif |
case 't':// motor test |
if(RxDataLen > 20) // |
{ |
memcpy(&MotorTest[0], (uint8_t*)pRxData, sizeof(MotorTest)); |
} |
else |
{ |
memcpy(&MotorTest[0], (uint8_t*)pRxData, 4); |
} |
//Request_MotorTest = TRUE; |
MotorTest_Active = 255; |
PcAccess = 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 *) &ParamSet, sizeof(ParamSet)); |
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(&ParamSet, (uint8_t*)&pRxData[2], sizeof(ParamSet)); |
ParamSet_WriteToEEProm(pRxData[0]); |
TurnOver180Nick = (int32_t) ParamSet.AngleTurnOverNick * 2500L; |
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
tempchar1 = GetActiveParamSet(); |
Beep(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; |
PcAccess = 255; |
break; |
case 'b': // submit extern control |
memcpy(&ExternControl, (uint8_t*)pRxData, sizeof(ExternControl)); |
ConfirmFrame = ExternControl.Frame; |
PcAccess = 255; |
break; |
case 'h':// request for display columns |
PcAccess = 255; |
RemoteKeys |= pRxData[0]; |
if(RemoteKeys) DisplayLine = 0; |
Request_Display = TRUE; |
break; |
case 'l':// request for display columns |
PcAccess = 255; |
MenuItem = pRxData[0]; |
Request_Display1 = TRUE; |
break; |
case 'v': // request for version and board release |
Request_VerInfo = TRUE; |
break; |
case 'g':// get external control data |
Request_ExternalControl = TRUE; |
break; |
case 'd': // request for the debug data |
DebugData_Interval = (uint16_t) pRxData[0] * 10; |
if(DebugData_Interval > 0) Request_DebugData = TRUE; |
break; |
case 'c': // request for the 3D data |
Data3D_Interval = (uint16_t) pRxData[0] * 10; |
if(Data3D_Interval > 0) Request_Data3D = TRUE; |
break; |
default: |
//unsupported command received |
break; |
} |
break; // default: |
} |
// unlock the rxd buffer after processing |
pRxData = 0; |
RxDataLen = 0; |
rxd_buffer_locked = FALSE; |
} |
//############################################################################ |
//Routine für die Serielle Ausgabe |
int16_t uart_putchar (int8_t c) |
//############################################################################ |
{ |
if (c == '\n') |
uart_putchar('\r'); |
// wait until previous character was send |
loop_until_bit_is_set(UCSR0A, UDRE0); |
// send character |
UDR0 = c; |
return (0); |
} |
//--------------------------------------------------------------------------------------------- |
void USART0_TransmitTxData(void) |
{ |
if(!txd_complete) return; |
if(Request_VerInfo && txd_complete) |
{ |
SendOutData('V', FC_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, sizeof(UART_VersionInfo)); |
Request_VerInfo = FALSE; |
} |
if(Request_Display && txd_complete) |
{ |
LCD_PrintMenu(); |
SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20); |
DisplayLine++; |
if(DisplayLine >= 4) DisplayLine = 0; |
Request_Display = FALSE; |
} |
if(Request_Display1 && txd_complete) |
{ |
LCD_PrintMenu(); |
SendOutData('L', FC_ADDRESS, 3, &MenuItem, sizeof(MenuItem), &MaxMenuItem, sizeof(MaxMenuItem), DisplayBuff, sizeof(DisplayBuff)); |
Request_Display1 = FALSE; |
} |
if(Request_DebugLabel != 0xFF) // Texte für die Analogdaten |
{ |
uint8_t label[16]; // local sram buffer |
memcpy_P(label, ANALOG_LABEL[Request_DebugLabel], 16); // read lable from flash to sram buffer |
SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &Request_DebugLabel, sizeof(Request_DebugLabel), label, 16); |
Request_DebugLabel = 0xFF; |
} |
if(ConfirmFrame && txd_complete) // Datensatz ohne CRC bestätigen |
{ |
SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame)); |
ConfirmFrame = 0; |
} |
if( ((DebugData_Interval && CheckDelay(DebugData_Timer)) || Request_DebugData) && txd_complete) |
{ |
SendOutData('D', FC_ADDRESS, 1,(uint8_t *) &DebugOut, sizeof(DebugOut)); |
DebugData_Timer = SetDelay(DebugData_Interval); |
Request_DebugData = FALSE; |
} |
if( ((Data3D_Interval && CheckDelay(Data3D_Timer)) || Request_Data3D) && txd_complete) |
{ |
SendOutData('C', FC_ADDRESS, 1,(uint8_t *) &Data3D, sizeof(Data3D)); |
Data3D.AngleNick = (int16_t)((10 * IntegralGyroNick) / GYRO_DEG_FACTOR); // convert to multiple of 0.1° |
Data3D.AngleRoll = (int16_t)((10 * IntegralGyroRoll) / GYRO_DEG_FACTOR); // convert to multiple of 0.1° |
Data3D.Heading = (int16_t)((10 * YawGyroHeading) / GYRO_DEG_FACTOR); // 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 *) &ExternControl, sizeof(ExternControl)); |
Request_ExternalControl = FALSE; |
} |
#ifdef USE_MK3MAG |
if((CheckDelay(Compass_Timer)) && txd_complete) |
{ |
ToMk3Mag.Attitude[0] = (int16_t)((10 * IntegralGyroNick) / GYRO_DEG_FACTOR); // approx. 0.1 deg |
ToMk3Mag.Attitude[1] = (int16_t)((10 * IntegralGyroRoll) / GYRO_DEG_FACTOR); // approx. 0.1 deg |
ToMk3Mag.UserParam[0] = FCParam.UserParam1; |
ToMk3Mag.UserParam[1] = FCParam.UserParam2; |
ToMk3Mag.CalState = CompassCalState; |
SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag)); |
// the last state is 5 and should be send only once to avoid multiple flash writing |
if(CompassCalState > 4) CompassCalState = 0; |
Compass_Timer = SetDelay(99); |
} |
#endif |
if(Request_MotorTest && txd_complete) |
{ |
SendOutData('T', FC_ADDRESS, 0); |
Request_MotorTest = FALSE; |
} |
if(Request_PPMChannels && txd_complete) |
{ |
SendOutData('P', FC_ADDRESS, 1, (uint8_t *)&PPM_in, sizeof(PPM_in)); |
Request_PPMChannels = FALSE; |
} |
} |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/uart0.h |
---|
0,0 → 1,71 |
#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 PcAccess; |
extern uint8_t RemotePollDisplayLine; |
extern uint8_t MotorTest_Active; |
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 Digital[2]; |
uint8_t RemoteButtons; |
int8_t Nick; |
int8_t Roll; |
int8_t Yaw; |
uint8_t Gas; |
int8_t Height; |
uint8_t free; |
uint8_t Frame; |
uint8_t Config; |
} __attribute__((packed)) ExternControl_t; |
extern ExternControl_t ExternControl; |
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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/uart1.c |
---|
0,0 → 1,171 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "main.h" |
#include "uart1.h" |
#if defined (USE_KILLAGREG) || 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_KILLAGREG) || 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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/uart1.h |
---|
0,0 → 1,8 |
#ifndef _UART1_H |
#define _UART1_H |
extern void USART1_Init (void); |
#endif //_UART1_H |
/branches/MicroMag3_Nick666/V0.74d Code Redesign killagreg/ubx.c |
---|
0,0 → 1,289 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "main.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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/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/MicroMag3_Nick666/V0.74d Code Redesign killagreg/version.txt |
---|
0,0 → 1,287 |
------- |
V0.53 27.04.2007 H.Buss |
- erste öffentliche Version |
V0.53b 29.04.2007 H.Buss |
- der FAKTOR_I war versehentlich auf Null, dann liegt der MikroKopter nicht so hart in der Luft |
V0.53c 29.04.2007 H.Buss |
- es gib ein Menü, in dem die Werte der Kanäle nach Nick, Roll, Gas,... sortiert sind. |
Die angezeigten Werte waren nicht die Werte der Funke |
V0.54 01.05.2007 H.Buss |
- die Paramtersätze können jetzt vor dem Start ausgewählt werden |
Dazu wird beim Kalibrieren der Messwerte (Gashebel oben links) der Nick-Rollhebel abgefragt: |
2 3 4 |
1 x 5 |
- - - |
Bedeutet: Nick-Rollhebel Links Mitte = Setting:1 Links Oben = Setting:2 usw. |
- der Faktor_I für den Hauptregler ist hinzugekommen. Im Heading-Hold-Modus sollte er vergössert werden, was Stabilität bringt |
V0.55 14.05.2007 H.Buss |
- es können nun Servos an J3,J4,J5 mit den Kanälen 5-7 gesteuert werden |
V0.56 14.05.2007 H.Buss |
- es gab Probleme mit Funken, die mehr als 8 Kanäle haben, wenn mehrere Kanäle dann auf Null waren |
- Funken, die nicht bis +-120 aussteuern können, sollten jetzt auch gehen |
V0.57 24.05.2007 H.Buss |
- Der Höhenregler kann nun auch mittels Schalter bedient werden |
- Bug im Gier-Algorithmus behoben; Schnelles Gieren fürhrte dazu, dass der MK zu weit gedreht hat |
- Kompass-Einfluss dämpfen bei Neigung |
- Man kann zwischen Kompass FIX (Richtung beim Kalibrieren) und Variabel (einstellbar per Gier) wählen |
- Der Motortest vom Kopter-Tool geht jetzt |
- Man kann den Parametersätzen einen Namen geben |
- Das Kamerasetting ist unter Setting 2 defaultmässig integriert |
V0.58 30.05.2007 H.Buss |
- Der Höhenregler-Algorithmus wird nun umgangen, wenn der Höhenreglerschalter aus ist |
V0.60 17.08.2007 H.Buss |
- "Schwindel-Bug" behoben |
- Die Poti-Werte werden jetzt auf Unterlauf (<0) überprüft |
- Poti4 zugefügt |
- Es werden jetzt 8 Kanäle ausgewertet |
- Kamera-Servo (an J7) |
- Die Settings müssen überschrieben werden |
V0.61 - V0.63 H.Buss 27.09.2007 |
- Poti 4 und Kanal 8 werden im Menü angezeigt |
- ein paar Kleinigkeiten bei den DefaultKonstanten2 bereinigt |
- Analog.c: Aktuell_ax korrigiert |
- auf 32 Debug-Kanäle erweitert |
- Loopings sind jetzt möglich und einzeln im KopterTool freischaltbar |
- leichte Anpassungen im Gier - Geschwindigkeit und Drift |
- die Hardwareversion V1.1 wird erkannt und das Programm stellt sich auf die geänderte Gyroverstärkung und die geänderten Portpins ein |
- die Software startet nach dem Einschalten schneller, weil der Luftdruckoffset schneller gefunden wird |
- die PPM-Ausgänge liegen wieder an den Pins an |
- Details an der Sensordatenverarbeitung -> es fliegt sich geringfügig anders |
- der MK ist bei wenig Gas nicht mehr so giftig -> soll das Landen vereinfachen |
- I2C-Bus läuft jetzt sicher nach einer Störung wieder an |
- Sticksignale werden präziser ausgewertet |
- Stick-Kanäle werden ans Kopter-Tool übertragen |
- Es muss die Version V1.47 des Kopter-Tool verwendet werden |
- Die Settings werden auf Default zurückgesetzt |
- am Piepen kann man die Fehlerart unterscheiden |
1. einzelnes Piepen beim Einschalten und Kalibrieren |
2. langsames Intervall mindestens 1 Sek -> Empfangsausfall |
3. schnelleres Intervall mindestens 1 Sek -> Akku |
4. sehr schnelles Intervall mindestens 1 Sek -> Kommunikation zu den Reglern gestört |
V0.64 H.Buss 30.09.2007 |
- beim Gieren wurden die Achsen nicht hart genug geregelt |
V0.65a H.Buss 15.10.2007 |
- Integral im Mischer wieder integriert |
- Feinabstimmung im ACC/Gyro Abgleich -> 1/32 & 100 |
- ACC/Gyro Abgleich auch bei HH |
V0.66a H.Buss 3.11.2007 |
- Messwertverarbeitung aus dem Analog-Interrupt entfernt |
- Analogmessung hängt jetzt am FC-Timing |
- Looping-Stick-Hysterese eingebaut |
- Looping-180°-Umschlag einstellbar |
- Achsenkopplung: Gierbewegung verkoppelt Nick und Roll |
- Lageregelung nach ACC-Sensor verbessert |
- zusätzlicher I-Anteil in der Lageregelung verbessert die Neutrallage |
- Gyrodriftkompensation überarbeitet |
- Bug in der Gier-Stick-Berechnung behoben |
- Gyro-Messung auf 1kHz beschleunigt |
V0.67a H.Buss 16.11.2007 |
- der Hauptregler-I-Anteil wirkt jetzt nur noch auf den Winkel (ausser im HH-Mode) |
- Gyro-Acc-Abgleich jetzt wieder in jedem Zyklus |
- Feinabstimmung |
- Beim HH-Modus gab es noch Bugs |
V0.67e H.Buss 29.11.2007 |
- Parameter: Dynamic Stability und Driftfaktor eingeführt |
- Die Namen der Analogwerte werden jetzt zum Koptertool übertragen |
- Kompatibilität zum Koptertool erhöht |
V0.67f H.Buss 04.12.2007 |
- Das Integral des Hauptreglers wird jetzt linear entladen und nicht mehr proportional |
- Schub für Gier wird jetzt auf den Gaswert begrenzt, dadurch steigt der MK nicht mehr beim Gieren. Gier ist allerdings nicht mehr so agressiv |
- Die ACC-Nullwerte können jetzt dauerhaft im EEPROM gespeichert werden (Stick:Vollgas und Gier rechts) |
V0.68a I.Busker 28.12.2007 |
- SPI.c & SPI.h ins Projekt aufgenommen |
SPI-Kommuikation kann in SPI.h aktiviert/deaktivert werden |
V0.68c H.Buss 05.01.2008 |
- Stickauswertung verbessert -> träger und präziser |
- Alle Settings angepasst |
V0.69g H.Buss 05.05.2008 |
- kleinere Bugs beseitigt |
- Schneller Sinkflug jetzt möglich |
- Min- und Maxgas in den Settings geändert |
- Lagewinkel wird jetzt in 0,1 Grad an Kompass und Navi gesendet |
- Kalibrierung für MK3Mag -> Nick unten beim Kalibrieren |
- Kompassroutine um den Ersatzkompass (Gyro unterstützt Kompasswert) erweitert |
V0.69h H.Buss 21.05.2008 |
- STICK_GAIN = 4 eingeführt. Das erhöht die Auflösung der Sollwerte. Stick_P und Stick_I müssen nun um Faktor 4 erhöht werden |
- SenderOkay auch an das Naviboard übertragen |
- Bessere Parameter bei Senderausfall |
V0.69j H.Buss 30.05.2008 |
- Höhere Auflösung der Achsenkopplung |
V0.69k H.Buss 31.05.2008 |
- Bug in SPI.C behoben |
- in 0.69h war ein Bug, der zu ungewollten Loopings führen konnte |
V0.69L H.Buss 14.06.2008 |
- feinere Cam-Servo-Auflösung |
V0.70a H.Buss 01.07.2008 |
- Unterstützung der V1.3-Hardware mit automatischem Hardware-Gyro-Abgleich |
V0.70b H.Buss 14.07.2008 |
- flexible Einstellungsmöglichkeit von J16 und J17 (Transistorausgänge) |
- eigene Parameter für GPS-Naviboard |
- eigener Parameter für ExternalControl (war vorher UserParameter1 bzw. 8) |
- neue Parameter im EEPROM-Datensatz: J16Bitmask, J16Timing, ExternalControl, Navi... |
- MikroKopterFlags eingeführt, damit das Navi den Status des MKs kennt |
- KopterTool-Kompatibilität auf 8 erhöht |
V0.70c H.Buss 30.07.2008 |
- Parameter der Datenfusion leicht modifiziert |
- EEPROM-Parameter für Looping-Umschlag angepasst (von 100 auf 85) |
- MaxStick wird auf 100 begrenzt |
V0.70d H.Buss 02.08.2008 |
- Transistorausgänge: das oberste Bit der Blinkmaske (im KopterTool linkes Bit) gibt nun den Zustand des Ausgangs im Schalterbetrieb an |
0.71b: H.Buss 19.10.2008 |
Kommunikation zum Navi erweitert |
- Beeptime jetzt 32Bit |
- Datenfusion und Driftkopensation wird durch NaviBoard unterstützt |
0.71c: H.Buss 20.10.2008 |
- LoopConfig heisst jetzt BitConfig |
- 3-Fach-Schalter für Höhensteuerung möglich -> kann man mit GPS-Schalter zusammenlegen |
- bei den Settings wurde Setting[0] mit abgespeichert, welches es nicht gab. |
- in Zukunft werden bei neuen EEPROM-Settings die Kanäle von Setting 1 übernommen |
- Variablen NaviWindCorrection, NaviSpeedCompensation, NaviOperatingRadius eingeführt |
0.71f: H.Buss 15.11.2008 |
- Ausschalten der Höhenregelung per Schalter um 0,3 sek verzögert |
- bei der seriellen Übertragung hat die FC jetzt als SlaveAdresse die 1 |
- VersionInfo.NaviKompatibel eingeführt |
- wenn manuell gegiert wird, wird der GyroKompass-Wert auf den Kompasswert gesetzt |
- Luftdruckwert wird an das Navi übertragen |
- Der Baro-Offset wird jetzt nachgeführt, um den Messbereich zu erweitern. Geht nur bei Höhenregler mit Schalter |
- Debugdaten können jetzt mit 'f' gepollt werden |
0.71g: Gregor 09.12.2008 |
- Kommunikation überarbeitet |
Infos hier: http://www.mikrokopter.de/ucwiki/en/SerialCommands |
0.71h: H.Buss 15.12.2008 - Freigegebene Version |
- NaviAngleLimitation als Parameter zum Navi implementiert |
- Antwort auf CMD: 't' entfernt |
0.72d: H.Buss 22.01.2009 |
- OCTO als Compilerschalter |
- Unterstützung der FC 2.0 (ME) |
- GYRO_D eingeführt |
- Achsenkopplung jetzt auch auf Nick/Roll-Bewegung |
0.72e: H.Buss 27.01.2009 |
- die 0.72d hatte kein Integral im Gier |
- Parameter eingeführt: |
EE_Parameter.NaviGpsPLimit |
EE_Parameter.NaviGpsILimit |
EE_Parameter.NaviGpsDLimit |
EE_Parameter.NaviPH_LoginTime |
EE_Parameter.AchsKopplung2 |
EE_Parameter.CouplingYawCorrection |
0.72f: H.Buss 28.01.2009 |
- Bug im Ersatzkompass entfernt |
0.72h: H.Buss 05.02.2009 |
- Algorithmen beschleunigt -> Floats durch Fixkomma ersetzt |
- Achsentkopplung weiter verbessert |
- Nick- und Roll im Octo-Mischer auf jeweils vier Motoren aufgeteilt |
0.72i: H.Buss 07.02.2009 |
- Abtastrate von 1kHz auf 2kHz erhöht |
0.72j: H.Buss 09.02.2009 |
- neue Implementierung der Servoausgänge |
0.72k: H.Buss 10.02.2009 |
- Abtastrate auf 5kHz erhöht |
0.72l: H.Buss 13.02.2009 |
- Signalfilterung überarbeitet |
- OCTO2 implementiert |
0.72m: H.Buss 13.02.2009 |
- Code Cleanup |
0.72o: H.Buss 24.02.2009 |
- Abtastrate auf 2kHz |
- HW-Version an Navi |
- neuer Datensatz 'c' -> Lagedaten für 3D-Grafik |
- Auswerteroutine für Spectrum-Satteliten implementiert |
- Kanalsettings werden beim Parameterreset nicht mehr gelöscht |
- die Driftkompensation wird jetzt feiner aufgelöst --> EE_Parameter.Driftkomp muss mal 8 genommen werden |
- die Integrale und ACC-Werte werden jetzt im Scope in ca. 0,1° angezeigt (wie beim NaviBrd) |
0.72p: H.Buss 01.03.2009 |
- Octo3 erstellt |
- Analogwerte umbenannt |
0.73a: H.Buss 12.03.2009 |
- MixerTabelle implementiert |
0.73b: H.Buss 14.03.2009 |
- Es wird geprüft, ob alle notwendigen BL-Regler angeschlossen sind |
0.73a-d: H.Buss 05.04.2009 |
- MixerTabelle implementiert |
- I2C-Bus auf bis zu 12 Motoren erweitert |
- die Busfehler der BL-Regler werden im Menü angezeigt |
- Revision der MixerTabelle eingeführt |
- MixerTabelle wird bei Parameterreset neu initialisiert |
- Motortest auf [12] erweitert |
- Motorschalter nicht mehr 3-Stufig |
0.74a |
- Datenfusion im Flug auch, wenn ACC-Z < 512 |
- Wert für die Messbereichserweiterung abgefangen |
0.74d |
- Die Driftkompensation ist jetzt dreistufig -> 0,5% pro sekunde zusätzlich eingeführt |
Anpassungen bzgl. V0.74d |
G.Stobrawa 28.04.2009: |
- Code stärker modularisiert und restrukturiert |
- viele Kommentare zur Erklärug eingefügt |
- konsequent englische Variablennamen |
- PPM24 Support für bis zu 12 RC-Kanäle. |
- Suport for DSL Receiver at 2nd UART |
- Makefile: EXT=NAVICTRL Unterstützung der SPI Communikation zum Naviboard |
- Makefile: EXT=MK3MAG Unerstützung des MK3MAG/CMPS03 direkt an FC und Conrad UBLOX Modul |
- Makefile: EXT=KILLAGREG Unterstützung vom KillagregBoard mit MM3 und Conrad UBLOX Modul |
- Ausertung des UBX-Protocols an 1. oder 2. Uart |
- GPS-Hold-Funktion hinzugefügt |
- GPS-Home-Funktion hinzugefügt (wird beim Motorstart gelernt, und bei Motorenstop wieder gelöscht) |
- Zusätzliche Punkte im Menü des KopterTool zur Anzeige des GPS-Status und der MM3-Kalibierparameter |
Weiter Detail siehe Readme.txt im Verzeichnis Hex-Files. |