Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1179 → Rev 1180

/branches/V0.72p Code Redesign killagreg/uart.h
File deleted
/branches/V0.72p Code Redesign killagreg/_Settings.h
File deleted
/branches/V0.72p Code Redesign killagreg/flight.pnps
File deleted
\ No newline at end of file
/branches/V0.72p Code Redesign killagreg/Kopter-Tool/MikroKopter-Tool.exe
Cannot display: file marked as a binary type.
svn:mime-type = application/octet-stream
Property changes:
Deleted: svn:mime-type
-application/octet-stream
\ No newline at end of property
/branches/V0.72p Code Redesign killagreg/flight.pnproj
File deleted
\ No newline at end of file
/branches/V0.72p Code Redesign killagreg/FlightCtrl.aps
File deleted
/branches/V0.72p Code Redesign killagreg/uart.c
File deleted
/branches/V0.72p Code Redesign killagreg/Hex-Files/Flight-Ctrl_MEGA644_NAVICTRL_V0_71h_SVN1080.hex
File deleted
/branches/V0.72p Code Redesign killagreg/Hex-Files/Flight-Ctrl_MEGA644p_MK3MAG_V0_71h_SVN1080.hex
File deleted
/branches/V0.72p Code Redesign killagreg/Hex-Files/Flight-Ctrl_MEGA644_MK3MAG_V0_71h_SVN1080.hex
File deleted
/branches/V0.72p Code Redesign killagreg/Hex-Files/Flight-Ctrl_MEGA644p_KILLAGREG_V0_71h_SVN1080.hex
File deleted
/branches/V0.72p Code Redesign killagreg/Hex-Files/Flight-Ctrl_MEGA644p_NAVICTRL_V0_71h_SVN1083.hex
File deleted
/branches/V0.72p Code Redesign killagreg/Hex-Files/Flight-Ctrl_MEGA644_KILLAGREG_V0_71h_SVN1080.hex
File deleted
/branches/V0.72p Code Redesign killagreg/analog.c
1,9 → 1,53
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + only for non-profit use
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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>
16,21 → 60,26
#include "eeprom.h"
#include "twimaster.h"
 
volatile int16_t Current_AccZ = 0;
volatile uint16_t Test = 0;
 
volatile int16_t UBat = 100;
volatile int16_t AdValueGyrNick = 0, AdValueGyrRoll = 0, AdValueGyrYaw = 0;
uint8_t AnalogOffsetNick = 115, AnalogOffsetRoll = 115, AnalogOffsetYaw = 115;
uint8_t GyroDefectNick = 0, GyroDefectRoll = 0, GyroDefectYaw = 0;
volatile int16_t AdValueAccRoll = 0, AdValueAccNick = 0, AdValueAccTop = 0;
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;
int8_t ExpandBaro = 0;
uint8_t PressureSensorOffset;
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 */
/*****************************************************/
51,11 → 100,11
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 = (1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
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));
// Enable AD conversion
// Start AD conversion
ADC_Enable();
// restore global interrupt flags
SREG = sreg;
83,30 → 132,33
}
 
 
void SearchGyroOffset(void)
void SearchDacGyroOffset(void)
{
uint8_t i, ready = 0;
 
GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
for(i = 140; i != 0; i--)
{
if(ready == 3 && i > 10) i = 9;
ready = 0;
if(AdValueGyrNick < 1020) AnalogOffsetNick--; else if(AdValueGyrNick > 1030) AnalogOffsetNick++; else ready++;
if(AdValueGyrRoll < 1020) AnalogOffsetRoll--; else if(AdValueGyrRoll > 1030) AnalogOffsetRoll++; else ready++;
if(AdValueGyrYaw < 1020) AnalogOffsetYaw-- ; else if(AdValueGyrYaw > 1030) AnalogOffsetYaw++ ; else ready++;
twi_state = TWI_STATE_GYRO_OFFSET_TX; // set twi_state in TWI ISR to start of Gyro Offset
I2C_Start(); // initiate data transmission
if(AnalogOffsetNick < 10) { GyroDefectNick = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefectNick = 1; AnalogOffsetNick = 245;};
if(AnalogOffsetRoll < 10) { GyroDefectRoll = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefectRoll = 1; AnalogOffsetRoll = 245;};
if(AnalogOffsetYaw < 10) { GyroDefectYaw = 1; AnalogOffsetYaw = 10;}; if(AnalogOffsetYaw > 245) { GyroDefectYaw = 1; AnalogOffsetYaw = 245;};
while(twi_state); // wait for end of data transmission
average_pressure = 0;
ADC_Enable();
while(average_pressure == 0);
if(i < 10) Delay_ms_Mess(10);
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++;
twi_state = TWI_STATE_GYRO_OFFSET_TX; // set twi_state in TWI ISR to start of Gyro Offset
I2C_Start(); // 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); // 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);
}
Delay_ms_Mess(70);
}
 
 
115,124 → 167,180
/*****************************************************/
/* Interrupt Service Routine for ADC */
/*****************************************************/
// runs at 156.25 kHz or 6.4 µs
// if after (70.4µs) all 11 states are processed the interrupt is disabled
// 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
// The routine changes the ADC input muxer running
// thru the state machine by the following order.
// state 0: ch0 (yaw gyro)
// state 1: ch1 (roll gyro)
// state 2: ch2 (nick gyro)
// state 3: ch4 (battery voltage -> UBat)
// state 4: ch6 (acc y -> Current_AccY)
// state 5: ch7 (acc x -> Current_AccX)
// state 6: ch0 (yaw gyro average with first reading -> AdValueGyrYaw)
// state 7: ch1 (roll gyro average with first reading -> AdValueGyrRoll)
// state 8: ch2 (nick gyro average with first reading -> AdValueGyrNick)
// state 9: ch5 (acc z add also 4th part of acc x and acc y to reading)
// state10: ch3 (air pressure averaging over 5 single readings -> tmpAirPressure)
 
/*
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 adc_channel = 0, state = 0;
static uint16_t yaw1, roll1, nick1;
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;
// disable further AD conversion
ADC_Disable();
 
// state machine
switch(state++)
{
case 0:
yaw1 = ADC; // get Gyro Yaw Voltage 1st sample
adc_channel = 1; // set next channel to ADC1 = ROLL GYRO
MeasurementCounter++; // increment total measurement counter
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 1:
roll1 = ADC; // get Gyro Roll Voltage 1st sample
adc_channel = 2; // set next channel to ADC2 = NICK GYRO
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 2:
nick1 = ADC; // get Gyro Nick Voltage 1st sample
adc_channel = 4; // set next channel to ADC4 = UBAT
break;
case 3:
// 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
adc_channel = 6; // set next channel to ADC6 = ACC_Y
break;
case 4:
AdValueAccRoll = NeutralAccY - ADC; // get acceleration in Y direction
adc_channel = 7; // set next channel to ADC7 = ACC_X
break;
case 5:
AdValueAccNick = ADC - NeutralAccX; // get acceleration in X direction
adc_channel = 0; // set next channel to ADC7 = YAW GYRO
break;
case 6:
// average over two samples to create current AdValueGyrYaw
if(BoardRelease == 10) AdValueGyrYaw = (ADC + yaw1) / 2;
else if (BoardRelease == 20) AdValueGyrYaw = 1023 - (ADC + yaw1);
else AdValueGyrYaw = ADC + yaw1; // gain is 2 times lower on FC 1.1
adc_channel = 1; // set next channel to ADC7 = ROLL GYRO
break;
case 7:
// average over two samples to create current ADValueGyrRoll
if(BoardRelease == 10) AdValueGyrRoll = (ADC + roll1) / 2;
else AdValueGyrRoll = ADC + roll1; // gain is 2 times lower on FC 1.1
adc_channel = 2; // set next channel to ADC2 = NICK GYRO
break;
case 8:
// average over two samples to create current ADValueNick
if(BoardRelease == 10) AdValueGyrNick = (ADC + nick1) / 2;
else AdValueGyrNick = ADC + nick1; // gain is 2 times lower on FC 1.1
adc_channel = 5; // set next channel to ADC5 = ACC_Z
break;
case 9:
// get z acceleration
AdValueAccTop = (int16_t) ADC - NeutralAccZ; // get plain acceleration in Z direction
AdValueAccTop += abs(AdValueAccNick) / 4 + abs(AdValueAccRoll) / 4;
if(AdValueAccTop > 1)
{
if(NeutralAccZ < 750)
{
NeutralAccZ += 0.02;
if(Model_Is_Flying < 500) NeutralAccZ += 0.1;
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(NeutralAccZ > 550)
{
NeutralAccZ-= 0.02;
if(Model_Is_Flying < 500) NeutralAccZ -= 0.1;
}
else if(AdValueAccTop < -1)
{
if(AdBiasAccTop > 550)
{
AdBiasAccTop -= 0.02;
if(ModelIsFlying < 500) AdBiasAccTop -= 0.1;
}
}
Current_AccZ = ADC;
Reading_Integral_Top += AdValueAccTop; // Integrieren
Reading_Integral_Top -= Reading_Integral_Top / 1024; // dämfen
adc_channel = 3; // set next channel to ADC3 = air pressure
}
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 10:
tmpAirPressure += ADC; // sum vadc values
if(++average_pressure >= 5) // if 5 values are summerized for averaging
{
ReadingAirPressure = ADC; // update measured air pressure
HeightD = (7 * HeightD + (int16_t)FCParam.Height_D * (int16_t)(255 * ExpandBaro + StartAirPressure - tmpAirPressure - ReadingHeight))/8; // D-Part = CurrentValue - OldValue
AirPressure = (tmpAirPressure + 3 * AirPressure) / 4; // averaging using history
ReadingHeight = 255 * ExpandBaro + StartAirPressure - AirPressure;
average_pressure = 0; // reset air pressure measurement counter
tmpAirPressure = 0;
}
adc_channel = 0; // set next channel to ADC0 = GIER GYRO
state = 0; // reset state machine
break;
default:
adc_channel = 0;
state = 0;
break;
}
// set adc muxer to next adc_channel
ADMUX = (ADMUX & 0xE0) | adc_channel;
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/V0.72p Code Redesign killagreg/analog.h
3,23 → 3,29
 
#include <inttypes.h>
 
extern volatile uint16_t MeasurementCounter;
extern volatile int16_t UBat;
extern volatile int16_t AdValueGyrNick, AdValueGyrRoll, AdValueGyrYaw;
extern uint8_t AnalogOffsetNick, AnalogOffsetRoll, AnalogOffsetYaw;
extern volatile int16_t AdValueAccRoll, AdValueAccNick, AdValueAccTop;
extern volatile int16_t Current_AccZ;
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 uint16_t MeasurementCounter;
extern int8_t ExpandBaro;
extern uint8_t PressureSensorOffset;
extern volatile int16_t HeightD;
extern volatile uint16_t ReadingAirPressure;
extern volatile int16_t StartAirPressure;
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 SearchGyroOffset(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
/branches/V0.72p Code Redesign killagreg/eeprom.c
1,9 → 1,61
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Konstanten
// + 0-250 -> normale Werte
// + Contant Values
// + 0-250 -> normale Values
// + 251 -> Poti1
// + 252 -> Poti2
// + 253 -> Poti3
// + 254 -> Poti4
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#ifndef EEMEM
16,6 → 68,7
#include "eeprom.h"
#include "printf_P.h"
#include "led.h"
#include "main.h"
 
 
// byte array in eeprom
30,59 → 83,72
/***************************************************/
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_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.Height_MinGas = 30;
ParamSet.MaxHeight = 251; // Wert : 0-250 251 -> Poti1
ParamSet.Height_P = 10; // Wert : 0-32
ParamSet.Height_D = 30; // Wert : 0-250
ParamSet.Height_ACC_Effect = 30; // Wert : 0-250
ParamSet.Height_Gain = 4; // Wert : 0-50
ParamSet.Stick_P = 15; // Wert : 1-24
ParamSet.Stick_D = 30; // Wert : 0-250
ParamSet.Yaw_P = 12; // Wert : 1-20
ParamSet.Gas_Min = 8; // Wert : 0-32
ParamSet.Gas_Max = 230; // Wert : 33-250
ParamSet.GyroAccFactor = 30; // Wert : 1-64
ParamSet.CompassYawEffect = 128; // Wert : 0-250
ParamSet.Gyro_P = 80; // Wert : 0-250
ParamSet.Gyro_I = 150; // Wert : 0-250
ParamSet.LowVoltageWarning = 94; // Wert : 0-250
ParamSet.EmergencyGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust
ParamSet.EmergencyGasDuration = 30; // Wert : 0-250 // Zeit bis auf EmergencyGas geschaltet wird, wg. Rx-Problemen
ParamSet.UfoArrangement = 0; // X oder + Formation
ParamSet.I_Factor = 32;
ParamSet.UserParam1 = 0; //zur freien Verwendung
ParamSet.UserParam2 = 0; //zur freien Verwendung
ParamSet.UserParam3 = 0; //zur freien Verwendung
ParamSet.UserParam4 = 0; //zur freien Verwendung
ParamSet.UserParam5 = 0; // zur freien Verwendung
ParamSet.UserParam6 = 0; // zur freien Verwendung
ParamSet.UserParam7 = 0; // zur freien Verwendung
ParamSet.UserParam8 = 0; // zur freien Verwendung
ParamSet.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos
ParamSet.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo
ParamSet.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo
ParamSet.ServoNickMin = 50; // Wert : 0-250 // Anschlag
ParamSet.ServoNickMax = 150; // Wert : 0-250 // Anschlag
ParamSet.ServoNickRefresh = 5;
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; // Wert: 0-250 Schwelle für Stickausschlag
ParamSet.LoopThreshold = 90;
ParamSet.LoopHysteresis = 50;
ParamSet.BitConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt
ParamSet.Yaw_PosFeedback = 90;
ParamSet.Yaw_NegFeedback = 5;
ParamSet.AngleTurnOverNick = 85;
ParamSet.AngleTurnOverRoll = 85;
ParamSet.GyroAccTrim = 16; // 1/k
ParamSet.DriftComp = 4;
ParamSet.BitConfig = 0;
ParamSet.AxisCoupling1 = 90;
ParamSet.AxisCoupling2 = 67;
ParamSet.AxisCouplingYawCorrection = 0;
ParamSet.GyroAccTrim = 16;
ParamSet.DynamicStability = 100;
ParamSet.J16Bitmask = 95;
ParamSet.J17Bitmask = 243;
93,6 → 159,9
ParamSet.NaviGpsP = 90;
ParamSet.NaviGpsI = 90;
ParamSet.NaviGpsD = 90;
ParamSet.NaviGpsPLimit = 75;
ParamSet.NaviGpsILimit = 75;
ParamSet.NaviGpsDLimit = 75;
ParamSet.NaviGpsACC = 0;
ParamSet.NaviGpsMinSat = 6;
ParamSet.NaviStickThreshold = 8;
99,7 → 168,8
ParamSet.NaviWindCorrection = 90;
ParamSet.NaviSpeedCompensation = 30;
ParamSet.NaviOperatingRadius = 100;
ParamSet.NaviAngleLimitation = 60;
ParamSet.NaviAngleLimitation = 100;
ParamSet.NaviPHLoginTime = 4;
memcpy(ParamSet.Name, "Sport\0",6);
}
 
109,6 → 179,22
/***************************************************/
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;
118,50 → 204,47
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.Height_MinGas = 30;
ParamSet.MaxHeight = 251; // Wert : 0-250 251 -> Poti1
ParamSet.Height_P = 10; // Wert : 0-32
ParamSet.Height_D = 30; // Wert : 0-250
ParamSet.Height_ACC_Effect = 30; // Wert : 0-250
ParamSet.Height_Gain = 3; // Wert : 0-50
ParamSet.Stick_P = 12; // Wert : 1-24
ParamSet.Stick_D = 16; // Wert : 0-250
ParamSet.Yaw_P = 6; // Wert : 1-20
ParamSet.Gas_Min = 8; // Wert : 0-32
ParamSet.Gas_Max = 230; // Wert : 33-250
ParamSet.GyroAccFactor = 30; // Wert : 1-64
ParamSet.CompassYawEffect = 128; // Wert : 0-250
ParamSet.Gyro_P = 80; // Wert : 0-250
ParamSet.Gyro_I = 120; // Wert : 0-250
ParamSet.LowVoltageWarning = 94; // Wert : 0-250
ParamSet.EmergencyGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust
ParamSet.EmergencyGasDuration = 30; // Wert : 0-250 // Zeit bis auf EmergencyGas geschaltet wird, wg. Rx-Problemen
ParamSet.UfoArrangement = 0; // X oder + Formation
ParamSet.I_Factor = 32;
ParamSet.UserParam1 = 0; // zur freien Verwendung
ParamSet.UserParam2 = 0; // zur freien Verwendung
ParamSet.UserParam3 = 0; // zur freien Verwendung
ParamSet.UserParam4 = 0; // zur freien Verwendung
ParamSet.UserParam5 = 0; // zur freien Verwendung
ParamSet.UserParam6 = 0; // zur freien Verwendung
ParamSet.UserParam7 = 0; // zur freien Verwendung
ParamSet.UserParam8 = 0; // zur freien Verwendung
ParamSet.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos
ParamSet.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo
ParamSet.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo
ParamSet.ServoNickMin = 50; // Wert : 0-250 // Anschlag
ParamSet.ServoNickMax = 150; // Wert : 0-250 // Anschlag
ParamSet.ServoNickRefresh = 5;
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; // Wert: 0-250 Schwelle für Stickausschlag
ParamSet.LoopThreshold = 90;
ParamSet.LoopHysteresis = 50;
ParamSet.BitConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts
ParamSet.Yaw_PosFeedback = 90; // Faktor, mit dem Yaw die Achsen Roll und Nick verkoppelt
ParamSet.Yaw_NegFeedback = 5;
ParamSet.AngleTurnOverNick = 85;
ParamSet.AngleTurnOverRoll = 85;
ParamSet.GyroAccTrim = 32; // 1/k
ParamSet.DriftComp = 4;
ParamSet.BitConfig = 0;
ParamSet.AxisCoupling1 = 90;
ParamSet.AxisCoupling2 = 67;
ParamSet.AxisCouplingYawCorrection = 60;
ParamSet.GyroAccTrim = 32;
ParamSet.DynamicStability = 75;
ParamSet.J16Bitmask = 95;
ParamSet.J17Bitmask = 243;
172,6 → 255,9
ParamSet.NaviGpsP = 90;
ParamSet.NaviGpsI = 90;
ParamSet.NaviGpsD = 90;
ParamSet.NaviGpsPLimit = 75;
ParamSet.NaviGpsILimit = 75;
ParamSet.NaviGpsDLimit = 75;
ParamSet.NaviGpsACC = 0;
ParamSet.NaviGpsMinSat = 6;
ParamSet.NaviStickThreshold = 8;
178,7 → 264,8
ParamSet.NaviWindCorrection = 90;
ParamSet.NaviSpeedCompensation = 30;
ParamSet.NaviOperatingRadius = 100;
ParamSet.NaviAngleLimitation = 60;
ParamSet.NaviAngleLimitation = 100;
ParamSet.NaviPHLoginTime = 4;
memcpy(ParamSet.Name, "Normal\0", 7);
}
 
188,6 → 275,22
/***************************************************/
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;
197,50 → 300,47
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.Height_MinGas = 30;
ParamSet.MaxHeight = 251; // Wert : 0-250 251 -> Poti1
ParamSet.Height_P = 10; // Wert : 0-32
ParamSet.Height_D = 30; // Wert : 0-250
ParamSet.Height_ACC_Effect = 30; // Wert : 0-250
ParamSet.Height_Gain = 3; // Wert : 0-50
ParamSet.Stick_P = 8; // Wert : 1-24
ParamSet.Stick_D = 16; // Wert : 0-250
ParamSet.Yaw_P = 6; // Wert : 1-20
ParamSet.Gas_Min = 8; // Wert : 0-32
ParamSet.Gas_Max = 230; // Wert : 33-250
ParamSet.GyroAccFactor = 30; // Wert : 1-64
ParamSet.CompassYawEffect = 128; // Wert : 0-250
ParamSet.Gyro_P = 100; // Wert : 0-250
ParamSet.Gyro_I = 120; // Wert : 0-250
ParamSet.LowVoltageWarning = 94; // Wert : 0-250
ParamSet.EmergencyGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust
ParamSet.EmergencyGasDuration = 20; // Wert : 0-250 // Zeit bis auf EmergencyGas geschaltet wird, wg. Rx-Problemen
ParamSet.UfoArrangement = 0; // X oder + Formation
ParamSet.I_Factor = 16;
ParamSet.UserParam1 = 0; // zur freien Verwendung
ParamSet.UserParam2 = 0; // zur freien Verwendung
ParamSet.UserParam3 = 0; // zur freien Verwendung
ParamSet.UserParam4 = 0; // zur freien Verwendung
ParamSet.UserParam5 = 0; // zur freien Verwendung
ParamSet.UserParam6 = 0; // zur freien Verwendung
ParamSet.UserParam7 = 0; // zur freien Verwendung
ParamSet.UserParam8 = 0; // zur freien Verwendung
ParamSet.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos
ParamSet.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo
ParamSet.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo
ParamSet.ServoNickMin = 50; // Wert : 0-250 // Anschlag
ParamSet.ServoNickMax = 150; // Wert : 0-250 // Anschlag
ParamSet.ServoNickRefresh = 5;
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; // Wert: 0-250 Schwelle für Stickausschlag
ParamSet.LoopThreshold = 90;
ParamSet.LoopHysteresis = 50;
ParamSet.BitConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts
ParamSet.Yaw_PosFeedback = 90; // Faktor, mit dem Yaw die Achsen Roll und Nick verkoppelt
ParamSet.Yaw_NegFeedback = 5;
ParamSet.AngleTurnOverNick = 85;
ParamSet.AngleTurnOverRoll = 85;
ParamSet.GyroAccTrim = 32; // 1/k
ParamSet.DriftComp = 4;
ParamSet.BitConfig = 0;
ParamSet.AxisCoupling1 = 90;
ParamSet.AxisCoupling2 = 67;
ParamSet.AxisCouplingYawCorrection = 70;
ParamSet.GyroAccTrim = 32;
ParamSet.DynamicStability = 50;
ParamSet.J16Bitmask = 95;
ParamSet.J17Bitmask = 243;
251,6 → 351,9
ParamSet.NaviGpsP = 90;
ParamSet.NaviGpsI = 90;
ParamSet.NaviGpsD = 90;
ParamSet.NaviGpsPLimit = 75;
ParamSet.NaviGpsILimit = 75;
ParamSet.NaviGpsDLimit = 75;
ParamSet.NaviGpsACC = 0;
ParamSet.NaviGpsMinSat = 6;
ParamSet.NaviStickThreshold = 8;
257,7 → 360,8
ParamSet.NaviWindCorrection = 90;
ParamSet.NaviSpeedCompensation = 30;
ParamSet.NaviOperatingRadius = 100;
ParamSet.NaviAngleLimitation = 60;
ParamSet.NaviAngleLimitation = 100;
ParamSet.NaviPHLoginTime = 4;
memcpy(ParamSet.Name, "Beginner\0", 9);
}
 
314,6 → 418,8
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();
349,18 → 455,52
/***************************************************/
void ParamSet_Init(void)
{
// version check
uint8_t Channel_Backup = 0;
// parameter version check
if(eeprom_read_byte(&EEPromArray[PID_VERSION]) != EEPARAM_VERSION)
{
// if version check faild
printf("\n\rInit. EEPROM: Generating Default-Parameter...");
ParamSet_DefaultSet1(); // Fill ParamSet Structure to default parameter set 1 (Sport)
// fill all 5 parameter settings with set 1 except otherwise defined
printf("\n\rInit. EEPROM");
 
// check if cannel 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 (uint8_t i = 1;i < 6; i++)
{
if(i==2) ParamSet_DefaultSet2(); // Kamera
if(i==3) ParamSet_DefaultSet3(); // Beginner
if(i>3) ParamSet_DefaultSet2(); // Kamera
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
/branches/V0.72p Code Redesign killagreg/eeprom.h
3,13 → 3,13
 
#include <inttypes.h>
 
#define EEPROM_ADR_PARAM_BEGIN 0
#define EEPROM_ADR_PARAM_BEGIN 0
#define PID_VERSION 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_Z 8 // word
#define PID_ACC_TOP 8 // word
 
#ifdef USE_KILLAGREG
#define PID_MM3_X_OFF 10 // byte
20,8 → 20,11
#define PID_MM3_Z_RANGE 17 // word
#endif
 
#define EEPROM_ADR_PARAMSET_BEGIN 100
#define EEPROM_ADR_CHANNELS 80 // 8 bytes
 
#define EEPROM_ADR_PARAMSET_LENGTH 98 // word
#define EEPROM_ADR_PARAMSET_BEGIN 100
 
// bit mask for ParamSet.GlobalConfig
#define CFG_HEIGHT_CONTROL 0x01
#define CFG_HEIGHT_SWITCH 0x02
49,77 → 52,83
#define CH_POTI3 6
#define CH_POTI4 7
 
#define EEPARAM_VERSION 73 // is count up, if EE_Paramater stucture has changed (compatibility)
#define EEPARAM_VERSION 74 // is count up, if EE_Paramater 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 Height_MinGas; // Wert : 0-100
uint8_t Height_D; // Wert : 0-250
uint8_t MaxHeight; // Wert : 0-32
uint8_t Height_P; // Wert : 0-32
uint8_t Height_Gain; // Wert : 0-50
uint8_t Height_ACC_Effect; // Wert : 0-250
uint8_t Stick_P; // Wert : 1-6
uint8_t Stick_D; // Wert : 0-64
uint8_t Yaw_P; // Wert : 1-20
uint8_t Gas_Min; // Wert : 0-32
uint8_t Gas_Max; // Wert : 33-250
uint8_t GyroAccFactor; // Wert : 1-64
uint8_t CompassYawEffect; // Wert : 0-32
uint8_t Gyro_P; // Wert : 10-250
uint8_t Gyro_I; // 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 I_Factor; // 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 ServoNickRefresh; //
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 Yaw_PosFeedback; // Wert: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung)
uint8_t Yaw_NegFeedback; // Wert: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick Gegenkoppelt (NickRollGegenkopplung)
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;
uint8_t DynamicStability;
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 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;
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 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];
{
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)
/branches/V0.72p Code Redesign killagreg/fc.c
57,10 → 57,9
#include "main.h"
#include "eeprom.h"
#include "timer0.h"
#include "_Settings.h"
#include "analog.h"
#include "fc.h"
#include "uart.h"
#include "uart0.h"
#include "rc.h"
#include "twimaster.h"
#include "timer2.h"
76,40 → 75,49
#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 Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
// gyro neutral readings
int16_t AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;
int16_t StartNeutralRoll = 0, StartNeutralNick = 0;
// mean accelerations
int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop;
int16_t GyroNick, GyroRoll, GyroYaw;
 
// gyro bias
int16_t BiasHiResGyroNick = 0, BiasHiResGyroRoll = 0, AdBiasGyroYaw = 0;
 
// accelerations
int16_t AccNick, AccRoll, AccTop;
 
// neutral acceleration readings
volatile int16_t NeutralAccX=0, NeutralAccY=0;
volatile float NeutralAccZ = 0;
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 IntegralNick = 0,IntegralNick2 = 0;
int32_t IntegralRoll = 0,IntegralRoll2 = 0;
int32_t IntegralYaw = 0;
int32_t Reading_IntegralGyroNick = 0, Reading_IntegralGyroNick2 = 0;
int32_t Reading_IntegralGyroRoll = 0, Reading_IntegralGyroRoll2 = 0;
int32_t Reading_IntegralGyroYaw = 0;
int32_t MeanIntegralNick;
int32_t MeanIntegralRoll;
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 IntegralAccNick = 0, IntegralAccRoll = 0;
volatile int32_t Reading_Integral_Top = 0;
int32_t MeanAccNick = 0, MeanAccRoll = 0;
volatile int32_t ReadingIntegralTop = 0;
 
// compass course
volatile int16_t CompassHeading = -1; // negative angle indicates invalid data.
volatile int16_t CompassCourse = -1;
volatile int16_t CompassOffCourse = 0;
volatile uint8_t CompassCalState = 0;
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;
int32_t YawGyroHeading; // Yaw Gyro Integral supported by compass
int16_t YawGyroDrift;
 
 
117,46 → 125,55
 
 
// MK flags
uint16_t Model_Is_Flying = 0;
volatile uint8_t MKFlags = 0;
uint16_t ModelIsFlying = 0;
uint8_t MKFlags = 0;
 
int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L;
 
float Gyro_P_Factor;
float Gyro_I_Factor;
uint8_t GyroPFactor, GyroIFactor; // the PD factors for the attitude control
int16_t Ki = 10300 / 33;
 
int16_t DiffNick, DiffRoll;
 
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
 
// setpoints for motors
volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left;
 
volatile uint8_t Motor1, Motor2, Motor3, Motor4, Motor5, Motor6, Motor7, Motor8;
 
 
// stick values derived by rc channels readings
int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0;
int16_t GPS_Nick = 0, GPS_Roll = 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;
 
float Ki = FACTOR_I;
uint8_t LoopingNick = 0, LoopingRoll = 0;
uint8_t LoopingLeft = 0, LoopingRight = 0, LoopingDown = 0, LoopingTop = 0;
 
uint8_t Looping_Nick = 0, Looping_Roll = 0;
uint8_t Looping_Left = 0, Looping_Right = 0, Looping_Down = 0, Looping_Top = 0;
 
fc_param_t FCParam = {48,251,16,58,64,8,150,150,2,10,0,0,0,0,0,0,0,0,100,70,90,65,64,100};
 
fc_param_t FCParam = {48,251,16,58,64,150,150,2,10,0,0,0,0,0,0,0,0,100,70,0,0,100};
 
 
/************************************************************************/
/* 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)
174,66 → 191,139
/************************************************************************/
/* Neutral Readings */
/************************************************************************/
void SetNeutral(void)
void SetNeutral(uint8_t AccAdjustment)
{
NeutralAccX = 0;
NeutralAccY = 0;
NeutralAccZ = 0;
AdNeutralNick = 0;
AdNeutralRoll = 0;
AdNeutralYaw = 0;
FCParam.Yaw_PosFeedback = 0;
FCParam.Yaw_NegFeedback = 0;
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;
CalibMean();
 
// sample values with bias set to zero
Delay_ms_Mess(100);
CalibMean();
 
if(BoardRelease == 13) SearchDacGyroOffset();
 
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Height Control activated?
{
if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset();
}
AdNeutralNick = AdValueGyrNick;
AdNeutralRoll = AdValueGyrRoll;
AdNeutralYaw = AdValueGyrYaw;
StartNeutralRoll = AdNeutralRoll;
StartNeutralNick = AdNeutralNick;
if(GetParamWord(PID_ACC_NICK) > 1023)
 
// 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++)
{
NeutralAccY = abs(Mean_AccRoll) / ACC_AMPLIFY;
NeutralAccX = abs(Mean_AccNick) / ACC_AMPLIFY;
NeutralAccZ = Current_AccZ;
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
else // restore from eeprom
{
NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK);
NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL);
NeutralAccZ = (int16_t)GetParamWord(PID_ACC_Z);
AdBiasAccNick = (int16_t)GetParamWord(PID_ACC_NICK);
AdBiasAccRoll = (int16_t)GetParamWord(PID_ACC_ROLL);
AdBiasAccTop = (int16_t)GetParamWord(PID_ACC_TOP);
}
Reading_IntegralGyroNick = 0;
Reading_IntegralGyroNick2 = 0;
Reading_IntegralGyroRoll = 0;
Reading_IntegralGyroRoll2 = 0;
Reading_IntegralGyroYaw = 0;
Reading_GyroNick = 0;
Reading_GyroRoll = 0;
Reading_GyroYaw = 0;
Delay_ms_Mess(100);
// 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;
Reading_Integral_Top = 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;
TurnOver180Roll = ((int32_t) ParamSet.AngleTurnOverRoll * 2500L) +15000L;
 
ExternHeightValue = 0;
GPS_Nick = 0;
GPS_Roll = 0;
YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR;
YawGyroDrift = 0;
 
GPSStickNick = 0;
GPSStickRoll = 0;
 
MKFlags |= MKFLAG_CALIBRATE;
FCParam.Kalman_K = -1;
FCParam.Kalman_MaxDrift = ParamSet.DriftComp * 16;
FCParam.Kalman_MaxFusion = 32;
 
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;
}
 
/************************************************************************/
241,145 → 331,199
/************************************************************************/
void Mean(void)
{
static int32_t tmpl,tmpl2;
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 offset corrected gyro readings (~ to angular velocity)
Reading_GyroYaw = AdNeutralYaw - AdValueGyrYaw;
Reading_GyroRoll = AdValueGyrRoll - AdNeutralRoll;
Reading_GyroNick = AdValueGyrNick - AdNeutralNick;
// Get bias free gyro readings
GyroNick = HiResGyroNick / HIRES_GYRO_AMPLIFY; // unfiltered gyro rate
FilterGyroNick = FilterHiResGyroNick / HIRES_GYRO_AMPLIFY; // use filtered gyro rate
 
// Acceleration Sensor
// sliding average sensor readings
Mean_AccNick = ((int32_t)Mean_AccNick * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccNick))) / 2L;
Mean_AccRoll = ((int32_t)Mean_AccRoll * 1 + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 2L;
Mean_AccTop = ((int32_t)Mean_AccTop * 1 + ((int32_t)AdValueAccTop)) / 2L;
// 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;
}
 
// sum sensor readings for later averaging
IntegralAccNick += ACC_AMPLIFY * AdValueAccNick;
IntegralAccRoll += ACC_AMPLIFY * AdValueAccRoll;
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;
NaviAccRoll += AdValueAccRoll;
NaviCntAcc++;
 
// Yaw
 
// 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)
Reading_IntegralGyroYaw += Reading_GyroYaw;
YawGyroHeading += Reading_GyroYaw;
if(YawGyroHeading >= (360L * YAW_GYRO_DEG_FACTOR)) YawGyroHeading -= 360L * YAW_GYRO_DEG_FACTOR; // 360° Wrap
if(YawGyroHeading < 0) YawGyroHeading += 360L * YAW_GYRO_DEG_FACTOR;
YawGyroHeading += GyroYaw;
ReadingIntegralGyroYaw += GyroYaw;
 
 
// Coupling fraction
if(!Looping_Nick && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
if(! LoopingNick && !LoopingRoll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
{
tmpl = (Reading_GyroYaw * Reading_IntegralGyroNick) / 2048L;
tmpl *= FCParam.Yaw_PosFeedback;
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 = ( Reading_GyroYaw * Reading_IntegralGyroRoll) / 2048L;
tmpl2 *= FCParam.Yaw_PosFeedback;
 
tmpl2 = ((GyroYaw + tmp14) * AngleRoll) / 2048L;
tmpl2 *= FCParam.AxisCoupling1;
tmpl2 /= 4096L;
if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1;
}
else tmpl = tmpl2 = 0;
 
// Roll
Reading_GyroRoll += tmpl;
Reading_GyroRoll += (tmpl2 * FCParam.Yaw_NegFeedback) / 512L;
Reading_IntegralGyroRoll2 += Reading_GyroRoll;
Reading_IntegralGyroRoll += Reading_GyroRoll - AttitudeCorrectionRoll;
if(Reading_IntegralGyroRoll > TurnOver180Roll)
{
Reading_IntegralGyroRoll = -(TurnOver180Roll - 10000L);
Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll;
TrimNick = -tmpl2 + tmpl / 100L;
TrimRoll = tmpl - tmpl2 / 100L;
}
if(Reading_IntegralGyroRoll < -TurnOver180Roll)
{
Reading_IntegralGyroRoll = (TurnOver180Roll - 10000L);
Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll;
}
if(AdValueGyrRoll < 15) Reading_GyroRoll = -1000;
if(AdValueGyrRoll < 7) Reading_GyroRoll = -2000;
if(BoardRelease == 10)
{
if(AdValueGyrRoll > 1010) Reading_GyroRoll = +1000;
if(AdValueGyrRoll > 1017) Reading_GyroRoll = +2000;
}
else
{
if(AdValueGyrRoll > 2020) Reading_GyroRoll = +1000;
if(AdValueGyrRoll > 2034) Reading_GyroRoll = +2000;
CouplingNickRoll = 0;
CouplingRollNick = 0;
TrimNick = 0;
TrimRoll = 0;
}
// Nick
Reading_GyroNick -= tmpl2;
Reading_GyroNick -= (tmpl*FCParam.Yaw_NegFeedback) / 512L;
Reading_IntegralGyroNick2 += Reading_GyroNick;
Reading_IntegralGyroNick += Reading_GyroNick - AttitudeCorrectionNick;
if(Reading_IntegralGyroNick > TurnOver180Nick)
 
 
// 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)
{
Reading_IntegralGyroNick = -(TurnOver180Nick - 25000L);
Reading_IntegralGyroNick2 = Reading_IntegralGyroNick;
ReadingIntegralGyroRoll = -(TurnOver180Roll - 10000L);
ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll;
}
if(Reading_IntegralGyroNick < -TurnOver180Nick)
if(ReadingIntegralGyroRoll < -TurnOver180Roll)
{
Reading_IntegralGyroNick = (TurnOver180Nick - 25000L);
Reading_IntegralGyroNick2 = Reading_IntegralGyroNick;
ReadingIntegralGyroRoll = (TurnOver180Roll - 10000L);
ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll;
}
if(AdValueGyrNick < 15) Reading_GyroNick = -1000;
if(AdValueGyrNick < 7) Reading_GyroNick = -2000;
if(BoardRelease == 10)
 
// Nick
ReadingIntegralGyroNick2 += FilterGyroNick + TrimNick;
ReadingIntegralGyroNick += FilterGyroNick + TrimNick - AttitudeCorrectionNick;
if(ReadingIntegralGyroNick > TurnOver180Nick)
{
if(AdValueGyrNick > 1010) Reading_GyroNick = +1000;
if(AdValueGyrNick > 1017) Reading_GyroNick = +2000;
ReadingIntegralGyroNick = -(TurnOver180Nick - 25000L);
ReadingIntegralGyroNick2 = ReadingIntegralGyroNick;
}
else
if(ReadingIntegralGyroNick < -TurnOver180Nick)
{
if(AdValueGyrNick > 2020) Reading_GyroNick = +1000;
if(AdValueGyrNick > 2034) Reading_GyroNick = +2000;
ReadingIntegralGyroNick = (TurnOver180Nick - 25000L);
ReadingIntegralGyroNick2 = ReadingIntegralGyroNick;
}
 
// start ADC again to capture measurement values for the next loop
ADC_Enable();
IntegralGyroYaw = ReadingIntegralGyroYaw;
IntegralGyroNick = ReadingIntegralGyroNick;
IntegralGyroRoll = ReadingIntegralGyroRoll;
IntegralGyroNick2 = ReadingIntegralGyroNick2;
IntegralGyroRoll2 = ReadingIntegralGyroRoll2;
 
IntegralYaw = Reading_IntegralGyroYaw;
IntegralNick = Reading_IntegralGyroNick;
IntegralRoll = Reading_IntegralGyroRoll;
IntegralNick2 = Reading_IntegralGyroNick2;
IntegralRoll2 = Reading_IntegralGyroRoll2;
 
if((ParamSet.GlobalConfig & CFG_ROTARY_RATE_LIMITER) && !Looping_Nick && !Looping_Roll)
#define D_LIMIT 128
 
if(FCParam.GyroD)
{
if(Reading_GyroNick > 200) Reading_GyroNick += 4 * (Reading_GyroNick - 200);
else if(Reading_GyroNick < -200) Reading_GyroNick += 4 * (Reading_GyroNick + 200);
if(Reading_GyroRoll > 200) Reading_GyroRoll += 4 * (Reading_GyroRoll - 200);
else if(Reading_GyroRoll < -200) Reading_GyroRoll += 4 * (Reading_GyroRoll + 200);
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);
}
}
 
/************************************************************************/
/* Averaging Measurement Readings for Calibration */
/************************************************************************/
void CalibMean(void)
{
if(BoardRelease == 13) SearchGyroOffset();
// stop ADC to avoid changing values during calculation
ADC_Disable();
// 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;
 
Reading_GyroNick = AdValueGyrNick;
Reading_GyroRoll = AdValueGyrRoll;
Reading_GyroYaw = AdValueGyrYaw;
// 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);
}
 
Mean_AccNick = ACC_AMPLIFY * (int32_t)AdValueAccNick;
Mean_AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll;
Mean_AccTop = (int32_t)AdValueAccTop;
// start ADC (enables internal trigger so that the ISR in analog.c
// updates the readings once)
ADC_Enable();
 
TurnOver180Nick = (int32_t) ParamSet.AngleTurnOverNick * 2500L;
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
}
 
 
/************************************************************************/
/* Transmit Motor Data via I2C */
/************************************************************************/
387,21 → 531,44
{
if(!(MKFlags & MKFLAG_MOTOR_RUN))
{
Motor_Rear = 0;
Motor_Front = 0;
Motor_Right = 0;
Motor_Left = 0;
if(MotorTest[0]) Motor_Front = MotorTest[0];
if(MotorTest[1]) Motor_Rear = MotorTest[1];
if(MotorTest[2]) Motor_Left = MotorTest[2];
if(MotorTest[3]) Motor_Right = MotorTest[3];
MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off
}
DebugOut.Analog[12] = Motor_Front;
DebugOut.Analog[13] = Motor_Rear;
DebugOut.Analog[14] = Motor_Left;
DebugOut.Analog[15] = Motor_Right;
#ifdef USE_QUADRO
Motor1 = 0;
Motor2 = 0;
Motor3 = 0;
Motor4 = 0;
if(MotorTest[0]) Motor1 = MotorTest[0];
if(MotorTest[1]) Motor2 = MotorTest[1];
if(MotorTest[2]) Motor3 = MotorTest[2];
if(MotorTest[3]) Motor4 = MotorTest[3];
#else
Motor1 = 0;
Motor2 = 0;
Motor3 = 0;
Motor4 = 0;
Motor5 = 0;
Motor6 = 0;
Motor7 = 0;
Motor8 = 0;
if(MotorTest[0]) {Motor1 = MotorTest[0]; Motor2 = MotorTest[0];}
if(MotorTest[3]) {Motor3 = MotorTest[3]; Motor4 = MotorTest[3];}
if(MotorTest[1]) {Motor5 = MotorTest[1]; Motor6 = MotorTest[1];}
if(MotorTest[2]) {Motor7 = MotorTest[2]; Motor8 = MotorTest[2];}
 
#endif
MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off
}
#ifdef USE_QUADRO
 
DebugOut.Analog[12] = Motor1; // Front
DebugOut.Analog[13] = Motor2; // Rear
DebugOut.Analog[14] = Motor4; // Left
DebugOut.Analog[15] = Motor3; // Right
#else // OCTO Motor addresses are counted clockwise starting at the head
DebugOut.Analog[12] = (Motor1 + Motor2) / 2;
DebugOut.Analog[13] = (Motor5 + Motor6) / 2;
DebugOut.Analog[14] = (Motor7 + Motor8) / 2;
DebugOut.Analog[15] = (Motor3 + Motor4) / 2;
#endif
//Start I2C Interrupt Mode
twi_state = TWI_STATE_MOTOR_TX;
I2C_Start();
410,7 → 577,7
 
 
/************************************************************************/
/* Maps the parameter to poti values */
/* Map the parameter to poti values */
/************************************************************************/
void ParameterMapping(void)
{
421,13 → 588,14
#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.Height_D,ParamSet.Height_D,0,100);
CHK_POTI_MM(FCParam.Height_P,ParamSet.Height_P,0,100);
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.Gyro_P,ParamSet.Gyro_P,10,255);
CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I);
CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor);
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);
438,8 → 606,9
CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8);
CHK_POTI(FCParam.ServoNickControl,ParamSet.ServoNickControl);
CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit);
CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback);
CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback);
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);
455,7 → 624,7
CHK_POTI(FCParam.NaviSpeedCompensation,ParamSet.NaviSpeedCompensation);
#endif
CHK_POTI(FCParam.ExternalControl,ParamSet.ExternalControl);
Ki = (float) FCParam.I_Factor * FACTOR_I;
Ki = 10300 / ( FCParam.IFactor + 1 );
}
}
 
483,19 → 652,30
/************************************************************************/
void MotorControl(void)
{
int16_t MotorValue, pd_result, h, tmp_int;
int16_t YawMixFraction, GasMixFraction;
static int32_t SumNick = 0, SumRoll = 0;
int16_t MotorValue, 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 IntegralErrorNick = 0;
static int32_t IntegralErrorRoll = 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 int8_t TimerDebugOut = 0;
static uint16_t UpdateCompassCourse = 0;
static int32_t CorrectionNick, CorrectionRoll;
// high resolution motor values for smoothing of PID motor outputs
static int16_t MotorValue1 = 0, MotorValue2 = 0, MotorValue3 = 0, MotorValue4 = 0;
#ifndef USE_QUADRO
static int16_t MotorValue5 = 0, MotorValue6 = 0, MotorValue7 = 0, MotorValue8 = 0;
#endif
 
Mean();
GRN_ON;
504,7 → 684,7
// determine gas value
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMixFraction = StickGas;
if(GasMixFraction < ParamSet.Gas_Min + 10) GasMixFraction = ParamSet.Gas_Min + 10;
if(GasMixFraction < ParamSet.GasMin + 10) GasMixFraction = ParamSet.GasMin + 10;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// RC-signal is bad
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
514,17 → 694,17
{
if(BeepModulation == 0xFFFF)
{
BeepTime = 15000; // 1.5 seconds
BeepModulation = 0x0C00;
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()
MKFlags &= ~(MKFLAG_MOTOR_RUN|MKFLAG_EMERGENCY_LANDING); // clear motor run flag that stop the motors in SendMotorData()
}
RED_ON; // set red led
if(Model_Is_Flying > 1000) // wahrscheinlich in der Luft --> langsam absenken
if(ModelIsFlying > 1000) // wahrscheinlich in der Luft --> langsam absenken
{
GasMixFraction = ParamSet.EmergencyGas; // set emergency gas
MKFlags |= (MKFLAG_EMERGENCY_LANDING); // ser flag fpr emergency landing
549,17 → 729,17
RcLostTimer = ParamSet.EmergencyGasDuration * 50;
if(GasMixFraction > 40 && (MKFlags & MKFLAG_MOTOR_RUN) )
{
if(Model_Is_Flying < 0xFFFF) Model_Is_Flying++;
if(ModelIsFlying < 0xFFFF) ModelIsFlying++;
}
if(Model_Is_Flying < 256)
if(ModelIsFlying < 256)
{
SumNick = 0;
SumRoll = 0;
IPartNick = 0;
IPartRoll = 0;
StickYaw = 0;
if(Model_Is_Flying == 250)
if(ModelIsFlying == 250)
{
UpdateCompassCourse = 1;
Reading_IntegralGyroYaw = 0;
ReadingIntegralGyroYaw = 0;
SetPointYaw = 0;
}
}
606,7 → 786,7
{
delay_neutral = 0;
GRN_OFF;
Model_Is_Flying = 0;
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
634,7 → 814,7
// update active parameter set in eeprom
SetActiveParamSet(setting);
ParamSet_ReadFromEEProm(GetActiveParamSet());
SetNeutral();
SetNeutral(NO_ACC_CALIB);
Beep(GetActiveParamSet());
}
else
659,7 → 839,7
else // nick and roll are centered
{
ParamSet_ReadFromEEProm(GetActiveParamSet());
SetNeutral();
SetNeutral(NO_ACC_CALIB);
Beep(GetActiveParamSet());
}
}
666,7 → 846,7
else // nick and roll are centered
{
ParamSet_ReadFromEEProm(GetActiveParamSet());
SetNeutral();
SetNeutral(NO_ACC_CALIB);
Beep(GetActiveParamSet());
}
}
676,17 → 856,20
// 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;
SetParamWord(PID_ACC_NICK, 0xFFFF); // make value invalid
Model_Is_Flying = 0;
SetNeutral();
// Save ACC neutral settings to eeprom
SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX);
SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY);
SetParamWord(PID_ACC_Z, (uint16_t)NeutralAccZ);
ModelIsFlying = 0;
SetNeutral(ACC_CALIB);
Beep(GetActiveParamSet());
}
}
697,36 → 880,49
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < -85)
{
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// and yaw stick is rightmost --> start motors
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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
Model_Is_Flying = 1;
ModelIsFlying = 1;
MKFlags |= (MKFLAG_MOTOR_RUN|MKFLAG_START); // set flag RUN and START
SetPointYaw = 0;
Reading_IntegralGyroYaw = 0;
Reading_IntegralGyroNick = 0;
Reading_IntegralGyroRoll = 0;
Reading_IntegralGyroNick2 = IntegralNick;
Reading_IntegralGyroRoll2 = IntegralRoll;
SumNick = 0;
SumRoll = 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
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// and yaw stick is leftmost --> stop motors
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
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
Model_Is_Flying = 0;
ModelIsFlying = 0;
MKFlags &= ~(MKFLAG_MOTOR_RUN);
}
}
734,6 → 930,7
}
// remapping of paameters only if the signal rc-sigbnal conditions are good
} // eof RC_Quality > 150
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// new values from RC
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
741,21 → 938,30
{
ParameterMapping(); // remapping params (online poti replacement)
// calculate Stick inputs by rc channels (P) and changing of rc channels (D)
StickNick = (StickNick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_P) / 4;
StickNick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_D;
StickNick -= (GPS_Nick);
StickNick = (StickNick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.StickP) / 4;
StickNick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.StickD;
StickNick -= (GPSStickNick);
 
StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4;
StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D;
StickRoll -= (GPS_Roll);
StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.StickP) / 4;
StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.StickD;
StickRoll -= (GPSStickRoll);
 
// direct mapping of yaw and gas
// 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
Gyro_P_Factor = ((float) FCParam.Gyro_P + 10.0) / (256.0 / STICK_GAIN);
Gyro_I_Factor = ((float) FCParam.Gyro_I) / (44000 / STICK_GAIN);
GyroPFactor = FCParam.GyroP + 10;
GyroIFactor = FCParam.GyroI;
 
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
764,8 → 970,8
 
if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128)
{
StickNick += (int16_t) ExternControl.Nick * (int16_t) ParamSet.Stick_P;
StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P;
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;
773,14 → 979,9
if(StickGas < 0) StickGas = 0;
 
// disable I part of gyro control feedback
if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor = 0;
// avoid negative scaling factors
if(Gyro_P_Factor < 0) Gyro_P_Factor = 0;
if(Gyro_I_Factor < 0) Gyro_I_Factor = 0;
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;
798,48 → 999,49
// Looping?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_LEFT) Looping_Left = 1;
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_LEFT) LoopingLeft = 1;
else
{
if(Looping_Left) // Hysteresis
if(LoopingLeft) // Hysteresis
{
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Left = 0;
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) LoopingLeft = 0;
}
}
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_RIGHT) Looping_Right = 1;
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_RIGHT) LoopingRight = 1;
else
{
if(Looping_Right) // Hysteresis
if(LoopingRight) // Hysteresis
{
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0;
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) Looping_Top = 1;
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_UP) LoopingTop = 1;
else
{
if(Looping_Top) // Hysteresis
if(LoopingTop) // Hysteresis
{
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0;
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) Looping_Down = 1;
if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_DOWN) LoopingDown = 1;
else
{
if(Looping_Down) // Hysteresis
if(LoopingDown) // Hysteresis
{
if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0;
if(PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) LoopingDown = 0;
}
}
 
if(Looping_Left || Looping_Right) Looping_Roll = 1; else Looping_Roll = 0;
if(Looping_Top || Looping_Down) {Looping_Nick = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Nick = 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(Looping_Roll || Looping_Nick)
if(LoopingRoll || LoopingNick)
{
if(GasMixFraction > ParamSet.LoopGasLimit) GasMixFraction = ParamSet.LoopGasLimit;
FunnelCourse = 1;
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
851,10 → 1053,10
StickYaw = 0;
StickNick = 0;
StickRoll = 0;
Gyro_P_Factor = (float) 100 / (256.0 / STICK_GAIN);
Gyro_I_Factor = (float) 120 / (44000 / STICK_GAIN);
Looping_Roll = 0;
Looping_Nick = 0;
GyroPFactor = 90;
GyroIFactor = 120;
LoopingRoll = 0;
LoopingNick = 0;
MaxStickNick = 0;
MaxStickRoll = 0;
}
865,22 → 1067,22
 
#define BALANCE_NUMBER 256L
// sum for averaging
MeanIntegralNick += IntegralNick;
MeanIntegralRoll += IntegralRoll;
MeanIntegralGyroNick += IntegralGyroNick;
MeanIntegralGyroRoll += IntegralGyroRoll;
 
if(Looping_Nick || Looping_Roll) // if looping in any direction
if( LoopingNick || LoopingRoll) // if looping in any direction
{
// reset averaging for acc and gyro integral as well as gyro integral acc correction
MeasurementCounter = 0;
 
IntegralAccNick = 0;
IntegralAccRoll = 0;
MeanAccNick = 0;
MeanAccRoll = 0;
 
MeanIntegralNick = 0;
MeanIntegralRoll = 0;
MeanIntegralGyroNick = 0;
MeanIntegralGyroRoll = 0;
 
Reading_IntegralGyroNick2 = Reading_IntegralGyroNick;
Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll;
ReadingIntegralGyroNick2 = ReadingIntegralGyroNick;
ReadingIntegralGyroRoll2 = ReadingIntegralGyroRoll;
 
AttitudeCorrectionNick = 0;
AttitudeCorrectionRoll = 0;
887,16 → 1089,16
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!Looping_Nick && !Looping_Roll) // if not lopping in any direction
if(! LoopingNick && !LoopingRoll && (AdValueAccZ > 512)) // if not lopping in any direction or rapid falling
{
int32_t tmp_long, tmp_long2;
if(FCParam.Kalman_K != -1)
if( FCParam.KalmanK != -1)
{
// determine the deviation of gyro integral from averaged acceleration sensor
tmp_long = (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
tmp_long = (tmp_long * FCParam.Kalman_K) / (32 * 16);
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
tmp_long2 = (tmp_long2 * FCParam.Kalman_K) / (32 * 16);
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
{
909,17 → 1111,17
tmp_long2 /= 3;
}
// limit correction effect
if(tmp_long > (int32_t)FCParam.Kalman_MaxFusion) tmp_long = (int32_t)FCParam.Kalman_MaxFusion;
if(tmp_long < -(int32_t)FCParam.Kalman_MaxFusion) tmp_long =-(int32_t)FCParam.Kalman_MaxFusion;
if(tmp_long2 > (int32_t)FCParam.Kalman_MaxFusion) tmp_long2 = (int32_t)FCParam.Kalman_MaxFusion;
if(tmp_long2 <-(int32_t)FCParam.Kalman_MaxFusion) tmp_long2 =-(int32_t)FCParam.Kalman_MaxFusion;
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 averaged acceleration sensor
tmp_long = (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
// 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)(IntegralRoll / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
tmp_long2 = (int32_t)(IntegralGyroRoll / ParamSet.GyroAccFactor - (int32_t)AccRoll);
tmp_long2 /= 16;
 
if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
935,14 → 1137,12
 
#define BALANCE 32
// limit correction effect
if(tmp_long > BALANCE) tmp_long = BALANCE;
if(tmp_long < -BALANCE) tmp_long =-BALANCE;
if(tmp_long2 > BALANCE) tmp_long2 = BALANCE;
if(tmp_long2 <-BALANCE) tmp_long2 =-BALANCE;
CHECK_MIN_MAX(tmp_long, -BALANCE, BALANCE);
CHECK_MIN_MAX(tmp_long2, -BALANCE, BALANCE);
}
// correct current readings
Reading_IntegralGyroNick -= tmp_long;
Reading_IntegralGyroRoll -= tmp_long2;
ReadingIntegralGyroNick -= tmp_long;
ReadingIntegralGyroRoll -= tmp_long2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// MeasurementCounter is incremented in the isr of analog.c
950,32 → 1150,32
{
static int16_t cnt = 0;
static int8_t last_n_p, last_n_n, last_r_p, last_r_n;
static int32_t MeanIntegralNick_old, MeanIntegralRoll_old;
static int32_t MeanIntegralGyroNick_old, MeanIntegralGyroRoll_old;
 
// if not lopping in any direction (this should be alwais the case,
// 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(!Looping_Nick && !Looping_Roll && !FunnelCourse)
if(! LoopingNick && !LoopingRoll && !FunnelCourse && ParamSet.DriftComp)
{
// Calculate mean value of the gyro integrals
MeanIntegralNick /= BALANCE_NUMBER;
MeanIntegralRoll /= BALANCE_NUMBER;
MeanIntegralGyroNick /= BALANCE_NUMBER;
MeanIntegralGyroRoll /= BALANCE_NUMBER;
 
// Calculate mean of the acceleration values
IntegralAccNick = (ParamSet.GyroAccFactor * IntegralAccNick) / BALANCE_NUMBER;
IntegralAccRoll = (ParamSet.GyroAccFactor * IntegralAccRoll ) / 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
IntegralErrorNick = (int32_t)(MeanIntegralNick - (int32_t)IntegralAccNick);
CorrectionNick = IntegralErrorNick / ParamSet.GyroAccTrim;
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
IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
CorrectionRoll = IntegralErrorRoll / ParamSet.GyroAccTrim;
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.Kalman_K == -1) )
if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FCParam.KalmanK == -1) )
{
AttitudeCorrectionNick /= 2;
AttitudeCorrectionRoll /= 2;
984,15 → 1184,18
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gyro-Drift ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// deviation of gyro nick integral (IntegralNick is corrected by averaged acc sensor)
IntegralErrorNick = IntegralNick2 - IntegralNick;
Reading_IntegralGyroNick2 -= IntegralErrorNick;
// deviation of gyro nick integral (IntegralNick is corrected by averaged acc sensor)
IntegralErrorRoll = IntegralRoll2 - IntegralRoll;
Reading_IntegralGyroRoll2 -= IntegralErrorRoll;
// 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(YawGyroDrift > BALANCE_NUMBER/2) AdNeutralYaw++;
if(YawGyroDrift < -BALANCE_NUMBER/2) AdNeutralYaw--;
if(ParamSet.DriftComp)
{
if(YawGyroDrift > BALANCE_NUMBER/2) AdBiasGyroYaw++;
if(YawGyroDrift < -BALANCE_NUMBER/2) AdBiasGyroYaw--;
}
YawGyroDrift = 0;
 
#define ERROR_LIMIT (BALANCE_NUMBER * 4)
999,16 → 1202,16
#define ERROR_LIMIT2 (BALANCE_NUMBER * 16)
#define MOVEMENT_LIMIT 20000
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralErrorNick) / 4096;
cnt = 1;// + labs(IntegralGyroNickError) / 4096;
CorrectionNick = 0;
if((labs(MeanIntegralNick_old - MeanIntegralNick) < MOVEMENT_LIMIT) || (FCParam.Kalman_MaxDrift > 3* 16))
if((labs(MeanIntegralGyroNick_old - MeanIntegralGyroNick) < MOVEMENT_LIMIT) || (FCParam.KalmanMaxDrift > 3 * 8))
{
if(IntegralErrorNick > ERROR_LIMIT2)
if(IntegralGyroNickError > ERROR_LIMIT2)
{
if(last_n_p)
{
cnt += labs(IntegralErrorNick) / ERROR_LIMIT2;
CorrectionNick = IntegralErrorNick / 8;
cnt += labs(IntegralGyroNickError) / (ERROR_LIMIT2 / 8);
CorrectionNick = IntegralGyroNickError / 8;
if(CorrectionNick > 5000) CorrectionNick = 5000;
AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER;
}
1015,12 → 1218,12
else last_n_p = 1;
}
else last_n_p = 0;
if(IntegralErrorNick < -ERROR_LIMIT2)
if(IntegralGyroNickError < -ERROR_LIMIT2)
{
if(last_n_n)
{
cnt += labs(IntegralErrorNick) / ERROR_LIMIT2;
CorrectionNick = IntegralErrorNick / 8;
cnt += labs(IntegralGyroNickError) / (ERROR_LIMIT2 / 8);
CorrectionNick = IntegralGyroNickError / 8;
if(CorrectionNick < -5000) CorrectionNick = -5000;
AttitudeCorrectionNick += CorrectionNick / BALANCE_NUMBER;
}
1034,22 → 1237,22
BadCompassHeading = 1000;
}
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
if(cnt * 16 > FCParam.Kalman_MaxDrift) cnt = FCParam.Kalman_MaxDrift / 16;
if(FCParam.KalmanMaxDrift) if(cnt > FCParam.KalmanMaxDrift) cnt = FCParam.KalmanMaxDrift;
// correct Gyro Offsets
if(IntegralErrorNick > ERROR_LIMIT) AdNeutralNick += cnt;
if(IntegralErrorNick < -ERROR_LIMIT) AdNeutralNick -= cnt;
if(IntegralGyroNickError > ERROR_LIMIT) BiasHiResGyroNick += cnt;
if(IntegralGyroNickError < -ERROR_LIMIT) BiasHiResGyroNick -= cnt;
 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralErrorNick) / 4096;
cnt = 1;// + labs(IntegralGyroNickError) / 4096;
CorrectionRoll = 0;
if((labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT) || (FCParam.Kalman_MaxDrift > 3 * 16))
if((labs(MeanIntegralGyroRoll_old - MeanIntegralGyroRoll) < MOVEMENT_LIMIT) || (FCParam.KalmanMaxDrift > 3 * 8))
{
if(IntegralErrorRoll > ERROR_LIMIT2)
if(IntegralGyroRollError > ERROR_LIMIT2)
{
if(last_r_p)
{
cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2;
CorrectionRoll = IntegralErrorRoll / 8;
cnt += labs(IntegralGyroRollError) / (ERROR_LIMIT2 / 8);
CorrectionRoll = IntegralGyroRollError / 8;
if(CorrectionRoll > 5000) CorrectionRoll = 5000;
AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
}
1056,12 → 1259,12
else last_r_p = 1;
}
else last_r_p = 0;
if(IntegralErrorRoll < -ERROR_LIMIT2)
if(IntegralGyroRollError < -ERROR_LIMIT2)
{
if(last_r_n)
{
cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2;
CorrectionRoll = IntegralErrorRoll / 8;
cnt += labs(IntegralGyroRollError) / (ERROR_LIMIT2 / 8);
CorrectionRoll = IntegralGyroRollError / 8;
if(CorrectionRoll < -5000) CorrectionRoll = -5000;
AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
}
1076,9 → 1279,9
}
// correct Gyro Offsets
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
if(cnt * 16 > FCParam.Kalman_MaxDrift) cnt = FCParam.Kalman_MaxDrift / 16;
if(IntegralErrorRoll > ERROR_LIMIT) AdNeutralRoll += cnt;
if(IntegralErrorRoll < -ERROR_LIMIT) AdNeutralRoll -= cnt;
if(FCParam.KalmanMaxDrift) if(cnt > FCParam.KalmanMaxDrift) cnt = FCParam.KalmanMaxDrift;
if(IntegralGyroRollError > ERROR_LIMIT) BiasHiResGyroRoll += cnt;
if(IntegralGyroRollError < -ERROR_LIMIT) BiasHiResGyroRoll -= cnt;
 
}
else // looping is active
1088,21 → 1291,21
FunnelCourse = 0;
}
 
// if Gyro_I_Factor == 0 , for example at Heading Hold, ignore attitude correction
if(!Gyro_I_Factor)
// if GyroIFactor == 0 , for example at Heading Hold, ignore attitude correction
if(!GyroIFactor)
{
AttitudeCorrectionRoll = 0;
AttitudeCorrectionNick = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
MeanIntegralNick_old = MeanIntegralNick;
MeanIntegralRoll_old = MeanIntegralRoll;
MeanIntegralGyroNick_old = MeanIntegralGyroNick;
MeanIntegralGyroRoll_old = MeanIntegralGyroRoll;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
// reset variables used for averaging
IntegralAccNick = 0;
IntegralAccRoll = 0;
MeanIntegralNick = 0;
MeanIntegralRoll = 0;
// reset variables used for next averaging
MeanAccNick = 0;
MeanAccRoll = 0;
MeanIntegralGyroNick = 0;
MeanIntegralGyroRoll = 0;
MeasurementCounter = 0;
} // end of averaging
 
1119,14 → 1322,13
}
}
// exponential stick sensitivity in yawring rate
tmp_int = (int32_t) ParamSet.Yaw_P * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx²
tmp_int += (ParamSet.Yaw_P * StickYaw) / 4;
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 Reading_IntegralGyroYaw with SetPointYaw(StickYaw)
Reading_IntegralGyroYaw -= tmp_int;
// trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw)
ReadingIntegralGyroYaw -= tmp_int;
// limit the effect
if(Reading_IntegralGyroYaw > 50000) Reading_IntegralGyroYaw = 50000;
if(Reading_IntegralGyroYaw <-50000) Reading_IntegralGyroYaw =-50000;
CHECK_MIN_MAX(ReadingIntegralGyroYaw, -50000, 50000)
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Compass
1155,17 → 1357,16
#endif
 
// get maximum attitude angle
w = abs(IntegralNick / 512);
v = abs(IntegralRoll / 512);
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 / YAW_GYRO_DEG_FACTOR)) % 360) - 180;
if(UpdateCompassCourse)
else error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180;
if(abs(GyroYaw) > 128) // spinning fast
{
error = 0;
YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR;
}
if(!BadCompassHeading && w < 25)
{
1173,7 → 1374,8
if(UpdateCompassCourse)
{
BeepTime = 200;
CompassCourse = (YawGyroHeading / YAW_GYRO_DEG_FACTOR);
YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;
CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR);
UpdateCompassCourse = 0;
}
}
1186,13 → 1388,13
{
v = 64 + (MaxStickNick + MaxStickRoll) / 8;
// calc course deviation
r = ((540 + (YawGyroHeading / YAW_GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180;
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;
Reading_IntegralGyroYaw += v;
ReadingIntegralGyroYaw += v;
}
else
{ // wait a while
1217,8 → 1419,8
}
else
{
GPS_Nick = 0;
GPS_Roll = 0;
GPSStickNick = 0;
GPSStickRoll = 0;
}
#endif
 
1228,25 → 1430,25
if(!TimerDebugOut--)
{
TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz)
DebugOut.Analog[0] = IntegralNick / ParamSet.GyroAccFactor;
DebugOut.Analog[1] = IntegralRoll / ParamSet.GyroAccFactor;
DebugOut.Analog[2] = Mean_AccNick;
DebugOut.Analog[3] = Mean_AccRoll;
DebugOut.Analog[4] = Reading_GyroYaw;
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] = (Reading_Integral_Top / 512);
DebugOut.Analog[6] = (ReadingIntegralTop / 512);
DebugOut.Analog[8] = CompassHeading;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = RC_Quality;
DebugOut.Analog[11] = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
//DebugOut.Analog[16] = Mean_AccTop;
//DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
//DebugOut.Analog[18] = FromNaviCtrl_Value.OsdBar;
DebugOut.Analog[27] = (int16_t)FCParam.Kalman_MaxDrift;
DebugOut.Analog[29] = (int16_t)FCParam.Kalman_K;
DebugOut.Analog[30] = GPS_Nick;
DebugOut.Analog[31] = GPS_Roll;
 
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[29] = (int16_t)FCParam.KalmanK;
DebugOut.Analog[30] = GPSStickNick;
DebugOut.Analog[31] = GPSStickRoll;
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1253,23 → 1455,52
// calculate control feedback from angle (gyro integral) and agular velocity (gyro signal)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
if(Looping_Nick) Reading_GyroNick = Reading_GyroNick * Gyro_P_Factor;
else Reading_GyroNick = IntegralNick * Gyro_I_Factor + Reading_GyroNick * Gyro_P_Factor;
if(Looping_Roll) Reading_GyroRoll = Reading_GyroRoll * Gyro_P_Factor;
else Reading_GyroRoll = IntegralRoll * Gyro_I_Factor + Reading_GyroRoll * Gyro_P_Factor;
Reading_GyroYaw = Reading_GyroYaw * (2 * Gyro_P_Factor) + IntegralYaw * Gyro_I_Factor / 2;
#define TRIM_LIMIT 200
CHECK_MIN_MAX(TrimNick, -TRIM_LIMIT, TRIM_LIMIT);
CHECK_MIN_MAX(TrimRoll, -TRIM_LIMIT, TRIM_LIMIT);
 
DebugOut.Analog[21] = Reading_GyroNick;
DebugOut.Analog[22] = Reading_GyroRoll;
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
 
// octo has a double yaw momentum because of the doubled motor number
// therefore double D-Part and halfen P-Part for the same result
#ifdef USE_OCTO
PDPartYaw = (int32_t)(GyroYaw * 4 * (int32_t)GyroPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroIFactor) / (4 * (44000 / STICK_GAIN));
#else
PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroIFactor) / (2 * (44000 / STICK_GAIN));
#endif
 
//DebugOut.Analog[21] = PDPartNick;
//DebugOut.Analog[22] = PDPartRoll;
 
// limit control feedback
#define MAX_SENSOR (4096 * STICK_GAIN)
if(Reading_GyroNick > MAX_SENSOR) Reading_GyroNick = MAX_SENSOR;
if(Reading_GyroNick < -MAX_SENSOR) Reading_GyroNick = -MAX_SENSOR;
if(Reading_GyroRoll > MAX_SENSOR) Reading_GyroRoll = MAX_SENSOR;
if(Reading_GyroRoll < -MAX_SENSOR) Reading_GyroRoll = -MAX_SENSOR;
if(Reading_GyroYaw > MAX_SENSOR) Reading_GyroYaw = MAX_SENSOR;
if(Reading_GyroYaw < -MAX_SENSOR) Reading_GyroYaw = -MAX_SENSOR;
#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);
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Height Control
1333,15 → 1564,14
// if current height is above the setpoint reduce gas
if((h > SetPointHeight) && HeightControlActive)
{
// GasMixFraction - HightDeviation * P - HeightChange * D - ACCTop * DACC
// height difference -> P control part
h = ((h - SetPointHeight) * (int16_t) FCParam.Height_P) / (16 / STICK_GAIN);
h = ((h - SetPointHeight) * (int16_t) FCParam.HeightP) / (16 / STICK_GAIN);
h = GasMixFraction - h; // reduce gas
// height gradient --> D control part
//h -= (HeightD * FCParam.Height_D) / (8 / STICK_GAIN); // 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 = ((Reading_Integral_Top / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN);
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;
1348,11 → 1578,11
// update height control gas
HeightControlGas = (HeightControlGas*15 + h) / 16;
// limit gas reduction
if(HeightControlGas < ParamSet.Height_MinGas * STICK_GAIN)
if(HeightControlGas < ParamSet.HeightMinGas * STICK_GAIN)
{
if(GasMixFraction >= ParamSet.Height_MinGas * STICK_GAIN) HeightControlGas = ParamSet.Height_MinGas * 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.Height_MinGas * STICK_GAIN) HeightControlGas = GasMixFraction;
if(GasMixFraction < ParamSet.HeightMinGas * STICK_GAIN) HeightControlGas = GasMixFraction;
}
// limit gas to stick setting
if(HeightControlGas > GasMixFraction) HeightControlGas = GasMixFraction;
1360,87 → 1590,255
}
}
// limit gas to parameter setting
if(GasMixFraction > (ParamSet.Gas_Max - 20) * STICK_GAIN) GasMixFraction = (ParamSet.Gas_Max - 20) * STICK_GAIN;
if(GasMixFraction > (ParamSet.GasMax - 20) * STICK_GAIN) GasMixFraction = (ParamSet.GasMax - 20) * STICK_GAIN;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Mixer and PI-Controller
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.Analog[7] = GasMixFraction;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Yaw-Fraction
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
YawMixFraction = Reading_GyroYaw - SetPointYaw * STICK_GAIN; // yaw controller
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)
{
if(YawMixFraction > (GasMixFraction / 2)) YawMixFraction = GasMixFraction / 2;
if(YawMixFraction < -(GasMixFraction / 2)) YawMixFraction = -(GasMixFraction / 2);
CHECK_MIN_MAX(YawMixFraction, -(GasMixFraction / 2), (GasMixFraction / 2));
}
else
{
if(YawMixFraction > (MIN_YAWGAS / 2)) YawMixFraction = MIN_YAWGAS / 2;
if(YawMixFraction < -(MIN_YAWGAS / 2)) YawMixFraction = -(MIN_YAWGAS / 2);
CHECK_MIN_MAX(YawMixFraction, -(MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
}
tmp_int = ParamSet.Gas_Max * STICK_GAIN;
if(YawMixFraction > ((tmp_int - GasMixFraction))) YawMixFraction = ((tmp_int - GasMixFraction));
if(YawMixFraction < -((tmp_int - GasMixFraction))) YawMixFraction = -((tmp_int - GasMixFraction));
tmp_int = ParamSet.GasMax * STICK_GAIN;
CHECK_MIN_MAX(YawMixFraction, -(tmp_int - GasMixFraction), (tmp_int - GasMixFraction));
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Nick-Axis
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffNick = Reading_GyroNick - StickNick; // get difference
if(Gyro_I_Factor) SumNick += IntegralNick * Gyro_I_Factor - StickNick; // I-part for attitude control
else SumNick += DiffNick; // I-part for head holding
if(SumNick > (STICK_GAIN * 16000L)) SumNick = (STICK_GAIN * 16000L);
if(SumNick < -(STICK_GAIN * 16000L)) SumNick = -(STICK_GAIN * 16000L);
pd_result = DiffNick + Ki * SumNick; // PI-controller for nick
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
 
tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction)/2)) / 64;
if(pd_result > tmp_int) pd_result = tmp_int;
if(pd_result < -tmp_int) pd_result = -tmp_int;
 
// Motor Front
MotorValue = GasMixFraction + pd_result + YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Front = MotorValue;
 
// Motor Rear
MotorValue = GasMixFraction - pd_result + YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Rear = MotorValue;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Roll-Axis
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffRoll = Reading_GyroRoll - StickRoll; // get difference
if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - StickRoll; // I-part for attitude control
else SumRoll += DiffRoll; // I-part for head holding
if(SumRoll > (STICK_GAIN * 16000L)) SumRoll = (STICK_GAIN * 16000L);
if(SumRoll < -(STICK_GAIN * 16000L)) SumRoll = -(STICK_GAIN * 16000L);
pd_result = DiffRoll + Ki * SumRoll; // PI-controller for roll
tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction)/2)) / 64;
if(pd_result > tmp_int) pd_result = tmp_int;
if(pd_result < -tmp_int) pd_result = -tmp_int;
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
 
// Motor Left
MotorValue = GasMixFraction + pd_result - YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Left = MotorValue;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 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);
 
// Motor Right
MotorValue = GasMixFraction - pd_result - YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Right = MotorValue;
#ifdef USE_QUADRO
 
// QuadroKopter Mixer
 
// Motor Front (++)
MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
MotorValue = MotorValue1 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor1 = MotorValue;
 
// Motor Rear (-+)
MotorValue = GasMixFraction - NickMixFraction + YawMixFraction;
MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
MotorValue = MotorValue2 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor2 = MotorValue;
 
// Motor Right (--)
MotorValue = GasMixFraction - RollMixFraction - YawMixFraction;
MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
MotorValue = MotorValue3 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor3 = MotorValue;
 
// Motor Left (+-)
MotorValue = GasMixFraction + RollMixFraction - YawMixFraction;
MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
MotorValue = MotorValue4 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor4 = MotorValue;
 
#endif
 
#ifdef USE_OCTO
 
// OctoKopter Mixer
 
// Motor 1 (+++)
MotorValue = GasMixFraction + NickMixFraction + RollMixFraction + YawMixFraction;
MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
MotorValue = MotorValue1 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor1= MotorValue;
 
// Motor 2 (+--)
MotorValue = GasMixFraction + NickMixFraction - RollMixFraction - YawMixFraction;
MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
MotorValue = MotorValue2 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor2 = MotorValue;
 
// Motor 3 (+-+)
MotorValue = GasMixFraction + NickMixFraction - RollMixFraction + YawMixFraction;
MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
MotorValue = MotorValue3 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor3 = MotorValue;
 
// Motor 4 (---)
MotorValue = GasMixFraction - NickMixFraction - RollMixFraction - YawMixFraction;
MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
MotorValue = MotorValue4 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor4 = MotorValue;
 
// Motor 5 (--+)
MotorValue = GasMixFraction - NickMixFraction - RollMixFraction + YawMixFraction;
MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
MotorValue = MotorValue5 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor5 = MotorValue;
 
// Motor 6 (-+-)
MotorValue = GasMixFraction - NickMixFraction + RollMixFraction - YawMixFraction;
MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
MotorValue = MotorValue6 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor6 = MotorValue;
 
// Motor7 (-++)
MotorValue = GasMixFraction - NickMixFraction + RollMixFraction + YawMixFraction;
MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
MotorValue = MotorValue7 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor7 = MotorValue;
 
// Motor8 (++-)
MotorValue = GasMixFraction + NickMixFraction + RollMixFraction - YawMixFraction;
MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
MotorValue = MotorValue8 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor8 = MotorValue;
#endif
 
#ifdef USE_OCTO2
 
// Octokopter Mixer alternativ setup
 
MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
MotorValue = MotorValue1 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor1 = MotorValue;
 
MotorValue = GasMixFraction + NickMixFraction - RollMixFraction - YawMixFraction;
MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
MotorValue = MotorValue2 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor2 = MotorValue;
 
MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
MotorValue = MotorValue3 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor3 = MotorValue;
 
MotorValue = GasMixFraction - NickMixFraction - RollMixFraction - YawMixFraction;
MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
MotorValue = MotorValue4 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor4 = MotorValue;
 
MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
MotorValue = MotorValue5 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor5 = MotorValue;
 
MotorValue = GasMixFraction - NickMixFraction + RollMixFraction - YawMixFraction;
MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
MotorValue = MotorValue6 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor6 = MotorValue;
 
MotorValue = GasMixFraction + RollMixFraction + YawMixFraction;
MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
MotorValue = MotorValue7 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor7 = MotorValue;
 
MotorValue = GasMixFraction + NickMixFraction + RollMixFraction - YawMixFraction;
MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
MotorValue = MotorValue8 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor8 = MotorValue;
#endif
 
#ifdef USE_OCTO3
 
// Octokopter Mixer alternativ setup
 
MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
MotorValue = MotorValue1 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor1 = MotorValue;
 
MotorValue = GasMixFraction + NickMixFraction - YawMixFraction;
MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
MotorValue = MotorValue2 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor2 = MotorValue;
 
MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
MotorValue = MotorValue3 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor3 = MotorValue;
 
MotorValue = GasMixFraction - RollMixFraction - YawMixFraction;
MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
MotorValue = MotorValue4 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor4 = MotorValue;
 
MotorValue = GasMixFraction - NickMixFraction + YawMixFraction;
MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
MotorValue = MotorValue5 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor5 = MotorValue;
 
MotorValue = GasMixFraction - NickMixFraction - YawMixFraction;
MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
MotorValue = MotorValue6 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor6 = MotorValue;
 
MotorValue = GasMixFraction + RollMixFraction + YawMixFraction;
MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
MotorValue = MotorValue7 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor7 = MotorValue;
 
MotorValue = GasMixFraction + RollMixFraction - YawMixFraction;
MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
MotorValue = MotorValue8 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor8 = MotorValue;
#endif
 
 
}
 
/branches/V0.72p Code Redesign killagreg/fc.h
7,20 → 7,37
 
#include <inttypes.h>
 
#define YAW_GYRO_DEG_FACTOR 1291L // Factor between Yaw Gyro Integral and HeadingAngle in deg
#define STICK_GAIN 4
// 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)
 
 
typedef struct
{
uint8_t Height_D;
uint8_t HeightD;
uint8_t MaxHeight;
uint8_t Height_P;
uint8_t HeightP;
uint8_t Height_ACC_Effect;
uint8_t CompassYawEffect;
uint8_t Gyro_P;
uint8_t Gyro_I;
uint8_t Yaw_P;
uint8_t I_Factor;
uint8_t GyroD;
uint8_t GyroP;
uint8_t GyroI;
uint8_t StickYawP;
uint8_t IFactor;
uint8_t UserParam1;
uint8_t UserParam2;
uint8_t UserParam3;
31,8 → 48,9
uint8_t UserParam8;
uint8_t ServoNickControl;
uint8_t LoopGasLimit;
uint8_t Yaw_PosFeedback;
uint8_t Yaw_NegFeedback;
uint8_t AxisCoupling1;
uint8_t AxisCoupling2;
uint8_t AxisCouplingYawCorrection;
uint8_t DynamicStability;
uint8_t ExternalControl;
uint8_t J16Timing;
48,30 → 66,33
uint8_t NaviWindCorrection;
uint8_t NaviSpeedCompensation;
#endif
int8_t Kalman_K;
int8_t Kalman_MaxDrift;
int8_t Kalman_MaxFusion;
int8_t KalmanK;
int8_t KalmanMaxDrift;
int8_t KalmanMaxFusion;
} fc_param_t;
 
extern fc_param_t FCParam;
 
// attitude
extern int32_t IntegralNick, IntegralRoll, IntegralYaw;
extern int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
 
// offsets
extern int16_t AdNeutralNick, AdNeutralRoll, AdNeutralYaw;
extern volatile int16_t NeutralAccX, NeutralAccY;
extern volatile float NeutralAccZ;
// rotation rates
extern int16_t GyroNick, GyroRoll, GyroYaw;
 
// attitude calcualted by temporal integral of gyro rates
extern int32_t IntegralGyroNick, IntegralGyroRoll, IntegralGyroYaw;
 
extern volatile int32_t Reading_Integral_Top; // calculated in analog.c
 
// 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 volatile int16_t CompassHeading;
extern volatile int16_t CompassCourse;
extern volatile int16_t CompassOffCourse;
extern volatile uint8_t CompassCalState;
extern int16_t CompassHeading;
extern int16_t CompassCourse;
extern int16_t CompassOffCourse;
extern uint8_t CompassCalState;
extern int32_t YawGyroHeading;
extern int16_t YawGyroHeadingInDeg;
 
79,8 → 100,8
extern int ReadingHeight;
extern int SetPointHeight;
 
// mean accelerations
extern int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop;
// accelerations
extern int16_t AccNick, AccRoll, AccTop;
 
// acceleration send to navi board
extern int16_t NaviAccNick, NaviAccRoll, NaviCntAcc;
92,12 → 113,12
// external control
extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw;
 
#define ACC_CALIB 1
#define NO_ACC_CALIB 0
 
void MotorControl(void);
void SendMotorData(void);
void CalibMean(void);
void Mean(void);
void SetNeutral(void);
void SetNeutral(uint8_t AccAdjustment);
void Beep(uint8_t numbeeps);
 
 
104,20 → 125,21
extern int16_t Poti1, Poti2, Poti3, Poti4, Poti5, Poti6, Poti7, Poti8;
 
// setpoints for motors
extern volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; //used by twimaster isr
extern volatile uint8_t Motor1, Motor2, Motor3, Motor4, Motor5, Motor6, Motor7, Motor8;
 
// current stick values
extern int16_t StickNick;
extern int16_t StickRoll;
extern int16_t StickYaw;
extern int16_t GPS_Nick;
extern int16_t GPS_Roll;
// current GPS-stick values
extern int16_t GPSStickNick;
extern int16_t GPSStickRoll;
 
// current stick elongations
extern int16_t MaxStickNick, MaxStickRoll, MaxStickYaw;
 
 
extern uint16_t Model_Is_Flying;
extern uint16_t ModelIsFlying;
 
 
// MKFlags
130,7 → 152,7
#define MKFLAG_RESERVE2 0x40
#define MKFLAG_RESERVE3 0x80
 
volatile extern uint8_t MKFlags;
extern uint8_t MKFlags;
 
#endif //_FC_H
 
/branches/V0.72p Code Redesign killagreg/gps.c
1,3 → 1,53
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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"
4,7 → 54,7
#include "ubx.h"
#include "mymath.h"
#include "timer0.h"
#include "uart.h"
#include "uart0.h"
#include "rc.h"
#include "eeprom.h"
 
140,8 → 190,8
// disable GPS control sticks
void GPS_Neutral(void)
{
GPS_Nick = 0;
GPS_Roll = 0;
GPSStickNick = 0;
GPSStickRoll = 0;
}
 
// calculates the GPS control stick values from the deviation to target position
250,12 → 300,12
// 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 GPS_Nick and GPS_Roll variable is contrarily to the stick values
// 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
// GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll.
// GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll.
 
coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR);
sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR);
coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GyroDegFactor);
sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GyroDegFactor);
PID_Nick = (coscompass * PID_North + sincompass * PID_East) / 8192;
PID_Roll = (sincompass * PID_North - coscompass * PID_East) / 8192;
 
263,8 → 313,8
// limit resulting GPS control vector
GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
 
GPS_Nick = (int16_t)PID_Nick;
GPS_Roll = (int16_t)PID_Roll;
GPSStickNick = (int16_t)PID_Nick;
GPSStickRoll = (int16_t)PID_Roll;
}
else // invalid GPS data or bad compass reading
{
/branches/V0.72p Code Redesign killagreg/led.c
1,3 → 1,53
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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"
/branches/V0.72p Code Redesign killagreg/main.c
53,14 → 53,13
 
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
 
#include "main.h"
#include "timer0.h"
#include "timer2.h"
#include "uart.h"
#if defined (__AVR_ATmega644P__)
#include "uart0.h"
#include "uart1.h"
#endif
#include "led.h"
#include "menu.h"
#include "fc.h"
78,42 → 77,66
#endif
#include "twimaster.h"
#include "eeprom.h"
#include "_Settings.h"
 
 
uint8_t BoardRelease = 10;
uint8_t CPUType = ATMEGA644;
 
 
//############################################################################
//Hauptprogramm
int main (void)
//############################################################################
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)
{
unsigned int timer;
uint8_t BoardRelease = 10;
// the board release is coded via the pull up or down the 2 status LED
 
// disable interrupts global
cli();
PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate
DDRB &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input
 
// get board release
DDRB = 0x00;
PORTB = 0x00;
for(timer = 0; timer < 1000; timer++); // make some delay
if(PINB & (1<<PINB0))
_delay_loop_2(1000); // make some delay
 
switch( PINB & ((1<<PINB1)|(1<<PINB0)) )
{
if(PINB & (1<<PINB1)) BoardRelease = 13;
else BoardRelease = 11; // 12 is the same hardware
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;
}
else
{
if(PINB & (1<<PINB1)) BoardRelease = 20; //
else BoardRelease = 10;
}
 
// set LED ports as output
DDRB |= (1<<DDB1)|(1<<DDB0);
RED_ON;
GRN_OFF;
return BoardRelease;
}
 
 
int16_t main (void)
{
unsigned int timer;
 
// disable interrupts global
cli();
 
// analyze hardware environment
CPUType = GetCPUType();
BoardRelease = GetBoardRelease();
 
// disable watchdog
MCUSR &=~(1<<WDRF);
WDTCSR |= (1<<WDCE)|(1<<WDE);
129,20 → 152,14
RED_OFF;
 
// initalize modules
//LED_Init(); Is done within ParamSet_Init() below
LED_Init();
TIMER0_Init();
TIMER2_Init();
USART0_Init();
 
#if defined (__AVR_ATmega644P__)
if (BoardRelease > 10) USART1_Init();
#endif
 
if(CPUType == ATMEGA644P) USART1_Init();
RC_Init();
ADC_Init();
I2C_Init();
 
 
#ifdef USE_NAVICTRL
SPI_MasterInit();
#endif
156,30 → 173,35
// enable interrupts global
sei();
 
printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",BoardRelease/10,BoardRelease%10, VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
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
// Parameter Set handling
ParamSet_Init();
 
if(GetParamWord(PID_ACC_NICK) > 1023)
{
printf("\n\rACC not calibrated!");
}
if(GetParamWord(PID_ACC_NICK) > 1023)
{
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");
{
printf("\n\rCalibrating air pressure sensor..");
timer = SetDelay(1000);
SearchAirPressureOffset();
while (!CheckDelay(timer));
printf("OK\n\r");
}
 
#ifdef USE_NAVICTRL
194,31 → 216,18
printf("\n\rSupport for MK3MAG Compass");
#endif
 
 
 
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
#if defined (__AVR_ATmega644P__)
if(BoardRelease == 10)
{
printf("\n\rSupport for GPS at 1st UART");
}
else
{
printf("\n\rSupport for GPS at 2nd UART");
}
#else // (__AVR_ATmega644__)
printf("\n\rSupport for GPS at 1st UART");
if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART");
else printf("\n\rSupport for GPS at 1st UART");
#endif
#endif
 
 
SetNeutral(NO_ACC_CALIB);
 
SetNeutral();
 
RED_OFF;
 
BeepTime = 2000;
ExternControl.Digital[0] = 0x55;
BeepTime = 2000;
ExternControl.Digital[0] = 0x55;
 
 
printf("\n\rControl: ");
227,34 → 236,35
 
printf("\n\n\r");
 
LCD_Clear();
LCD_Clear();
 
I2CTimeout = 5000;
 
I2CTimeout = 5000;
while (1)
{
if(UpdateMotor) // control interval
{
UpdateMotor = 0; // reset Flag, is enabled every 2 ms by isr of timer0
//PORTD |= (1<<PORTD4);
MotorControl();
//PORTD &= ~(1<<PORTD4);
if(UpdateMotor && ADReady) // control interval
{
UpdateMotor = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
 
SendMotorData();
//J4HIGH;
MotorControl();
//J4LOW;
 
RED_OFF;
SendMotorData(); // the flight control code
RED_OFF;
 
if(PcAccess) PcAccess--;
else
{
ExternControl.Config = 0;
ExternStickNick= 0;
ExternStickRoll = 0;
ExternStickYaw = 0;
}
 
if(!I2CTimeout)
{
 
if(PcAccess) PcAccess--;
else
{
ExternControl.Config = 0;
ExternStickNick= 0;
ExternStickRoll = 0;
ExternStickYaw = 0;
}
 
if(!I2CTimeout)
{
I2CTimeout = 5;
I2C_Reset();
if((BeepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN) )
269,31 → 279,32
RED_OFF;
}
 
if(SIO_DEBUG && (!UpdateMotor || !(MKFlags & MKFLAG_MOTOR_RUN) ))
// allow Serial Data Transmit if motors must not updated or motors are not running
if( !UpdateMotor || !(MKFlags & MKFLAG_MOTOR_RUN) )
{
USART0_TransmitTxData();
USART0_ProcessRxData();
}
else USART0_ProcessRxData();
USART0_ProcessRxData();
 
if(CheckDelay(timer))
{
if(UBat < ParamSet.LowVoltageWarning)
{
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();
LED_Update();
//J4LOW;
}
 
#ifdef USE_NAVICTRL
305,7 → 316,7
SPI_TransmitByte();
}
#endif
}
}
return (1);
}
 
/branches/V0.72p Code Redesign killagreg/main.h
3,18 → 3,12
 
#include <avr/io.h>
 
//set crystal frequency here
#if defined (__AVR_ATmega644__)
#define SYSCLK 20000000L //crystal freqency in Hz
#endif
#define ATMEGA644 0
#define ATMEGA644P 1
 
#if defined (__AVR_ATmega644P__)
#define SYSCLK 20000000L //crystal freqency in Hz
#endif
#define SYSCLK F_CPU
 
#define F_CPU SYSCLK
 
 
// 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);}
26,6 → 20,7
#include <inttypes.h>
 
extern uint8_t BoardRelease;
extern uint8_t CPUType;
 
#endif //_MAIN_H
 
/branches/V0.72p Code Redesign killagreg/makefile
1,37 → 1,83
#--------------------------------------------------------------------
# MCU name
#MCU = atmega644
MCU = atmega644p
F_CPU = 20000000
#-------------------------------------------------------------------
VERSION_MAJOR = 0
VERSION_MINOR = 71
VERSION_PATCH = 7
VERSION_MINOR = 72
VERSION_PATCH = 15
 
VERSION_SERIAL_MAJOR = 10 # Serial Protocol Major Version
VERSION_SERIAL_MINOR = 0 # Serial Protocol Minor Version
NC_SPI_COMPATIBLE = 3 # SPI Protocol Version
NC_SPI_COMPATIBLE = 5 # SPI Protocol Version
 
#-------------------------------------------------------------------
#OPTIONS
 
# Use one of the extensions for a gps solution
#EXT = KILLAGREG
EXT = NAVICTRL
#EXT = MK3MAG
 
# Use one of the motor setups
 
# Standard
SETUP = QUADRO
# 2 Arms in Front
#SETUP = OCTO
# 1 Arm in front
#SETUP = OCTO2
# 1 Arm with two Motors in front or Coax
#SETUP = OCTO3
 
#------------
# Quadro:
# 1
# 4 3
# 2
#------------
# Reverse Props on 1 2
 
#------------
# Octo:
# 1 2
# 8 3
# 7 4
# 6 5
#------------
 
#------------
# Octo2:
# 1
# 8 2
# 7 3
# 6 4
# 5
#------------
 
#------------
# Octo3:
# 1
# 2
# 8 7 3 4
# 5
# 6
#------------
# Reverse Props on octo: 1 3 5 7
 
 
#-------------------------------------------------------------------
# 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
#FUSE_SETTINGS = -U lfuse:w:0xff:m -U hfuse:w:0xdf:m
# -u bei neuen Controllern wieder einspielen
HEX_NAME = MEGA644_$(EXT)
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m
HEX_NAME = MEGA644_$(EXT)_$(SETUP)
endif
 
ifeq ($(MCU), atmega644p)
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m
HEX_NAME = MEGA644p_$(EXT)
HEX_NAME = MEGA644p_$(EXT)_$(SETUP)
endif
 
 
82,19 → 128,38
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 = s
OPT = 2
#OPT = s
 
##########################################################################################################
# List C source files here. (C dependencies are automatically generated.)
SRC = main.c uart.c printf_P.c timer0.c timer2.c analog.c menu.c led.c
SRC += twimaster.c rc.c fc.c eeprom.c fifo.c
ifeq ($(MCU), atmega644p)
SRC += uart1.c
endif
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 fifo.c uart1.c
 
ifeq ($(EXT), KILLAGREG)
SRC += mm3.c mymath.c gps.c ubx.c
endif
145,7 → 210,7
#CFLAGS += -std=c99
CFLAGS += -std=gnu99
 
CFLAGS += -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)
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
157,9 → 222,22
CFLAGS += -DUSE_NAVICTRL
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
/branches/V0.72p Code Redesign killagreg/menu.c
1,10 → 1,53
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + only for non-profit use
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
// + 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"
12,7 → 55,7
#include "timer2.h"
#include "fc.h"
#include "rc.h"
#include "uart.h"
#include "uart0.h"
#include "printf_P.h"
#include "analog.h"
 
24,8 → 67,6
#include "ubx.h"
#endif
 
#include "_Settings.h"
 
#if (!defined (USE_KILLAGREG) && !defined (USE_MK3MAG))
uint8_t MaxMenuItem = 11;
#else
99,7 → 140,11
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 ", GetActiveParamSet());
#ifndef USE_QUADRO
LCD_printfxy(0,2,"OCTO Setting: %d ", GetActiveParamSet());
#else
LCD_printfxy(0,2,"QUADRO Setting: %d ", GetActiveParamSet());
#endif
LCD_printfxy(0,3,"(c) Holger Buss");
break;
case 1:// Height Control Menu Item
119,8 → 164,8
break;
case 2:// Attitude Menu Item
LCD_printfxy(0,0,"Attitude");
LCD_printfxy(0,1,"Nick: %5i",IntegralNick/1024);
LCD_printfxy(0,2,"Roll: %5i",IntegralRoll/1024);
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
140,35 → 185,35
switch(BoardRelease)
{
case 10:
LCD_printfxy(0,1,"Nick %4i (%3i)",AdValueGyrNick - AdNeutralNick, AdNeutralNick);
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueGyrRoll - AdNeutralRoll, AdNeutralRoll);
LCD_printfxy(0,3,"Yaw %4i (%3i)",AdNeutralYaw - AdValueGyrYaw , AdNeutralYaw);
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:
LCD_printfxy(0,1,"Nick %4i (%3i)",AdValueGyrNick - AdNeutralNick, AdNeutralNick/2);
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueGyrRoll - AdNeutralRoll, AdNeutralRoll/2);
LCD_printfxy(0,3,"Yaw %4i (%3i)",AdNeutralYaw - AdValueGyrYaw , AdNeutralYaw/2);
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:
LCD_printfxy(0,1,"Nick %4i (%3i)(%3i)",AdValueGyrNick - AdNeutralNick, AdNeutralNick/2, AnalogOffsetNick);
LCD_printfxy(0,2,"Roll %4i (%3i)(%3i)",AdValueGyrRoll - AdNeutralRoll, AdNeutralRoll/2, AnalogOffsetRoll);
LCD_printfxy(0,3,"Yaw %4i (%3i)(%3i)",AdNeutralYaw - AdValueGyrYaw , AdNeutralYaw/2 , AnalogOffsetYaw );
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, NeutralAccX);
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueAccRoll, NeutralAccY);
LCD_printfxy(0,3,"Height %4i (%3i)",Mean_AccTop, (int)NeutralAccZ);
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: %5i",UBat);
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
186,7 → 231,7
case 10:// Servo Menu Item
LCD_printfxy(0,0,"Servo " );
LCD_printfxy(0,1,"Setpoint %3i",FCParam.ServoNickControl);
LCD_printfxy(0,2,"Position: %3i",ServoValue);
LCD_printfxy(0,2,"Position: %3i",ServoNickValue);
LCD_printfxy(0,3,"Range:%3i-%3i",ParamSet.ServoNickMin, ParamSet.ServoNickMax);
break;
case 11://Extern Control
/branches/V0.72p Code Redesign killagreg/mk3mag.c
1,3 → 1,53
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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>
/branches/V0.72p Code Redesign killagreg/printf_P.c
89,7 → 89,7
#include "old_macros.h"
#include "printf_P.h"
#include "menu.h"
#include "uart.h"
#include "uart0.h"
 
 
//#define LIGHTPRINTF
/branches/V0.72p Code Redesign killagreg/rc.c
1,13 → 1,53
/*#######################################################################################
Decodieren eines RC Summen Signals
#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + only for non-profit use
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
// + 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>
43,9 → 83,9
// low level
PORTD &= ~((1<<PORTD5)|(1<<PORTD4));
 
// PD3 can't be used in FC 1.1 if 2nd UART is activated
// PD3 can't be used if 2nd UART is activated
// because TXD1 is at that port
if(BoardRelease == 10)
if(CPUType != ATMEGA644P)
{
DDRD |= (1<<PORTD3);
PORTD &= ~(1<<PORTD3);
151,6 → 191,7
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;
163,11 → 204,11
}
index++; // next channel
// demux sum signal for channels 5 to 7 to J3, J4, J5
if(index == 5) PORTD |= (1<<PORTD5); else PORTD &= ~(1<<PORTD5);
if(index == 6) PORTD |= (1<<PORTD4); else PORTD &= ~(1<<PORTD4);
if(BoardRelease == 10)
if(index == 5) J3HIGH; else J3LOW;
if(index == 6) J4HIGH; else J3LOW;
if(CPUType != ATMEGA644P) // not used as TXD1
{
if(index == 7) PORTD |= (1<<PORTD3); else PORTD &= ~(1<<PORTD3);
if(index == 7) J5HIGH; else J5LOW;
}
}
}
/branches/V0.72p Code Redesign killagreg/rc.h
3,9 → 3,24
 
#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/V0.72p Code Redesign killagreg/spectrum.c
0,0 → 1,248
/*#######################################################################################
Decodieren eines RC Summen Signals oder Spektrum Empfänger-Satellit
#######################################################################################*/
 
#include "Spectrum.h"
#include "main.h"
 
//############################################################################
// 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/V0.72p Code Redesign killagreg/spectrum.h
0,0 → 1,8
/*#######################################################################################
Dekodieren eines Spectrum Signals
#######################################################################################*/
 
#ifndef _SPECTRUM_H
#define _SPECTRUM_H
void Uart1Init(void);
#endif //_RC_H
/branches/V0.72p Code Redesign killagreg/spi.c
1,14 → 1,63
// ######################## SPI - FlightCtrl ###################
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 "_Settings.h"
#include "main.h"
#include "spi.h"
#include "fc.h"
#include "rc.h"
#include "eeprom.h"
#include "uart.h"
#include "uart0.h"
#include "timer0.h"
#include "analog.h"
 
89,14 → 138,14
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) (IntegralNick / 130L); // convert to multiple of 0.1°
ToNaviCtrl.IntegralRoll = (int16_t) (IntegralRoll / 130L); // convert to multiple of 0.1°
ToNaviCtrl.GyroHeading = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
ToNaviCtrl.GyroNick = Reading_GyroNick;
ToNaviCtrl.GyroRoll = Reading_GyroRoll;
ToNaviCtrl.GyroYaw = Reading_GyroYaw;
ToNaviCtrl.AccNick = (int16_t) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc);
ToNaviCtrl.AccRoll = (int16_t) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc);
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)
155,7 → 204,11
{ // jump from 5 to 0
CompassCalState = 0;
}
ToNaviCtrl.Param.Int[1] = ReadingHeight;
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:
163,6 → 216,7
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:
176,10 → 230,10
if (SPI_RxDataValid)
{
// update gps controls
if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
if(abs(FromNaviCtrl.GPSStickNick) < 512 && abs(FromNaviCtrl.GPSStickRoll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
{
GPS_Nick = FromNaviCtrl.GPS_Nick;
GPS_Roll = FromNaviCtrl.GPS_Roll;
GPSStickNick = FromNaviCtrl.GPSStickNick;
GPSStickRoll = FromNaviCtrl.GPSStickRoll;
}
// update compass readings
if(FromNaviCtrl.CompassHeading <= 360)
194,9 → 248,9
switch (FromNaviCtrl.Command)
{
case SPI_KALMAN:
FCParam.Kalman_K = FromNaviCtrl.Param.Byte[0];
FCParam.Kalman_MaxFusion = FromNaviCtrl.Param.Byte[1];
FCParam.Kalman_MaxDrift = FromNaviCtrl.Param.Byte[2];
FCParam.KalmanK = FromNaviCtrl.Param.Byte[0];
FCParam.KalmanMaxFusion = FromNaviCtrl.Param.Byte[1];
FCParam.KalmanMaxDrift = FromNaviCtrl.Param.Byte[2];
break;
 
default:
206,8 → 260,8
else // no valid data from NaviCtrl
{
// disable GPS control
GPS_Nick = 0;
GPS_Roll = 0;
GPSStickNick = 0;
GPSStickRoll = 0;
}
}
 
/branches/V0.72p Code Redesign killagreg/spi.h
90,8 → 90,8
typedef struct
{
uint8_t Command;
int16_t GPS_Nick;
int16_t GPS_Roll;
int16_t GPSStickNick;
int16_t GPSStickRoll;
int16_t GPS_Yaw;
int16_t CompassHeading;
int16_t Status;
/branches/V0.72p Code Redesign killagreg/timer0.c
1,3 → 1,53
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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>
55,27 → 105,27
// 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);
// 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 devider for timer 0 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz
// 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);
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
OCR0A = 0; // for PB3
OCR0B = 120; // for PB4
 
// init Timer/Counter 0 Register
TCNT0 = 0;
TCNT0 = 0;
 
// Timer/Counter 0 Interrupt Mask Register
// enable timer overflow interrupt only
92,8 → 142,8
/*****************************************************/
ISR(TIMER0_OVF_vect) // 9.765 kHz
{
static uint8_t cnt_1ms = 1,cnt = 0;
uint8_t Beeper_On = 0;
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
101,11 → 151,11
 
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
cnt = 9;
cnt_1ms++;
cnt_1ms %= 2;
if(!cnt_1ms) UpdateMotor = 1; // every 2nd run (976Hz/2 = 488 Hz)
CountMilliseconds++; // increment millisecond counter
}
 
 
112,14 → 162,14
// beeper on if duration is not over
if(BeepTime)
{
BeepTime--; // decrement BeepTime
if(BeepTime & BeepModulation) Beeper_On = 1;
else Beeper_On = 0;
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;
Beeper_On = 0;
BeepModulation = 0xFFFF;
}
 
// if beeper is on
155,28 → 205,35
// -----------------------------------------------------------------------
uint16_t SetDelay (uint16_t t)
{
return(CountMilliseconds + t - 1);
return(CountMilliseconds + t - 1);
}
 
// -----------------------------------------------------------------------
int8_t CheckDelay(uint16_t t)
{
return(((t - CountMilliseconds) & 0x8000) >> 8); // check sign bit
return(((t - CountMilliseconds) & 0x8000) >> 8); // check sign bit
}
 
// -----------------------------------------------------------------------
void Delay_ms(uint16_t w)
{
unsigned int t_stop;
t_stop = SetDelay(w);
while (!CheckDelay(t_stop));
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)) ADC_Enable();
uint16_t t_stop;
t_stop = SetDelay(w);
while (!CheckDelay(t_stop))
{
if(ADReady)
{
ADReady = 0;
ADC_Enable();
}
}
}
 
/branches/V0.72p Code Redesign killagreg/timer2.c
1,11 → 1,64
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 "uart.h"
#include "uart0.h"
#include "main.h"
#include "rc.h"
 
volatile int16_t ServoValue = 0;
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)
28,11 → 81,12
PORTD &= ~(1<<PORTD7); // set PD7 to low
 
DDRC |= (1<<DDC6); // set PC6 as output (Reset for HEF4017)
PORTC &= ~(1<<PORTC6); // set PC6 to low
//PORTC &= ~(1<<PORTC6); // set PC6 to low
HEF4017R_ON; // enable reset
 
// Timer/Counter 2 Control Register A
 
// Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1)
// 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));
40,19 → 94,20
 
// Timer/Counter 2 Control Register B
 
// Set clock divider for timer 2 to SYSKLOCK/64 = 20MHz / 64 = 312.5 kHz
// The timer increments from 0x00 to 0xFF with an update rate of 312.5 kHz or 3.2 us
// hence the timer overflow interrupt frequency is 312.5 kHz / 256 = 1220.7 Hz or 0.8192 ms
// 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 64 (Bits: CS022 = 1, CS21 = 0, CS20 = 0)
TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS21)|(1<<CS20)|(1<<WGM22));
TCCR2B |= (1<<CS22);
// 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 = 10;
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
63,98 → 118,207
}
 
 
void Servo_On(void)
{
ServoActive = 1;
}
 
void Servo_Off(void)
{
ServoActive = 0;
HEF4017R_ON; // enable reset
}
 
/*****************************************************/
/* Control Servo Position */
/*****************************************************/
 
ISR(TIMER2_COMPA_vect) // every 256 * 3.2 us = 0.819 us ( on compare match of TCNT2 and OC2A)
ISR(TIMER2_COMPA_vect)
{
static uint8_t PostPulse = 0x80; // value for last pwm cycle in non inverting mode (clear pin on compare match)
static uint16_t FilterServo = 100; // initial value, after some iterations it becomes the average value of 2 * FCParam.ServoNickControl
static uint16_t ServoState = 40; // cycle down counter for this ISR
 
#define MULTIPLIER 4
// 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
 
if(BoardRelease < 99)
#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 positon
 
 
if(BoardRelease < 20)
{
switch(ServoState)
//---------------------------
// Nick servo state machine
//---------------------------
if(!PulseOutput) // pulse output complete
{
case 4:
// recalculate new ServoValue
ServoValue = 0x0030; // Offset (part 1)
FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset
ServoValue += FilterServo; // add filtered static offset
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
ServoValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );
ServoNickValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) );
}
else
{ // non inverting movement of servo
ServoValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );
ServoNickValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) );
}
// limit servo value to its parameter range definition
if(ServoValue < ((int16_t)ParamSet.ServoNickMin * 3) )
if(ServoNickValue < ((int16_t)ParamSet.ServoNickMin * MULTIPLYER) )
{
ServoValue = (int16_t)ParamSet.ServoNickMin * 3;
ServoNickValue = (int16_t)ParamSet.ServoNickMin * MULTIPLYER;
}
else
if(ServoValue > ((int16_t)ParamSet.ServoNickMax * 3) )
if(ServoNickValue > ((int16_t)ParamSet.ServoNickMax * MULTIPLYER) )
{
ServoValue = (int16_t)ParamSet.ServoNickMax * 3;
ServoNickValue = (int16_t)ParamSet.ServoNickMax * MULTIPLYER;
}
DebugOut.Analog[20] = ServoValue;
 
// determine prepulse width (remaining part of ServoValue/Timer Cycle)
if ((ServoValue % 255) < 45)
{ // if prepulse width is to short the execution time of this ISR is longer than the next compare match
// so balance with postpulse width
ServoValue += 77;
PostPulse = 0x60 - 77;
}
else
{
PostPulse = 0x60;
}
// set output compare register to 255 - prepulse width
OCR2A = 255 - (ServoValue % 256);
// connect OC2A in inverting mode (Clear pin on overflow, Set pin on compare match)
TCCR2A=(1<<COM2A1)|(1<<COM2A0)|(1<<WGM21)|(1<<WGM20);
RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
 
break;
ServoNickValue /= MULTIPLYER;
DebugOut.Analog[20] = ServoNickValue;
 
case 3:
case 2:
case 1:
// 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(ServoValue > 255) // is larger than a full timer 2 cycle
if(ServoIndex == 0) // if we are at the sync gap
{
PORTD |= (1<<PORTD7); // set PD7 to high
TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A
ServoValue -= 255; // substract full timer cycle
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 // the post pule must be generated
else // servo channels
{
TCCR2A=(1<<COM2A1)|(0<<COM2A0)|(1<<WGM21)|(1<<WGM20); // connect OC2A in non inverting mode
OCR2A = PostPulse; // Offset Part2
ServoState = 1; // jump to ServoState 0 with next ISR call
}
break;
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;
}
 
case 0:
ServoState = (uint16_t) ParamSet.ServoNickRefresh * MULTIPLIER; // reload ServoState
PORTD &= ~(1<<PORTD7); // set PD7 to low
TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A
break;
RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
 
default:
// do nothing
break;
ServoNickValue /= MULTIPLYER;
DebugOut.Analog[20] = ServoNickValue;
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;
}
ServoState--;
} // EOF BoardRelease < 20
else // output to HEF4014
} // EOF PPM state machine
 
// General pulse output generator
if(RemainingPulse > (255 + IRS_RUNTIME))
{
// code for HEF4014 output must be placed here
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/V0.72p Code Redesign killagreg/timer2.h
3,11 → 3,12
 
#include <inttypes.h>
 
extern volatile int16_t ServoValue;
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/V0.72p Code Redesign killagreg/twimaster.c
1,5 → 1,53
/*############################################################################
############################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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>
10,10 → 58,16
#include "analog.h"
 
volatile uint8_t twi_state = 0;
volatile uint8_t motor_write = 0;
volatile uint8_t motor_read = 0;
uint8_t motor_write = 0;
uint8_t motor_read = 0;
volatile uint8_t dac_channel = 0;
volatile uint8_t motor_rx[8];
 
#ifdef USE_QUADRO
uint8_t motor_rx[8];
#else
uint8_t motor_rx[16];
#endif
 
volatile uint16_t I2CTimeout = 100;
 
 
147,11 → 201,13
I2C_WriteByte(0);
}
 
 
/****************************************/
/* I2C ISR */
/****************************************/
 
#ifdef USE_QUADRO
ISR (TWI_vect)
 
{
 
switch (twi_state++) // First i2c_start from SendMotorData()
164,16 → 220,16
switch(motor_write)
{
case 0:
I2C_WriteByte(Motor_Front);
I2C_WriteByte(Motor1);
break;
case 1:
I2C_WriteByte(Motor_Rear);
I2C_WriteByte(Motor2);
break;
case 2:
I2C_WriteByte(Motor_Right);
I2C_WriteByte(Motor3);
break;
case 3:
I2C_WriteByte(Motor_Left);
I2C_WriteByte(Motor4);
break;
}
break;
226,13 → 282,13
switch(dac_channel)
{
case 0:
I2C_WriteByte(AnalogOffsetNick); // 1st byte for Channel A
I2C_WriteByte(DacOffsetGyroNick); // 1st byte for Channel A
break;
case 1:
I2C_WriteByte(AnalogOffsetRoll); // 1st byte for Channel B
I2C_WriteByte(DacOffsetGyroRoll); // 1st byte for Channel B
break;
case 2:
I2C_WriteByte(AnalogOffsetYaw ); // 1st byte for Channel C
I2C_WriteByte(DacOffsetGyroYaw ); // 1st byte for Channel C
break;
}
break;
266,3 → 322,133
motor_read = 0;
}
}
#else // USE_OCTO, USE_OCTO2, USE_OCTO3
ISR (TWI_vect)
{
 
switch (twi_state++) // First i2c_start from SendMotorData()
{
// Master Transmit
case 0: // Send SLA-W
I2C_WriteByte(0x52 + (motor_write * 2) );
break;
case 1: // Send Data to Slave
switch(motor_write)
{
case 0:
I2C_WriteByte(Motor1);
break;
case 1:
I2C_WriteByte(Motor2);
break;
case 2:
I2C_WriteByte(Motor3);
break;
case 3:
I2C_WriteByte(Motor4);
break;
case 5:
I2C_WriteByte(Motor5);
break;
case 6:
I2C_WriteByte(Motor6);
break;
case 7:
I2C_WriteByte(Motor7);
break;
case 8:
I2C_WriteByte(Motor8);
break;
}
break;
case 2: // repeat case 0+1 for all motors
I2C_Stop();
if (motor_write < 7)
{
motor_write++; // jump to next motor
twi_state = 0; // and repeat from state 0
}
else
{ // data to last motor send
motor_write = 0; // reset motor write counter
}
I2C_Start(); // Repeated start -> switch slave or switch Master Transmit -> Master Receive
break;
 
// Master Receive
case 3: // Send SLA-R
I2C_WriteByte(0x53 + (motor_read * 2) );
break;
case 4:
//Transmit 1st byte
I2C_ReceiveByte();
break;
case 5: //Read 1st byte and transmit 2nd Byte
motor_rx[motor_read] = TWDR;
I2C_ReceiveLastByte();
break;
case 6:
//Read 2nd byte
motor_rx[motor_read + 8] = TWDR;
motor_read++;
if (motor_read > 7) motor_read = 0;
I2C_Stop();
twi_state = 0;
I2CTimeout = 10;
break;
 
// 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();
I2CTimeout = 10;
// repeat case 7...10 until all DAC Channels are updated
if(dac_channel < 2)
{
dac_channel ++; // jump to next channel
twi_state = 7; // and repeat from state 7
I2C_Start(); // start transmission for next channel
}
else
{ // data to last motor send
dac_channel = 0; // reset dac channel counter
twi_state = 0; // reset twi_state
}
break;
 
default:
I2C_Stop();
twi_state = 0;
I2CTimeout = 10;
motor_write = 0;
motor_read = 0;
}
}
#endif // USE_OCTO, USE_OCTO2
 
/branches/V0.72p Code Redesign killagreg/twimaster.h
9,7 → 9,13
#define TWI_STATE_GYRO_OFFSET_TX 7
 
extern volatile uint8_t twi_state;
extern volatile uint8_t motor_rx[8];
 
#ifdef USE_QUADRO
extern uint8_t motor_rx[8];
#else
extern uint8_t motor_rx[16];
#endif
 
extern volatile uint16_t I2CTimeout;
 
extern void I2C_Init (void); // Initialize I2C
/branches/V0.72p Code Redesign killagreg/uart0.c
0,0 → 1,694
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 <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[4] = {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
 
 
const 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",
" ",
" ",
"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
memcpy(&MotorTest[0], (uint8_t*)pRxData, sizeof(MotorTest));
//Request_MotorTest = TRUE;
PcAccess = 255;
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_VERSION;
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_VERSION)) // 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
{
SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &Request_DebugLabel, sizeof(Request_DebugLabel), ANALOG_LABEL[Request_DebugLabel], 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) (IntegralNick / (GyroDegFactor/ 10)); // approx. 0.1 deg
ToMk3Mag.Attitude[1] = (int16_t) (IntegralRoll / (GyroDegFactor/ 10)); // 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/V0.72p Code Redesign killagreg/uart0.h
0,0 → 1,69
#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[4];
 
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/V0.72p Code Redesign killagreg/uart1.c
1,7 → 1,55
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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"
#include "fifo.h"
143,6 → 191,6
uint8_t c;
c = UDR1; // get data byte
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
if (BoardRelease > 10) ubx_parser(c); // and put it into the ubx protocol parser
ubx_parser(c); // and put it into the ubx protocol parser
#endif
}
/branches/V0.72p Code Redesign killagreg/ubx.c
1,9 → 1,59
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 "uart.h"
//#include "uart0.h"
 
// ubx protocol parser state machine
#define UBXSTATE_IDLE 0
/branches/V0.72p Code Redesign killagreg/version.txt
184,15 → 184,66
- 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
 
Anpassungen bzgl. V0.71h
G.Stobrawa 18.12.2008:
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
 
Anpassungen bzgl. V0.72p
G.Stobrawa 01.03.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.
- 2. Uart wird nun unterstützt (MCU = atmega644p im Makefile)
- Makefile: EXT=NAVICTRL Unterstützung der SPI Communikation zum Naviboard
 
205,6 → 256,9
- 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
 
- Makefile: SETUP = QUADRO für Quadrokopter
- Makefile: SETUP = OCTO für Oktokopter
 
Weiter Detail siehe Readme.txt im Verzeichnis Hex-Files.