• MikroKopter.de
• Wiki
• SHOP
• Datenschutz
• Impressum

FlightCtrlBL-CtrlMK3MagNaviCtrlProjects

Català-Valencià – Catalan中文 – Chinese (Simplified)中文 – Chinese (Traditional)Česky – CzechDansk – DanishNederlands – DutchEnglish – EnglishSuomi – FinnishFrançais – FrenchDeutsch – Germanעברית – Hebrewहिंदी – HindiMagyar – HungarianBahasa Indonesia – IndonesianItaliano – Italian日本語 – Japanese한국어 – KoreanМакедонски – Macedonianमराठी – MarathiNorsk – NorwegianPolski – PolishPortuguês – PortuguesePortuguês – Portuguese (Brazil)Русский – RussianSlovenčina – SlovakSlovenščina – SlovenianEspañol – SpanishSvenska – SwedishTürkçe – TurkishУкраїнська – UkrainianOëzbekcha – Uzbek

Compare Revisions

• This comparison shows the changes necessary to convert path

  → /branches/V0.70d Code Redesign killagreg/ @ 935

  → /branches/V0.70d Code Redesign killagreg/ @ 936

  ↔ Reverse comparison

• Compare Path:	Rev

  With Path:	Rev

Ignore whitespace Rev 935 → Rev 936

/branches/V0.70d Code Redesign killagreg/Hex-Files/Flight-Ctrl_MEGA644p_NAVICTRL_V0_69k.hex
File deleted

/branches/V0.70d Code Redesign killagreg/Hex-Files/Flight-Ctrl_MEGA644p_MK3MAG_V0_69k.hex
File deleted

/branches/V0.70d Code Redesign killagreg/Hex-Files/Flight-Ctrl_MEGA644_KILLAGREG_V0_69k.hex
File deleted

/branches/V0.70d Code Redesign killagreg/Hex-Files/Flight-Ctrl_MEGA644_MK3MAG_V0_69k.hex
File deleted

/branches/V0.70d Code Redesign killagreg/Hex-Files/Flight-Ctrl_MEGA644p_KILLAGREG_V0_69k.hex
File deleted

/branches/V0.70d Code Redesign killagreg/Hex-Files/Flight-Ctrl_MEGA644_NAVICTRL_V0_69k.hex
File deleted

/branches/V0.70d Code Redesign killagreg/Hex-Files/Readme.txt
1,6 → 1,6
V0.69k Ausgangsversion.
V0.70b Ausgangsversion.
G.Stobrawa 17.07.2008:
G.Stobrawa 18.07.2008:
- Code stärker modularisiert und restrukturiert
- viele Kommentare zur Erklärug eingefügt
13,8 → 13,6
- GPS-Hold-Funktion hinzugefügt
- GPS-Home-Funktion hinzugefügt (wird beim Motorstart gelernt, und bei Motorenstop wieder gelöscht)
- Poti3 steuert die GPS Funktionen (Poti3 < 70:GPS inaktiv, 70<=Poti3<160: GPS Hold, 160<=Poti3: GPS Home)
- LED Steuerung an J16, parametrierbar durch die User Parameter 7 & 8. UserParam7 legt die
LEDOnTime in Vielfachen von 2ms fest und UserParam 8 die LEDOffTime.
- Zusätzliche Punkte im Menü des KopterTool zur Anzeige des GPS-Status und der MM3-Kalibierparameter
58,14 → 56,6
Parameter 7 --> LED1Time for J16
Parameter 8 --> LED2Time for J17
- LED-Steuerung an J16/J17
Das Verhalten der über einen Transistor gegen Masse geschalteten Ausgänge J16/J17 wird mit
Hilfe der Userparameter 7 und 8 angepasst.
Es gilt:
UP7/8 < 20: Ausgang liegt dauerhaft auf Masse.
UP7/8 > 220: Ausgang ist dauerhaft hochohmig.
Sonst wird geblinkt, wobei sich die Frequenz mit dem Userparameter zwischen 2 bis 25 Hz einstellen lässt.
- Zusätzliche akustische Signale:

/branches/V0.70d Code Redesign killagreg/Kopter-Tool/MikroKopter-Tool.exe
Cannot display: file marked as a binary type.
svn:mime-type = application/octet-stream

/branches/V0.70d Code Redesign killagreg/_Settings.h
1,9 → 1,4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Testmodi
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define MOTOR_OFF 0
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Abstimmung
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define ACC_AMPLIFY 12

/branches/V0.70d Code Redesign killagreg/analog.c
14,12 → 14,16
#include "fc.h"
#include "printf_P.h"
#include "eeprom.h"
#include "twimaster.h"
volatile int16_t Current_AccZ = 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 int32_t AirPressure = 32000;
volatile uint8_t average_pressure = 0;
volatile int16_t StartAirPressure;
volatile uint16_t ReadingAirPressure = 1023;
uint8_t PressureSensorOffset;
77,6 → 81,35
}
void SearchGyroOffset(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);
}
Delay_ms_Mess(70);
}
/*****************************************************/
/* Interrupt Service Routine for ADC */
/*****************************************************/
101,7 → 134,6
{
static uint8_t adc_channel = 0, state = 0;
static uint16_t yaw1, roll1, nick1;
static uint8_t average_pressure = 0;
static int16_t tmpAirPressure = 0;
// disable further AD conversion
ADC_Disable();

/branches/V0.70d Code Redesign killagreg/analog.h
5,6 → 5,7
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 int32_t AirPressure;
14,10 → 15,10
extern volatile uint16_t ReadingAirPressure;
extern volatile int16_t StartAirPressure;
extern void SearchAirPressureOffset(void);
void SearchAirPressureOffset(void);
void SearchGyroOffset(void);
void ADC_Init(void);
extern 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.70d Code Redesign killagreg/eeprom.c
15,6 → 15,7
#include <string.h>
#include "eeprom.h"
#include "printf_P.h"
#include "led.h"
// byte array in eeprom
37,7 → 38,7
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;//0x01;
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
58,12 → 59,12
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 = 253; //zur freien Verwendung
ParamSet.UserParam2 = 100; //zur freien Verwendung
ParamSet.UserParam3 = 90; //zur freien Verwendung
ParamSet.UserParam4 = 90; //zur freien Verwendung
ParamSet.UserParam5 = 90; // zur freien Verwendung
ParamSet.UserParam6 = 90; // zur freien Verwendung
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
78,11 → 79,23
ParamSet.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt
ParamSet.Yaw_PosFeedback = 90;
ParamSet.Yaw_NegFeedback = 5;
ParamSet.AngleTurnOverNick = 100;
ParamSet.AngleTurnOverRoll = 100;
ParamSet.AngleTurnOverNick = 85;
ParamSet.AngleTurnOverRoll = 85;
ParamSet.GyroAccTrim = 16; // 1/k
ParamSet.DriftComp = 4;
ParamSet.DynamicStability = 100;
ParamSet.J16Bitmask = 95;
ParamSet.J17Bitmask = 243;
ParamSet.J16Timing = 15;
ParamSet.J17Timing = 15;
ParamSet.NaviGpsModeControl = 253;
ParamSet.NaviGpsGain = 100;
ParamSet.NaviGpsP = 90;
ParamSet.NaviGpsI = 90;
ParamSet.NaviGpsD = 90;
ParamSet.NaviGpsACC = 0;
ParamSet.NaviGpsMinSat = 6;
ParamSet.NaviStickThreshold = 8;
memcpy(ParamSet.Name, "Sport\0",6);
}
100,7 → 113,7
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;//0x01;
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
121,12 → 134,12
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 = 253; // zur freien Verwendung
ParamSet.UserParam2 = 100; // zur freien Verwendung
ParamSet.UserParam3 = 90; // zur freien Verwendung
ParamSet.UserParam4 = 90; // zur freien Verwendung
ParamSet.UserParam5 = 90; // zur freien Verwendung
ParamSet.UserParam6 = 90; // zur freien Verwendung
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
141,11 → 154,23
ParamSet.LoopConfig = 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 = 100;
ParamSet.AngleTurnOverRoll = 100;
ParamSet.AngleTurnOverNick = 85;
ParamSet.AngleTurnOverRoll = 85;
ParamSet.GyroAccTrim = 32; // 1/k
ParamSet.DriftComp = 4;
ParamSet.DynamicStability = 75;
ParamSet.J16Bitmask = 95;
ParamSet.J17Bitmask = 243;
ParamSet.J16Timing = 20;
ParamSet.J17Timing = 20;
ParamSet.NaviGpsModeControl = 253;
ParamSet.NaviGpsGain = 100;
ParamSet.NaviGpsP = 90;
ParamSet.NaviGpsI = 90;
ParamSet.NaviGpsD = 90;
ParamSet.NaviGpsACC = 0;
ParamSet.NaviGpsMinSat = 6;
ParamSet.NaviStickThreshold = 8;
memcpy(ParamSet.Name, "Normal\0", 7);
}
163,7 → 188,7
ParamSet.ChannelAssignment[CH_POTI2] = 6;
ParamSet.ChannelAssignment[CH_POTI3] = 7;
ParamSet.ChannelAssignment[CH_POTI4] = 8;
ParamSet.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE | CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX;//0x01;
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
184,12 → 209,12
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 = 253; // zur freien Verwendung
ParamSet.UserParam2 = 100; // zur freien Verwendung
ParamSet.UserParam3 = 90; // zur freien Verwendung
ParamSet.UserParam4 = 90; // zur freien Verwendung
ParamSet.UserParam5 = 90; // zur freien Verwendung
ParamSet.UserParam6 = 90; // zur freien Verwendung
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
204,11 → 229,23
ParamSet.LoopConfig = 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 = 100;
ParamSet.AngleTurnOverRoll = 100;
ParamSet.AngleTurnOverNick = 85;
ParamSet.AngleTurnOverRoll = 85;
ParamSet.GyroAccTrim = 32; // 1/k
ParamSet.DriftComp = 4;
ParamSet.DynamicStability = 50;
ParamSet.J16Bitmask = 95;
ParamSet.J17Bitmask = 243;
ParamSet.J16Timing = 30;
ParamSet.J17Timing = 30;
ParamSet.NaviGpsModeControl = 253;
ParamSet.NaviGpsGain = 100;
ParamSet.NaviGpsP = 90;
ParamSet.NaviGpsI = 90;
ParamSet.NaviGpsD = 90;
ParamSet.NaviGpsACC = 0;
ParamSet.NaviGpsMinSat = 6;
ParamSet.NaviStickThreshold = 8;
memcpy(ParamSet.Name, "Beginner\0", 9);
}
253,6 → 290,7
{
if (setnumber > 5) setnumber = 5;
eeprom_read_block((uint8_t *) &ParamSet.ChannelAssignment[0], &EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * setnumber], PARAMSET_STRUCT_LEN);
LED_Init();
}
/***************************************************/
265,6 → 303,7
eeprom_write_block((uint8_t *) &ParamSet.ChannelAssignment[0], &EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * setnumber], PARAMSET_STRUCT_LEN);
// set this parameter set to active set
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);
LED_Init();
}
/***************************************************/

/branches/V0.70d Code Redesign killagreg/eeprom.h
7,7 → 7,7
#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_NICK 4 // word
#define PID_ACC_ROLL 6 // word
#define PID_ACC_Z 8 // word
39,9 → 39,9
#define CFG_LOOP_RIGHT 0x08
// defines for lookup mk_param_struct.ChannelAssignment
#define CH_NICK 0
#define CH_NICK 0
#define CH_ROLL 1
#define CH_GAS 2
#define CH_GAS 2
#define CH_YAW 3
#define CH_POTI1 4
#define CH_POTI2 5
48,7 → 48,7
#define CH_POTI3 6
#define CH_POTI4 7
#define EEPARAM_VERSION 70 // is count up, if EE_Paramater stucture has changed (compatibility)
#define EEPARAM_VERSION 71 // is count up, if EE_Paramater stucture has changed (compatibility)
// values above 250 representing poti1 to poti4
typedef struct
98,6 → 98,19
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;
uint8_t NaviGpsP;
uint8_t NaviGpsI;
uint8_t NaviGpsD;
uint8_t NaviGpsACC;
uint8_t NaviGpsMinSat;
uint8_t NaviStickThreshold;
uint8_t ExternalControl; // for serial Control
uint8_t LoopConfig; // see upper defines for bitcoding
uint8_t ServoNickCompInvert; // Wert : 0-250 0 oder 1 // WICHTIG!!! am Ende lassen
uint8_t Reserved[4];

/branches/V0.70d Code Redesign killagreg/fc.c
74,14 → 74,13
#endif
#include "led.h"
volatile uint16_t I2CTimeout = 100;
// gyro readings
volatile int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
// gyro neutral readings
volatile int16_t AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;
volatile int16_t StartNeutralRoll = 0, StartNeutralNick = 0;
int16_t AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;
int16_t StartNeutralRoll = 0, StartNeutralNick = 0;
// mean accelerations
volatile int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop;
int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop;
// neutral acceleration readings
volatile int16_t NeutralAccX=0, NeutralAccY=0;
88,17 → 87,17
volatile float NeutralAccZ = 0;
// attitude gyro integrals
volatile int32_t IntegralNick = 0,IntegralNick2 = 0;
volatile int32_t IntegralRoll = 0,IntegralRoll2 = 0;
volatile int32_t IntegralYaw = 0;
volatile int32_t Reading_IntegralGyroNick = 0, Reading_IntegralGyroNick2 = 0;
volatile int32_t Reading_IntegralGyroRoll = 0, Reading_IntegralGyroRoll2 = 0;
volatile int32_t Reading_IntegralGyroYaw = 0;
volatile int32_t MeanIntegralNick;
volatile int32_t MeanIntegralRoll;
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;
// attitude acceleration integrals
volatile int32_t IntegralAccNick = 0, IntegralAccRoll = 0;
int32_t IntegralAccNick = 0, IntegralAccRoll = 0;
volatile int32_t Reading_Integral_Top = 0;
// compass course
115,10 → 114,9
int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0;
// flags
uint8_t MotorsOn = 0;
uint8_t EmergencyLanding = 0;
// MK flags
uint16_t Model_Is_Flying = 0;
volatile uint8_t MKFlags = 0;
int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L;
125,7 → 123,7
float Gyro_P_Factor;
float Gyro_I_Factor;
volatile int16_t DiffNick, DiffRoll;
int16_t DiffNick, DiffRoll;
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
164,7 → 162,7
{
while(numbeeps--)
{
if(MotorsOn) return; //auf keinen Fall im Flug!
if(MKFlags & MKFLAG_MOTOR_RUN) return; //auf keinen Fall bei laufenden Motoren!
BeepTime = 100; // 0.1 second
Delay_ms(250); // blocks 250 ms as pause to next beep,
// this will block the flight control loop,
217,6 → 215,7
Reading_GyroNick = 0;
Reading_GyroRoll = 0;
Reading_GyroYaw = 0;
Delay_ms_Mess(100);
StartAirPressure = AirPressure;
HeightD = 0;
Reading_Integral_Top = 0;
229,6 → 228,7
GPS_Roll = 0;
YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR;
YawGyroDrift = 0;
MKFlags |= MKFLAG_CALIBRATE;
}
/************************************************************************/
356,6 → 356,7
/************************************************************************/
void CalibMean(void)
{
if(BoardRelease >= 13) SearchGyroOffset();
// stop ADC to avoid changing values during calculation
ADC_Disable();
379,7 → 380,7
/************************************************************************/
void SendMotorData(void)
{
if(MOTOR_OFF || !MotorsOn)
if(!(MKFlags & MKFLAG_MOTOR_RUN))
{
Motor_Rear = 0;
Motor_Front = 0;
389,8 → 390,8
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;
397,8 → 398,7
DebugOut.Analog[15] = Motor_Right;
//Start I2C Interrupt Mode
twi_state = 0;
motor = 0;
twi_state = TWI_STATE_MOTOR_TX;
I2C_Start();
}
413,28 → 413,38
// else the last updated values are used
{
//update poti values by rc-signals
#define CHK_POTI(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;}
CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255);
CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100);
CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100);
CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255);
CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect,0,255);
CHK_POTI(FCParam.Gyro_P,ParamSet.Gyro_P,10,255);
CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I,0,255);
CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor,0,255);
CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1,0,255);
CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2,0,255);
CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3,0,255);
CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4,0,255);
CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5,0,255);
CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6,0,255);
CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255);
CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255);
CHK_POTI(FCParam.ServoNickControl,ParamSet.ServoNickControl,0,255);
CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit,0,255);
CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255);
CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255);
CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255);
#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(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(FCParam.UserParam1,ParamSet.UserParam1);
CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2);
CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3);
CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4);
CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5);
CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6);
CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7);
CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8);
CHK_POTI(FCParam.ServoNickControl,ParamSet.ServoNickControl);
CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit);
CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback);
CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback);
CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability);
CHK_POTI_MM(FCParam.J16Timing,ParamSet.J16Timing,1,255);
CHK_POTI_MM(FCParam.J17Timing,ParamSet.J17Timing,1,255);
CHK_POTI(FCParam.NaviGpsModeControl,ParamSet.NaviGpsModeControl);
CHK_POTI(FCParam.NaviGpsGain,ParamSet.NaviGpsGain);
CHK_POTI(FCParam.NaviGpsP,ParamSet.NaviGpsP);
CHK_POTI(FCParam.NaviGpsI,ParamSet.NaviGpsI);
CHK_POTI(FCParam.NaviGpsD,ParamSet.NaviGpsD);
CHK_POTI(FCParam.NaviGpsACC,ParamSet.NaviGpsACC);
CHK_POTI(FCParam.ExternalControl,ParamSet.ExternalControl);
Ki = (float) FCParam.I_Factor * FACTOR_I;
}
}
501,14 → 511,13
if(RcLostTimer) RcLostTimer--; // decremtent timer after rc sigal lost
else // rc lost countdown finished
{
MotorsOn = 0; // stop all motors
EmergencyLanding = 0; // emergency landing is over
MKFlags &= ~(MKFLAG_MOTOR_RUN|MKFLAG_EMERGENCY_LANDING); // clear motor run flag that stop the motors in SendMotorData()
}
ROT_ON; // set red led
RED_ON; // set red led
if(Model_Is_Flying > 1000) // wahrscheinlich in der Luft --> langsam absenken
{
GasMixFraction = ParamSet.EmergencyGas; // set emergency gas
EmergencyLanding = 1; // enable emergency landing
MKFlags |= (MKFLAG_EMERGENCY_LANDING); // ser flag fpr emergency landing
// set neutral rc inputs
PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] = 0;
PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
517,7 → 526,7
PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0;
}
else MotorsOn = 0; // switch of all motors
else MKFlags &= ~(MKFLAG_MOTOR_RUN); // clear motor run flag that stop the motors in SendMotorData()
} // eof RC_Quality < 120
else
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
525,10 → 534,10
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(RC_Quality > 140)
{
EmergencyLanding = 0; // switch off emergency landing if RC-signal is okay
MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing
// reset emergency timer
RcLostTimer = ParamSet.EmergencyGasDuration * 50;
if(GasMixFraction > 40)
if(GasMixFraction > 40 && (MKFlags & MKFLAG_MOTOR_RUN) )
{
if(Model_Is_Flying < 0xFFFF) Model_Is_Flying++;
}
537,8 → 546,14
SumNick = 0;
SumRoll = 0;
StickYaw = 0;
if(Model_Is_Flying == 250) UpdateCompassCourse = 1;
if(Model_Is_Flying == 250)
{
UpdateCompassCourse = 1;
Reading_IntegralGyroYaw = 0;
SetPointYaw = 0;
}
}
else MKFlags |= (MKFLAG_FLY); // set fly flag
if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
561,7 → 576,7
if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255;
// if motors are off and the gas stick is in the upper position
if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorsOn == 0)
if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && !(MKFlags & MKFLAG_MOTOR_RUN) )
{
// and if the yaw stick is in the leftmost position
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75)
681,7 → 696,7
{
delay_startmotors = 200; // do not repeat if once executed
Model_Is_Flying = 1;
MotorsOn = 1;
MKFlags |= (MKFLAG_MOTOR_RUN|MKFLAG_START); // set flag RUN and START
SetPointYaw = 0;
Reading_IntegralGyroYaw = 0;
Reading_IntegralGyroNick = 0;
690,12 → 705,6
Reading_IntegralGyroRoll2 = IntegralRoll;
SumNick = 0;
SumRoll = 0;
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
{
GPS_SetHomePosition();
}
#endif
}
}
else delay_startmotors = 0; // reset delay timer if sticks are not in this position
708,13 → 717,7
{
delay_stopmotors = 200; // do not repeat if once executed
Model_Is_Flying = 0;
MotorsOn = 0;
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
{
GPS_ClearHomePosition();
}
#endif
MKFlags &= ~(MKFLAG_MOTOR_RUN);
}
}
else delay_stopmotors = 0; // reset delay timer if sticks are not in this position
724,7 → 727,7
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// new values from RC
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!NewPpmData-- || EmergencyLanding) // NewData = 0 means new data from RC
if(!NewPpmData-- || (MKFlags & MKFLAG_EMERGENCY_LANDING) ) // NewData = 0 means new data from RC
{
int tmp_int;
ParameterMapping(); // remapping params (online poti replacement)
749,7 → 752,7
// Digital Control via DubWise
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define KEY_VALUE (FCParam.UserParam8 * 4) // step width
#define KEY_VALUE (FCParam.ExternalControl * 4) // step width
if(DubWiseKeys[1]) BeepTime = 10;
if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE;
else if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE;
765,7 → 768,7
if(DubWiseKeys[0] & 2) ExternHeightValue++;
if(DubWiseKeys[0] & 16) ExternHeightValue--;
StickNick += (STICK_GAIN * ExternStickNick) / 8;
StickNick += (STICK_GAIN * ExternStickNick) / 8;
StickRoll += (STICK_GAIN * ExternStickRoll) / 8;
StickYaw += (STICK_GAIN * ExternStickYaw);
773,7 → 776,7
//+ Analog control via serial communication
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ExternControl.Config & 0x01 && FCParam.UserParam8 > 128)
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;
792,9 → 795,17
// update max stick positions for nick and roll
if(abs(StickNick / STICK_GAIN) > MaxStickNick) MaxStickNick = abs(StickNick)/STICK_GAIN;
if(abs(StickNick / STICK_GAIN) > MaxStickNick)
{
MaxStickNick = abs(StickNick)/STICK_GAIN;
if(MaxStickNick > 100) MaxStickNick = 100;
}
else MaxStickNick--;
if(abs(StickRoll / STICK_GAIN) > MaxStickRoll) MaxStickRoll = abs(StickRoll)/STICK_GAIN;
if(abs(StickRoll / STICK_GAIN) > MaxStickRoll)
{
MaxStickRoll = abs(StickRoll)/STICK_GAIN;
if(MaxStickRoll > 100) MaxStickRoll = 100;
}
else MaxStickRoll--;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
845,19 → 856,11
if(GasMixFraction > ParamSet.LoopGasLimit) GasMixFraction = ParamSet.LoopGasLimit;
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ LED Control on J16/J17
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
LED1_Time = FCParam.UserParam7;
LED2_Time = FCParam.UserParam8;
LED_Update();
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// in case of emergency landing
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// set all inputs to save values
if(EmergencyLanding)
if(MKFlags & MKFLAG_EMERGENCY_LANDING)
{
StickYaw = 0;
StickNick = 0;
907,7 → 910,7
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
tmp_long2 /= 16;
if((MaxStickNick > 32) || (MaxStickRoll > 32)) // reduce effect during stick commands
if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
{
tmp_long /= 3;
tmp_long2 /= 3;
959,7 → 962,7
CorrectionRoll = IntegralErrorRoll / ParamSet.GyroAccTrim;
AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER;
if((MaxStickNick > 32) || (MaxStickRoll > 32) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25))
if((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25))
{
AttitudeCorrectionNick /= 2;
AttitudeCorrectionRoll /= 2;
1026,7 → 1029,7
else
{
cnt = 0;
BadCompassHeading = 500;
BadCompassHeading = 1000;
}
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
// correct Gyro Offsets
1066,7 → 1069,7
else
{
cnt = 0;
BadCompassHeading = 500;
BadCompassHeading = 1000;
}
// correct Gyro Offsets
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
1109,11 → 1112,10
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(abs(StickYaw) > 15 ) // yaw stick is activated
{
BadCompassHeading = 1000;
if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX))
{
UpdateCompassCourse = 1;
CompassCourse = YawGyroHeading;
BadCompassHeading = 250;
}
}
// exponential stick sensitivity in yawring rate
1134,7 → 1136,7
{
int16_t w, v, r,correction, error;
if(CompassCalState && MotorsOn == 0 )
if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) )
{
SetCompassCalState();
#ifdef USE_KILLAGREG
1153,32 → 1155,31
#endif
// get maximum attitude angle
w = abs(IntegralNick/512);
v = abs(IntegralRoll /512);
w = abs(IntegralNick / 512);
v = abs(IntegralRoll / 512);
if(v > w) w = v;
// update compass course
if (w < 25 && UpdateCompassCourse && !BadCompassHeading)
{
BeepTime = 200;
CompassCourse = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
UpdateCompassCourse = 0;
}
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;
correction = w / 8 + 1;
if(!BadCompassHeading && w < 25)
{
YawGyroDrift += error;
if(UpdateCompassCourse)
{
BeepTime = 200;
CompassCourse = (YawGyroHeading / YAW_GYRO_DEG_FACTOR);
UpdateCompassCourse = 0;
}
}
YawGyroHeading += (error * 8) / correction;
w = (w * FCParam.CompassYawEffect) / 64;
w = (w * FCParam.CompassYawEffect) / 32;
w = FCParam.CompassYawEffect - w;
if(w > 0)
if(w >= 0)
{
if(BadCompassHeading)
{ // wait a while
BadCompassHeading--;
}
else
{ //
YawGyroDrift += error;
if(!BadCompassHeading)
{
v = 64 + (MaxStickNick + MaxStickRoll) / 8;
// calc course deviation
r = ((540 + (YawGyroHeading / YAW_GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180;
1189,10 → 1190,14
else if (v < -w) v = -w;
Reading_IntegralGyroYaw += v;
}
else
{ // wait a while
BadCompassHeading--;
}
}
else
{ // ignore compass at extreme attitudes for a while
BadCompassHeading = 250;
BadCompassHeading = 500;
}
}
}
1203,11 → 1208,8
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
{
GPS_I_Factor = FCParam.UserParam2;
GPS_P_Factor = FCParam.UserParam5;
GPS_D_Factor = FCParam.UserParam6;
if(EmergencyLanding) GPS_Main(230); // enables Comming Home
else GPS_Main(Poti3); // behavior controlled by Poti3
GPS_Main();
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
}
else
{
1233,7 → 1235,7
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = RC_Quality;
DebugOut.Analog[11] = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
DebugOut.Analog[16] = Mean_AccTop;
//DebugOut.Analog[16] = Mean_AccTop;
DebugOut.Analog[20] = ServoValue;
1297,7 → 1299,7
GasMixFraction *= STICK_GAIN;
// If height control is activated and no emergency landing is active
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) )
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && !(MKFlags & MKFLAG_EMERGENCY_LANDING) )
{
int tmp_int;
// if height control is activated by an rc channel
1390,7 → 1392,7
MotorValue = GasMixFraction + pd_result + YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Front = MotorValue;
1399,7 → 1401,7
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;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Rear = MotorValue;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Roll-Axis
1419,7 → 1421,7
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;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Left = MotorValue;
// Motor Right
1427,7 → 1429,7
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;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Right = MotorValue;
}

/branches/V0.70d Code Redesign killagreg/fc.h
19,7 → 19,7
uint8_t CompassYawEffect;
uint8_t Gyro_P;
uint8_t Gyro_I;
uint8_t Gier_P;
uint8_t Yaw_P;
uint8_t I_Factor;
uint8_t UserParam1;
uint8_t UserParam2;
34,18 → 34,26
uint8_t Yaw_PosFeedback;
uint8_t Yaw_NegFeedback;
uint8_t DynamicStability;
uint8_t ExternalControl;
uint8_t J16Timing;
uint8_t J17Timing;
uint8_t NaviGpsModeControl;
uint8_t NaviGpsGain;
uint8_t NaviGpsP;
uint8_t NaviGpsI;
uint8_t NaviGpsD;
uint8_t NaviGpsACC;
} fc_param_t;
extern fc_param_t FCParam;
extern volatile uint16_t I2CTimeout;
// attitude
extern volatile int32_t IntegralNick, IntegralRoll, IntegralYaw;
extern volatile int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
extern int32_t IntegralNick, IntegralRoll, IntegralYaw;
extern int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
// offsets
extern volatile int16_t AdNeutralNick, AdNeutralRoll, AdNeutralYaw;
extern int16_t AdNeutralNick, AdNeutralRoll, AdNeutralYaw;
extern volatile int16_t NeutralAccX, NeutralAccY;
extern volatile float NeutralAccZ;
65,7 → 73,7
extern int SetPointHeight;
// mean accelerations
extern volatile int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop;
extern int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop;
// acceleration send to navi board
extern int16_t NaviAccNick, NaviAccRoll, NaviCntAcc;
100,10 → 108,22
// current stick elongations
extern int16_t MaxStickNick, MaxStickRoll, MaxStickYaw;
extern uint8_t MotorsOn;
extern uint8_t EmergencyLanding;
extern uint16_t Model_Is_Flying;
// MKFlags
#define MKFLAG_MOTOR_RUN 0x01
#define MKFLAG_FLY 0x02
#define MKFLAG_CALIBRATE 0x04
#define MKFLAG_START 0x08
#define MKFLAG_EMERGENCY_LANDING 0x10
#define MKFLAG_RESERVE1 0x20
#define MKFLAG_RESERVE2 0x40
#define MKFLAG_RESERVE3 0x80
volatile extern uint8_t MKFlags;
#endif //_FC_H

/branches/V0.70d Code Redesign killagreg/gps.c
8,26 → 8,25
#include "rc.h"
#include "eeprom.h"
#define TSK_IDLE 0
#define TSK_HOLD 1
#define TSK_HOME 2
typedef enum
{
GPS_FLIGHT_MODE_UNDEF,
GPS_FLIGHT_MODE_FREE,
GPS_FLIGHT_MODE_AID,
GPS_FLIGHT_MODE_HOME,
} FlightMode_t;
#define GPS_STICK_SENSE 15 // must be at least in a range where 90% of the trimming does not switch of the GPS function
#define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes
#define GPS_STICK_LIMIT 200 // limit of gps stick control to avoid critical flight attitudes
#define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral
#define GPS_P_LIMIT 25
#define GPS_P_LIMIT 3200
uint8_t GPS_P_Factor = 0, GPS_I_Factor = 0, GPS_D_Factor = 0;
typedef struct
{
int32_t Longitude;
int32_t Latitude;
int32_t Altitude;
uint8_t Status;
Status_t Status;
} GPS_Pos_t;
// GPS coordinates for hold position
34,56 → 33,110
GPS_Pos_t HoldPosition = {0,0,0,INVALID};
// GPS coordinates for home position
GPS_Pos_t HomePosition = {0,0,0,INVALID};
// the current flight mode
FlightMode_t FlightMode = GPS_FLIGHT_MODE_UNDEF;
// ---------------------------------------------------------------------------------
// checks nick and roll sticks for manual control
uint8_t IsManualControlled(void)
void GPS_UpdateParameter(void)
{
if ( (abs(PPM_in[ParamSet.ChannelAssignment[CH_NICK]]) < GPS_STICK_SENSE) && (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < GPS_STICK_SENSE)) return 0;
else return 1;
}
static FlightMode_t FlightModeOld = GPS_FLIGHT_MODE_UNDEF;
// set home position to current positon
void GPS_SetHomePosition(void)
{
if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
if((RC_Quality < 100) || (MKFlags & MKFLAG_EMERGENCY_LANDING))
{
HomePosition.Longitude = GPSInfo.longitude;
HomePosition.Latitude = GPSInfo.latitude;
HomePosition.Altitude = GPSInfo.altitude;
HomePosition.Status = VALID;
BeepTime = 1000; // signal if new home position was set
FlightMode = GPS_FLIGHT_MODE_FREE;
}
else
{
HomePosition.Status = INVALID;
if (FCParam.NaviGpsModeControl < 50) FlightMode = GPS_FLIGHT_MODE_AID;
else if(FCParam.NaviGpsModeControl < 180) FlightMode = GPS_FLIGHT_MODE_FREE;
else FlightMode = GPS_FLIGHT_MODE_HOME;
}
if (FlightMode != FlightModeOld)
{
BeepTime = 100;
}
FlightModeOld = FlightMode;
}
// set hold position to current positon
void GPS_SetHoldPosition(void)
// ---------------------------------------------------------------------------------
// This function defines a good GPS signal condition
uint8_t GPS_IsSignalOK(void)
{
if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
static uint8_t GPSFix = 0;
if( (GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D) && (GPSInfo.flags & FLAG_GPSFIXOK) && ((GPSInfo.satnum >= ParamSet.NaviGpsMinSat) || GPSFix))
{
HoldPosition.Longitude = GPSInfo.longitude;
HoldPosition.Latitude = GPSInfo.latitude;
HoldPosition.Altitude = GPSInfo.altitude;
HoldPosition.Status = VALID;
GPSFix = 1;
return(1);
}
else
else return (0);
}
// ---------------------------------------------------------------------------------
// rescale xy-vector length to limit
uint8_t GPS_LimitXY(int32_t *x, int32_t *y, int32_t limit)
{
uint8_t retval = 0;
int32_t len;
len = (int32_t)c_sqrt(*x * *x + *y * *y);
if (len > limit)
{
HoldPosition.Status = INVALID;
// normalize control vector components to the limit
*x = (*x * limit) / len;
*y = (*y * limit) / len;
retval = 1;
}
return(retval);
}
// clear home position
void GPS_ClearHomePosition(void)
// checks nick and roll sticks for manual control
uint8_t GPS_IsManualControlled(void)
{
HomePosition.Status = INVALID;
if ( (abs(PPM_in[ParamSet.ChannelAssignment[CH_NICK]]) < ParamSet.NaviStickThreshold) && (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < ParamSet.NaviStickThreshold)) return 0;
else return 1;
}
// set given position to current gps position
uint8_t GPS_SetCurrPosition(GPS_Pos_t * pGPSPos)
{
uint8_t retval = 0;
if(pGPSPos == NULL) return(retval); // bad pointer
if(GPS_IsSignalOK())
{ // is GPS signal condition is fine
pGPSPos->Longitude = GPSInfo.longitude;
pGPSPos->Latitude = GPSInfo.latitude;
pGPSPos->Altitude = GPSInfo.altitude;
pGPSPos->Status = NEWDATA;
retval = 1;
}
else
{ // bad GPS signal condition
pGPSPos->Status = INVALID;
retval = 0;
}
return(retval);
}
// clear position
uint8_t GPS_ClearPosition(GPS_Pos_t * pGPSPos)
{
uint8_t retval = 0;
if(pGPSPos == NULL) return(retval); // bad pointer
else
{
pGPSPos->Longitude = 0;
pGPSPos->Latitude = 0;
pGPSPos->Altitude = 0;
pGPSPos->Status = INVALID;
retval = 1;
}
return (retval);
}
// disable GPS control sticks
void GPS_Neutral(void)
{
96,7 → 149,7
// then the P part of the controller is deactivated.
void GPS_PIDController(GPS_Pos_t *pTargetPos)
{
int32_t temp, temp1, PID_Nick, PID_Roll;
int32_t PID_Nick, PID_Roll;
int32_t coscompass, sincompass;
int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
106,7 → 159,7
static GPS_Pos_t *pLastTargetPos = 0;
// if GPS data and Compass are ok
if((GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D) && (CompassHeading >= 0) )
if( GPS_IsSignalOK() && (CompassHeading >= 0) )
{
if(pTargetPos != NULL) // if there is a target position
156,28 → 209,21
//Calculate PID-components of the controller (negative sign for compensation)
// P-Part
P_North = -((int32_t)GPS_P_Factor * GPSPosDev_North)/2048;
P_East = -((int32_t)GPS_P_Factor * GPSPosDev_East)/2048;
P_North = -((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/16;
P_East = -((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/16;
// I-Part
I_North = -((int32_t)GPS_I_Factor * GPSPosDevIntegral_North)/8192;
I_East = -((int32_t)GPS_I_Factor * GPSPosDevIntegral_East)/8192;
I_North = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/64;
I_East = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/64;
// combine P- & I-Part
PID_North = P_North + I_North;
PID_East = P_East + I_East;
//limit PI-Part to limit the max velocity
//temp1 = ((int32_t)GPS_D_Factor * MAX_VELOCITY)/512; // the PI-Part limit
temp = (int32_t)c_sqrt(PID_North*PID_North + PID_East*PID_East); // the current PI-Part
if(temp > GPS_P_LIMIT) // P-Part limit is reached
//limit PI-Part
if(!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT))
{
// normalize P-part components to the P-Part limit
PID_North = (PID_North * GPS_P_LIMIT)/temp;
PID_East = (PID_East * GPS_P_LIMIT)/temp;
}
else // PI-Part under its limit
{
// P-Part limit is not reached
// update position error integrals
GPSPosDevIntegral_North += GPSPosDev_North/16;
if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT;
188,14 → 234,17
}
// D-Part
D_North = -((int32_t)GPS_D_Factor * GPSInfo.velnorth)/512;
D_East = -((int32_t)GPS_D_Factor * GPSInfo.veleast)/512;
D_North = -((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/4;
D_East = -((int32_t)FCParam.NaviGpsD * GPSInfo.veleast)/4;
// combine PI- and D-Part
PID_North += D_North;
PID_East += D_East;
// scale combination with gain.
PID_North = (PID_North * (int32_t)FCParam.NaviGpsGain) / (100 * 32);
PID_East = (PID_East * (int32_t)FCParam.NaviGpsGain) / (100 * 32);
// GPS to nick and roll settings
// A positive nick angle moves head downwards (flying forward).
210,23 → 259,16
// 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.
coscompass = (int32_t)c_cos_8192(CompassHeading);
sincompass = (int32_t)c_sin_8192(CompassHeading);
coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR);
sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR);
PID_Roll = (coscompass * PID_East - sincompass * PID_North) / 8192;
PID_Nick = -1*((sincompass * PID_East + coscompass * PID_North) / 8192);
// limit resulting GPS control vector
temp = (int32_t)c_sqrt(PID_Roll*PID_Roll + PID_Nick*PID_Nick);
if (temp > GPS_STICK_LIMIT)
{
// normalize control vector components to the limit
PID_Roll = (PID_Roll * GPS_STICK_LIMIT)/temp;
PID_Nick = (PID_Nick * GPS_STICK_LIMIT)/temp;
}
GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
GPS_Roll = (int16_t)PID_Roll;
GPS_Nick = (int16_t)PID_Nick;
GPS_Roll = (int16_t)PID_Roll;
}
else // invalid GPS data or bad compass reading
{
240,23 → 282,24
void GPS_Main(uint8_t ctrl)
void GPS_Main(void)
{
static uint8_t GPS_Task = TSK_IDLE;
static uint8_t GPS_P_Delay = 0;
int16_t satbeep;
uint16_t beep_rythm = 0;
// ctrl enables the gps feature
if(ctrl < 70) GPS_Task = TSK_IDLE;
else if (ctrl < 160) GPS_Task = TSK_HOLD;
else GPS_Task = TSK_HOME; // ctrl >= 160
GPS_UpdateParameter();
// store home position if start of flight flag is set
if(MKFlags & MKFLAG_CALIBRATE)
{
GPS_SetCurrPosition(&HomePosition);
}
switch(GPSInfo.status)
{
case INVALID: // invalid gps data
GPS_Neutral();
if(GPS_Task != TSK_IDLE)
if(FlightMode != GPS_FLIGHT_MODE_FREE)
{
BeepTime = 100; // beep if signal is neccesary
}
272,25 → 315,28
GPSInfo.status = INVALID;
}
break;
case VALID: // new valid data from gps device
case NEWDATA: // new valid data from gps device
// if the gps data quality is good
if (GPSInfo.satfix == SATFIX_3D)
beep_rythm++;
if (GPS_IsSignalOK())
{
switch(GPS_Task) // check what's to do
switch(FlightMode) // check what's to do
{
case TSK_IDLE:
case GPS_FLIGHT_MODE_FREE:
// update hold position to current gps position
GPS_SetHoldPosition(); // can get invalid if gps signal is bad
GPS_SetCurrPosition(&HoldPosition); // can get invalid if gps signal is bad
// disable gps control
GPS_Neutral();
break; // eof TSK_IDLE
case TSK_HOLD:
break;
case GPS_FLIGHT_MODE_AID:
if(HoldPosition.Status != INVALID)
{
if( IsManualControlled() ) // MK controlled by user
if( GPS_IsManualControlled() ) // MK controlled by user
{
// update hold point to current gps position
GPS_SetHoldPosition();
GPS_SetCurrPosition(&HoldPosition);
// disable gps control
GPS_Neutral();
GPS_P_Delay = 0;
297,10 → 343,10
}
else // GPS control active
{
if(GPS_P_Delay<7)
if(GPS_P_Delay < 7)
{ // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
GPS_P_Delay++;
GPS_SetHoldPosition(); // update hold point to current gps position
GPS_SetCurrPosition(&HoldPosition); // update hold point to current gps position
GPS_PIDController(NULL); // activates only the D-Part
}
else GPS_PIDController(&HoldPosition);// activates the P&D-Part
308,17 → 354,18
}
else // invalid Hold Position
{ // try to catch a valid hold position from gps data input
GPS_SetHoldPosition();
GPS_SetCurrPosition(&HoldPosition);
GPS_Neutral();
}
break; // eof TSK_HOLD
case TSK_HOME:
break;
case GPS_FLIGHT_MODE_HOME:
if(HomePosition.Status != INVALID)
{
// update hold point to current gps position
// to avoid a flight back if home comming is deactivated
GPS_SetHoldPosition();
if( IsManualControlled() ) // MK controlled by user
GPS_SetCurrPosition(&HoldPosition);
if( GPS_IsManualControlled() ) // MK controlled by user
{
GPS_Neutral();
}
334,7 → 381,7
if (HoldPosition.Status != INVALID)
{
if( IsManualControlled() ) // MK controlled by user
if( GPS_IsManualControlled() ) // MK controlled by user
{
GPS_Neutral();
}
345,7 → 392,7
}
else
{ // try to catch a valid hold position
GPS_SetHoldPosition();
GPS_SetCurrPosition(&HoldPosition);
GPS_Neutral();
}
}
354,16 → 401,13
GPS_Neutral();
break; // eof default
} // eof switch GPS_Task
} // eof 3D-FIX
else // no 3D-SATFIX
} // eof gps data quality is good
else // gps data quality is bad
{ // disable gps control
GPS_Neutral();
if(GPS_Task != TSK_IDLE)
{
satbeep = 1600 - (int16_t)GPSInfo.satnum * 200; // is zero at 8 sats
if (satbeep < 0) satbeep = 0;
BeepTime = 50 + (uint16_t)satbeep; // max 1650 * 0.1 ms =
}
// beep if signal is not sufficient
if(!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100;
else if (GPSInfo.satnum < ParamSet.NaviGpsMinSat && !(beep_rythm % 5)) BeepTime = 10;
}
// set current data as processed to avoid further calculations on the same gps data
GPSInfo.status = PROCESSED;

/branches/V0.70d Code Redesign killagreg/gps.h
3,13 → 3,7
#include <inttypes.h>
extern uint8_t GPS_P_Factor;
extern uint8_t GPS_I_Factor;
extern uint8_t GPS_D_Factor;
extern void GPS_Main(void);
extern void GPS_Main(uint8_t ctrl);
extern void GPS_SetHomePosition(void);
extern void GPS_ClearHomePosition(void);
#endif //_GPS_H

/branches/V0.70d Code Redesign killagreg/led.c
1,11 → 1,11
#include <inttypes.h>
#include "led.h"
#include "fc.h"
#include "eeprom.h"
uint16_t LED1_Time = 0;
uint16_t LED2_Time = 0;
uint8_t J16Blinkcount = 0, J16Mask = 1;
uint8_t J17Blinkcount = 0, J17Mask = 1;
// initializes the LED control outputs J16, J17
void LED_Init(void)
{
13,30 → 13,53
DDRC |= (1<<DDC2)|(1<<DDC3);
J16_OFF;
J17_OFF;
J16Blinkcount = 0; J16Mask = 128;
J17Blinkcount = 0; J17Mask = 128;
}
// called in UpdateMotors() every 2ms
// called in main loop every 2ms
void LED_Update(void)
{
static uint16_t J16_blinkcount = 0;
static uint16_t J17_blinkcount = 0;
static int8_t delay = 0;
if (LED1_Time < 20) J16_ON;
else if(LED1_Time < 220)
if(!delay--) // 10 ms intervall
{
if((2 * J16_blinkcount) < LED1_Time) J16_ON;
else J16_OFF;
if(J16_blinkcount++ >= LED1_Time) J16_blinkcount = 0;
}
else J16_ON;
delay = 4;
if (LED2_Time < 20) J17_ON;
else if(LED2_Time < 220)
{
if((2 * J17_blinkcount) < LED2_Time) J17_ON;
else J17_OFF;
if(J17_blinkcount++ >= LED2_Time) J17_blinkcount = 0;
if ((ParamSet.J16Timing > 250) && (FCParam.J16Timing > 230))
{
if(ParamSet.J16Bitmask & 128) J16_ON;
else J16_OFF;
}
else if ((ParamSet.J16Timing > 250) && (FCParam.J16Timing < 10))
{
if(ParamSet.J16Bitmask & 128) J16_OFF;
else J16_ON;
}
else if(!J16Blinkcount--)
{
J16Blinkcount = FCParam.J16Timing - 1;
if(J16Mask == 1) J16Mask = 128; else J16Mask /= 2;
if(J16Mask & ParamSet.J16Bitmask) J16_ON; else J16_OFF;
}
if ((ParamSet.J17Timing > 250) && (FCParam.J17Timing > 230))
{
if(ParamSet.J17Bitmask & 128) J17_ON;
else J17_OFF;
}
else if ((ParamSet.J17Timing > 250) && (FCParam.J17Timing < 10))
{
if(ParamSet.J17Bitmask & 128) J17_OFF;
else J17_ON;
}
else if(!J17Blinkcount--)
{
J17Blinkcount = FCParam.J17Timing - 1;
if(J17Mask == 1) J17Mask = 128; else J17Mask /= 2;
if(J17Mask & ParamSet.J17Bitmask) J17_ON; else J17_OFF;
}
}
else J17_ON;
}

/branches/V0.70d Code Redesign killagreg/led.h
11,12 → 11,9
#define J17_TOGGLE PORTC ^= (1<<PORTC3)
extern uint16_t LED1_Time;
extern uint16_t LED2_Time;
void LED_Init(void);
void LED_Update(void);
extern void LED_Init(void);
extern void LED_Update(void);
#endif //_LED_H

/branches/V0.70d Code Redesign killagreg/main.c
98,12 → 98,16
DDRB = 0x00;
PORTB = 0x00;
for(timer = 0; timer < 1000; timer++); // make some delay
if(PINB & (1<<PINB0)) BoardRelease = 11;
if(PINB & (1<<PINB0))
{
if(PINB & (1<<PINB1)) BoardRelease = 13;
else BoardRelease = 11;
}
else BoardRelease = 10;
// set LED ports as output
DDRB |= (1<<DDB1)|(1<<DDB0);
ROT_ON;
RED_ON;
GRN_OFF;
// disable watchdog
118,10 → 122,10
StickRoll = 0;
StickNick = 0;
ROT_OFF;
RED_OFF;
// initalize modules
LED_Init();
//LED_Init(); Is done within ParamSet_Init() below
TIMER0_Init();
TIMER2_Init();
USART0_Init();
152,6 → 156,7
VersionInfo.Major = VERSION_MAJOR;
VersionInfo.Minor = VERSION_MINOR;
VersionInfo.PCCompatible = VERSION_COMPATIBLE;
VersionInfo.Hardware = 1; // Flight Control
printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",BoardRelease/10,BoardRelease%10, VERSION_MAJOR, VERSION_MINOR,VERSION_INDEX + 'a');
printf("\n\r==============================");
211,7 → 216,7
SetNeutral();
ROT_OFF;
RED_OFF;
BeepTime = 2000;
ExternControl.Digital[0] = 0x55;
231,7 → 236,7
{
if(UpdateMotor) // control interval
{
UpdateMotor=0; // reset Flag, is enabled every 2 ms by isr of timer0
UpdateMotor = 0; // reset Flag, is enabled every 2 ms by isr of timer0
//PORTD |= (1<<PORTD4);
MotorControl();
//PORTD &= ~(1<<PORTD4);
238,7 → 243,7
SendMotorData();
ROT_OFF;
RED_OFF;
if(PcAccess) PcAccess--;
else
255,7 → 260,7
{
I2CTimeout = 5;
I2C_Reset();
if((BeepModulation == 0xFFFF) && MotorsOn)
if((BeepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN) )
{
BeepTime = 10000; // 1 second
BeepModulation = 0x0080;
264,10 → 269,10
else
{
I2CTimeout--;
ROT_OFF;
RED_OFF;
}
if(SIO_DEBUG && (!UpdateMotor || !MotorsOn))
if(SIO_DEBUG && (!UpdateMotor || !(MKFlags & MKFLAG_MOTOR_RUN) ))
{
USART0_TransmitTxData();
USART0_ProcessRxData();
290,6 → 295,8
#endif
timer = SetDelay(20); // every 20 ms
}
LED_Update();
}
#ifdef USE_NAVICTRL

/branches/V0.70d Code Redesign killagreg/main.h
3,17 → 3,13
#include <avr/io.h>
//Hier die Quarz Frequenz einstellen
#if defined (__AVR_ATmega32__)
#define SYSCLK 20000000L //Quarz Frequenz in Hz
#endif
//set crystal frequency here
#if defined (__AVR_ATmega644__)
#define SYSCLK 20000000L //Quarz Frequenz in Hz
#define SYSCLK 20000000L //crystal freqency in Hz
#endif
#if defined (__AVR_ATmega644P__)
#define SYSCLK 20000000L //Quarz Frequenz in Hz
#define SYSCLK 20000000L //crystal freqency in Hz
#endif
#define F_CPU SYSCLK
20,11 → 16,11
// neue Hardware
#define ROT_OFF {if(BoardRelease == 10) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);}
#define ROT_ON {if(BoardRelease == 10) PORTB |= (1<<PORTB0); else PORTB &=~(1<<PORTB0);}
#define ROT_FLASH PORTB ^= (1<<PORTB0)
#define GRN_OFF PORTB &=~(1<<PORTB1)
#define GRN_ON PORTB |= (1<<PORTB1)
#define RED_OFF {if(BoardRelease == 10) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);}
#define RED_ON {if(BoardRelease == 10) PORTB |= (1<<PORTB0); else PORTB &=~(1<<PORTB0);}
#define RED_FLASH PORTB ^= (1<<PORTB0)
#define GRN_OFF {if(BoardRelease < 12) PORTB &=~(1<<PORTB1); else PORTB |= (1<<PORTB1);}
#define GRN_ON {if(BoardRelease < 12) PORTB |= (1<<PORTB1); else PORTB &=~(1<<PORTB1);}
#define GRN_FLASH PORTB ^= (1<<PORTB1)
#include <inttypes.h>

/branches/V0.70d Code Redesign killagreg/makefile
1,14 → 1,14
#--------------------------------------------------------------------
# MCU name
#MCU = atmega644
MCU = atmega644p
MCU = atmega644
#MCU = atmega644p
F_CPU = 20000000
#-------------------------------------------------------------------
VERSION_MAJOR = 0
VERSION_MINOR = 69
VERSION_INDEX = 10
VERSION_MINOR = 70
VERSION_INDEX = 3
VERSION_COMPATIBLE = 7 # PC-Kompatibilität
VERSION_COMPATIBLE = 8 # PC-Kompatibilität
#-------------------------------------------------------------------
#OPTIONS
# Use one of the extensions for a gps solution

/branches/V0.70d Code Redesign killagreg/menu.c
121,7 → 121,7
break;
case 2:// Attitude Menu Item
LCD_printfxy(0,0,"Attitude");
LCD_printfxy(0,1,"Nick: %5i",IntegralNick/1024);
LCD_printfxy(0,1,"Nick: %5i",IntegralNick/1024);
LCD_printfxy(0,2,"Roll: %5i",IntegralRoll/1024);
LCD_printfxy(0,3,"Heading: %5i",CompassHeading);
break;
139,22 → 139,31
break;
case 5:// Gyro Sensor Menu Item
LCD_printfxy(0,0,"Gyro - Sensor");
if(BoardRelease == 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)",Reading_GyroYaw, AdNeutralYaw);
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);
break;
case 11:
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);
break;
case 12:
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 );
break;
}
else
{
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)",Reading_GyroYaw, AdNeutralYaw/2);
}
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,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);
break;

/branches/V0.70d Code Redesign killagreg/mm3.c
287,7 → 287,7
// change to z-Axis
break;
case 4:
ROT_ON; // find Min and Max of the Z-axis
RED_ON; // find Min and Max of the Z-axis
if(MM3.z_axis < z_min) z_min = MM3.z_axis;
if(MM3.z_axis > z_max) z_max = MM3.z_axis;
break;
328,7 → 328,7
uint16_t timer;
GRN_ON;
ROT_OFF;
RED_OFF;
// get maximum and minimum reading of all axis
while (measurement)
355,7 → 355,7
if (!beeper)
{
ROT_FLASH;
RED_FLASH;
GRN_FLASH;
BeepTime = 50;
beeper = 50;

/branches/V0.70d Code Redesign killagreg/spi.c
10,6 → 10,7
#include "eeprom.h"
#include "uart.h"
#include "timer0.h"
#include "analog.h"
#define SPI_TXSYNCBYTE1 0xAA
#define SPI_TXSYNCBYTE2 0x83
42,11 → 43,9
uint8_t SPITransferCompleted, SPI_ChkSum;
uint8_t SPI_RxDataValid;
uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_CAL_COMPASS };
uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_CAL_COMPASS };
uint8_t SPI_CommandCounter = 0;
#ifdef USE_SPI_COMMUNICATION
/*********************************************/
/* Initialize SPI interface to NaviCtrl */
/*********************************************/
103,8 → 102,26
ToNaviCtrl.Param.Byte[5] = FCParam.UserParam6;
ToNaviCtrl.Param.Byte[6] = FCParam.UserParam7;
ToNaviCtrl.Param.Byte[7] = FCParam.UserParam8;
ToNaviCtrl.Param.Byte[8] = MKFlags;
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); // calibrate and start are temporal states that are cleared immediately after transmitting
ToNaviCtrl.Param.Byte[9] = (uint8_t)UBat;
ToNaviCtrl.Param.Byte[10] = ParamSet.LowVoltageWarning;
ToNaviCtrl.Param.Byte[11] = GetActiveParamSet();
break;
case SPI_CMD_PARAMETER1:
ToNaviCtrl.Param.Byte[0] = FCParam.NaviGpsModeControl; // Parameters for the Naviboard
ToNaviCtrl.Param.Byte[1] = FCParam.NaviGpsGain;
ToNaviCtrl.Param.Byte[2] = FCParam.NaviGpsP;
ToNaviCtrl.Param.Byte[3] = FCParam.NaviGpsI;
ToNaviCtrl.Param.Byte[4] = FCParam.NaviGpsD;
ToNaviCtrl.Param.Byte[5] = FCParam.NaviGpsACC;
ToNaviCtrl.Param.Byte[6] = ParamSet.NaviGpsMinSat;
ToNaviCtrl.Param.Byte[7] = ParamSet.NaviStickThreshold;
ToNaviCtrl.Param.Byte[8] = 15; // MaxRadius
break;
case SPI_CMD_STICK:
tmp = PPM_in[ParamSet.ChannelAssignment[CH_GAS]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
ToNaviCtrl.Param.Byte[0] = (int8_t) tmp;
119,7 → 136,6
ToNaviCtrl.Param.Byte[6] = (uint8_t) Poti3;
ToNaviCtrl.Param.Byte[7] = (uint8_t) Poti4;
ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality;
ToNaviCtrl.Param.Byte[9] = (uint8_t) MotorsOn;
break;
case SPI_CMD_CAL_COMPASS:
144,8 → 160,8
// update gps controls
if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
{
GPS_Nick = FromNaviCtrl.GPS_Nick;
GPS_Roll = FromNaviCtrl.GPS_Roll;
GPS_Nick = FromNaviCtrl.GPS_Nick;
GPS_Roll = FromNaviCtrl.GPS_Roll;
}
// update compass readings
if(FromNaviCtrl.CompassHeading <= 360)
203,7 → 219,7
ToNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
SPITransferCompleted = 0; // tranfer is in progress
SPITransferCompleted = 0; // transfer is in progress
UpdateSPI_Buffer(); // update data in ToNaviCtrl
SPI_TxBufferIndex = 1; //proceed with 2nd byte
309,6 → 325,4
}
#endif //USE_SPI_COMMUNICATION

/branches/V0.70d Code Redesign killagreg/spi.h
5,7 → 5,6
//#include <util/delay.h>
#include <inttypes.h>
#define USE_SPI_COMMUNICATION
#define SPI_PROTOCOL_COMP 1
54,6 → 53,7
#define SPI_CMD_USER 10
#define SPI_CMD_STICK 11
#define SPI_CMD_CAL_COMPASS 12
#define SPI_CMD_PARAMETER1 13
typedef struct
{
93,7 → 93,7
int16_t GPS_Yaw;
int16_t CompassHeading;
int16_t Status;
uint8_t BeepTime;
uint16_t BeepTime;
union
{
int8_t Byte[12];
104,23 → 104,14
uint8_t Chksum;
} FromNaviCtrl_t;
#ifdef USE_SPI_COMMUNICATION
extern ToNaviCtrl_t ToNaviCtrl;
extern FromNaviCtrl_t FromNaviCtrl;
extern void SPI_MasterInit(void);
extern void SPI_StartTransmitPacket(void);
extern void SPI_TransmitByte(void);
#else
void SPI_MasterInit(void);
void SPI_StartTransmitPacket(void);
void SPI_TransmitByte(void);
// -------------------------------- Dummy -----------------------------------------
#define SPI_MasterInit() ;
#define SPI_StartTransmitPacket() ;
#define UpdateSPI_Buffer() ;
#define SPI_TransmitByte() ;
#endif
#endif //_SPI_H

/branches/V0.70d Code Redesign killagreg/timer2.c
2,10 → 2,12
#include <avr/interrupt.h>
#include "fc.h"
#include "eeprom.h"
#include "uart.h"
volatile int16_t ServoValue = 0;
volatile uint16_t ServoValue = 0;
/*****************************************************/
/* Initialize Timer 2 */
/*****************************************************/
20,33 → 22,33
// set PD7 as output of the PWM for nick servo
DDRD |=(1<<DDD7);
PORTD |= (1<<PORTD7);
PORTD &= ~(1<<PORTD7); // set PD7 to low
// Timer/Counter 2 Control Register A
// Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1)
// PD7: Clear OC2B on Compare Match, set OC2B at BOTTOM, non inverting PWM (Bits: COM2A1 = 1, COM2A0 = 0)
// PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
// PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
TCCR2A &= ~((1<<COM2B1)|(1<<COM2B0)|(1<<COM2A0));
TCCR2A |= (1<<COM2A1)|(1<<WGM21)|(1<<WGM20);
TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0));
TCCR2A |= (1<<WGM21)|(1<<WGM20);
// Timer/Counter 2 Control Register B
// Set clock divider for timer 2 to SYSKLOCK/256 = 20MHz / 256 = 78.128 kHz
// The timer increments from 0x00 to 0xFF with an update rate of 78.128 kHz or 12.8 us
// hence the timer overflow interrupt frequency is 78.128 kHz / 256 = 305.176 Hz or 3.276 ms
// 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
// divider 256 (Bits: CS022 = 1, CS21 = 1, CS20 = 0)
TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS20)|(1<<WGM22));
TCCR2B |= (1<<CS22)|(1<<CS21);
// divider 64 (Bits: CS022 = 1, CS21 = 0, CS20 = 0)
TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS21)|(1<<CS20)|(1<<WGM22));
TCCR2B |= (1<<CS22);
// Initialize the Output Compare Register A used for PWM generation on port PD7.
OCR2A = 10; //10 * 12.8us = 1.28 ms high time
// Initialize the Timer/Counter 2 Register
TCNT2 = 0;
// Initialize the Output Compare Register A used for PWM generation on port PD7.
OCR2A = 10;
// Timer/Counter 2 Interrupt Mask Register
// Enable timer output compare match A Interrupt only
TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2));
59,46 → 61,92
/*****************************************************/
/* Control Servo Position */
/*****************************************************/
ISR(TIMER2_COMPA_vect) // every OCR2A * 12.8 us (compare match)
ISR(TIMER2_COMPA_vect) // every 256 * 3.2 us = 0.819 us ( on compare match of TCNT2 and OC2A)
{
static uint8_t timer = 10;
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
if(!timer--)
{
// enable PWM on PD7 in non inverting mode
TCCR2A &= ~(0<<COM2A0);
TCCR2A |= (1<<COM2A1);
#define MULTIPLIER 4
ServoValue = FCParam.ServoNickControl;
// inverting movment of servo
if(ParamSet.ServoNickCompInvert & 0x01)
{
ServoValue += ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512;
}
else // non inverting movement of servo
{
ServoValue -= ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512;
}
// limit servo value to its parameter range definition
if(ServoValue < ParamSet.ServoNickMin)
{
ServoValue = ParamSet.ServoNickMin;
}
else if(ServoValue > ParamSet.ServoNickMax)
{
ServoValue = ParamSet.ServoNickMax;
}
// update PWM
OCR2A = ServoValue;
timer = ParamSet.ServoNickRefresh;
}
else
{
// disable PWM at PD7
TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0));
// set PD7 to low
PORTD &= ~(1<<PORTD7);
}
switch(ServoState)
{
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(ParamSet.ServoNickCompInvert & 0x01)
{ // inverting movement of servo
ServoValue += ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER);
}
else
{ // non inverting movement of servo
ServoValue -= ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER);
}
// limit servo value to its parameter range definition
if(ServoValue < ((uint16_t)ParamSet.ServoNickMin * 3) )
{
ServoValue = (uint16_t)ParamSet.ServoNickMin * 3;
}
else
if(ServoValue > ((uint16_t)ParamSet.ServoNickMax * 3) )
{
ServoValue = (uint16_t)ParamSet.ServoNickMax * 3;
}
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 thios 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);
break;
case 3:
case 2:
case 1:
if(ServoValue > 255) // is larger than a full timer 2 cycle
{
PORTD |= (1<<PORTD7); // set PD7 to high
TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A
ServoValue -= 255; // substract full timer cycle
}
else // the post pule must be generated
{
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;
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;
default:
// do nothing
break;
}
ServoState--;
}

/branches/V0.70d Code Redesign killagreg/timer2.h
3,10 → 3,11
#include <inttypes.h>
extern volatile int16_t ServoValue;
extern void TIMER2_Init(void);
extern volatile uint16_t ServoValue;
void TIMER2_Init(void);
#endif //_TIMER2_H

/branches/V0.70d Code Redesign killagreg/twimaster.c
7,11 → 7,31
#include "main.h"
#include "twimaster.h"
#include "fc.h"
#include "analog.h"
volatile uint8_t twi_state = 0;
volatile uint8_t motor = 0;
volatile uint8_t twi_state = 0;
volatile uint8_t motor_write = 0;
volatile uint8_t motor_read = 0;
volatile uint8_t dac_channel = 0;
volatile uint8_t motor_rx[8];
volatile uint16_t I2CTimeout = 100;
#define SCL_CLOCK 200000L
#define I2C_TIMEOUT 30000
#define TWSR_STATUS_MASK 0xF8
// for Master Transmitter Mode
#define I2C_STATUS_START 0x08
#define I2C_STATUS_REPEATSTART 0x10
#define I2C_STATUS_TX_SLA_ACK 0x18
#define I2C_STATUS_SLAW_NOACK 0x20
#define I2C_STATUS_TX_DATA_ACK 0x28
#define I2C_STATUS_TX_DATA_NOTACK 0x30
#define I2C_STATUS_RX_DATA_ACK 0x50
#define I2C_STATUS_RX_DATA_NOTACK 0x58
/**************************************************/
/* Initialize I2C (TWI) */
/**************************************************/
34,6 → 54,10
// set TWI Bit Rate Register
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
twi_state = 0;
motor_write = 0;
motor_read = 0;
SREG = sreg;
}
48,7 → 72,7
// enable TWI START Condition Bit (TWSTA = 1), MASTER
// disable TWI STOP Condition Bit (TWSTO = 0)
// disable TWI Write Collision Flag (TWWC = 0)
// enable i2c (TWIE = 1)
// enable i2c (TWEN = 1)
// enable TWI Interrupt (TWIE = 1)
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE);
}
64,31 → 88,11
// diable TWI START Condition Bit (TWSTA = 1), no MASTER
// enable TWI STOP Condition Bit (TWSTO = 1)
// disable TWI Write Collision Flag (TWWC = 0)
// enable i2c (TWIE = 1)
// enable i2c (TWEN = 1)
// disable TWI Interrupt (TWIE = 0)
TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN);
}
/****************************************/
/* Reset I2C */
/****************************************/
void I2C_Reset(void)
{
// stop i2c bus
I2C_Stop();
twi_state = 0;
motor = TWDR; // ??
motor = 0;
TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset
TWAMR = 0;
TWAR = 0;
TWDR = 0;
TWSR = 0;
TWBR = 0;
I2C_Init();
I2C_Start();
I2C_WriteByte(0);
}
/****************************************/
/* Write to I2C */
99,7 → 103,7
TWDR = byte;
// clear interrupt flag (TWINT = 1)
// enable i2c bus (TWEN = 1)
// enable intterupt (TWIW = 1)
// enable interrupt (TWIE = 1)
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
}
122,22 → 126,43
/****************************************/
/* Reset I2C */
/****************************************/
void I2C_Reset(void)
{
// stop i2c bus
I2C_Stop();
twi_state = 0;
motor_write = TWDR;
motor_write = 0;
motor_read = 0;
TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset
TWAMR = 0;
TWAR = 0;
TWDR = 0;
TWSR = 0;
TWBR = 0;
I2C_Init();
I2C_Start();
I2C_WriteByte(0);
}
/****************************************/
/* I2C ISR */
/****************************************/
ISR (TWI_vect)
{
static uint8_t motorread = 0;
switch (twi_state++) // First i2s_start from SendMotorData()
{
switch (twi_state++) // First i2c_start from SendMotorData()
{
// Master Transmit
case 0: // Send SLA-W
I2C_WriteByte(0x52+(motor*2));
I2C_WriteByte(0x52 + (motor_write * 2) );
break;
case 1: // Send Data to Salve
switch(motor++)
{
case 1: // Send Data to Slave
switch(motor_write)
{
case 0:
I2C_WriteByte(Motor_Front);
break;
150,16 → 175,25
case 3:
I2C_WriteByte(Motor_Left);
break;
}
}
break;
case 2: // repeat case 0+1 for all Slaves
if (motor<4) twi_state = 0;
case 2: // repeat case 0+1 for all motors
I2C_Stop();
if (motor_write < 3)
{
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 salve or switch Master Transmit -> Master Receive
break;
// Master Receive
case 3: // Send SLA-R
I2C_WriteByte(0x53+(motorread*2));
I2C_WriteByte(0x53 + (motor_read * 2) );
break;
case 4:
//Transmit 1st byte
166,19 → 200,69
I2C_ReceiveByte();
break;
case 5: //Read 1st byte and transmit 2nd Byte
motor_rx[motorread] = TWDR;
motor_rx[motor_read] = TWDR;
I2C_ReceiveLastByte();
break;
case 6:
//Read 2nd byte
motor_rx[motorread+4] = TWDR;
motorread++;
if (motorread > 3) motorread=0;
motor_rx[motor_read + 4] = TWDR;
motor_read++;
if (motor_read > 3) 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(AnalogOffsetNick); // 1st byte for Channel A
break;
case 1:
I2C_WriteByte(AnalogOffsetRoll); // 1st byte for Channel B
break;
case 2:
I2C_WriteByte(AnalogOffsetYaw ); // 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 = 0;
}
motor_write = 0;
motor_read = 0;
}
}

/branches/V0.70d Code Redesign killagreg/twimaster.h
4,30 → 4,17
#include <inttypes.h>
-
-//############################################################################
-// I2C Konstanten
-#define SCL_CLOCK 200000L
-#define I2C_TIMEOUT 30000
-#define I2C_START 0x08
-#define I2C_REPEATED_START 0x10
-#define I2C_TX_SLA_ACK 0x18
-#define I2C_TX_DATA_ACK 0x28
-#define I2C_RX_SLA_ACK 0x40
-#define I2C_RX_DATA_ACK 0x50
+#define TWI_STATE_MOTOR_TX 0
+#define TWI_STATE_GYRO_OFFSET_TX 7
-//############################################################################
-
extern volatile uint8_t twi_state;
-extern volatile uint8_t motor;
-extern volatile uint8_t motorread;
extern volatile uint8_t motor_rx[8];
+extern volatile uint16_t I2CTimeout;
-extern void I2C_Init (void); // Initialize I2C
-extern void I2C_Start (void); // Start I2C
+extern void I2C_Init (void); // Initialize I2C
+extern void I2C_Start(void); // Start I2C
extern void I2C_Stop (void); // Stop I2C
-extern void I2C_WriteByte (int8_t byte); // Write 1 Byte
extern void I2C_Reset(void); // Reset I2C
#endif

/branches/V0.70d Code Redesign killagreg/uart.c
81,7 → 81,7
"Motor_Rear ",
"Motor_Right ",
"Motor_Left ", //15
"Acc_Z ",
" ",
"SPI Error ",
"SPI Ok ",
" ",
371,6 → 371,7
Decode64((uint8_t *) &ExternControl,sizeof(ExternControl), 3, ReceivedBytes);
RemoteButtons |= ExternControl.RemoteButtons;
ConfirmFrame = ExternControl.Frame;
PcAccess = 255;
break;
case 'c': // extern control with debug request
Decode64((uint8_t *) &ExternControl,sizeof(ExternControl),3,ReceivedBytes);

/branches/V0.70d Code Redesign killagreg/uart.h
55,7 → 55,8
uint8_t Major;
uint8_t Minor;
uint8_t PCCompatible;
uint8_t Reserved[7];
uint8_t Hardware;
uint8_t Reserved[6];
} VersionInfo_t;
extern VersionInfo_t VersionInfo;

/branches/V0.70d Code Redesign killagreg/ubx.c
44,8 → 44,8
uint8_t res1; // reserved
uint8_t numSV; // Number of SVs used in navigation solution
uint32_t res2; // reserved
uint8_t Status;
} GPS_SOL_t;
Status_t Status;
} UBX_SOL_t;
typedef struct {
uint32_t ITOW; // ms GPS Millisecond Time of Week
55,8 → 55,8
int32_t HMSL; // mm Height above mean sea level
uint32_t Hacc; // mm Horizontal Accuracy Estimate
uint32_t Vacc; // mm Vertical Accuracy Estimate
uint8_t Status;
} GPS_POSLLH_t;
Status_t Status;
} UBX_POSLLH_t;
typedef struct {
uint32_t ITOW; // ms GPS Millisecond Time of Week
68,12 → 68,12
int32_t Heading; // 1e-05 deg Heading 2-D
uint32_t SAcc; // cm/s Speed Accuracy Estimate
uint32_t CAcc; // deg Course / Heading Accuracy Estimate
uint8_t Status;
} GPS_VELNED_t;
Status_t Status;
} UBX_VELNED_t;
GPS_SOL_t GpsSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
GPS_POSLLH_t GpsPosLlh = {0,0,0,0,0,0,0, INVALID};
GPS_VELNED_t GpsVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
UBX_SOL_t UbxSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
UBX_POSLLH_t UbxPosLlh = {0,0,0,0,0,0,0, INVALID};
UBX_VELNED_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
GPS_INFO_t GPSInfo = {0,0,0,0,0,0,0,0,0,0, INVALID};
volatile uint8_t GPSTimeout = 0;
80,36 → 80,39
void UpdateGPSInfo (void)
{
static uint32_t lasttime;
if (GpsSol.Status == VALID) // valid packet
if ((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA))
{
GPSInfo.satfix = GpsSol.GPSfix;
GPSInfo.satnum = GpsSol.numSV;
GPSInfo.PAcc = GpsSol.PAcc;
GPSInfo.VAcc = GpsSol.SAcc;
GpsSol.Status = PROCESSED; // never update old data
RED_FLASH;
if(GPSInfo.status != NEWDATA)
{
GPSInfo.status = INVALID;
// NAV SOL
GPSInfo.flags = UbxSol.Flags;
GPSInfo.satfix = UbxSol.GPSfix;
GPSInfo.satnum = UbxSol.numSV;
GPSInfo.PAcc = UbxSol.PAcc;
GPSInfo.VAcc = UbxSol.SAcc;
// NAV POSLLH
GPSInfo.longitude = UbxPosLlh.LON;
GPSInfo.latitude = UbxPosLlh.LAT;
GPSInfo.altitude = UbxPosLlh.HEIGHT;
GPSInfo.veleast = UbxVelNed.VEL_E;
GPSInfo.velnorth = UbxVelNed.VEL_N;
GPSInfo.veltop = -UbxVelNed.VEL_D;
GPSInfo.velground = UbxVelNed.GSpeed;
GPSInfo.status = NEWDATA;
}
// set state to collect new data
UbxSol.Status = PROCESSED; // never update old data
UbxPosLlh.Status = PROCESSED; // never update old data
UbxVelNed.Status = PROCESSED; // never update old data
}
if (GpsPosLlh.Status == VALID) // valid packet
{
GPSInfo.updatetime = GpsPosLlh.ITOW - lasttime;
lasttime = GpsPosLlh.ITOW;
GPSInfo.longitude = GpsPosLlh.LON;
GPSInfo.latitude = GpsPosLlh.LAT;
GPSInfo.altitude = GpsPosLlh.HEIGHT;
GpsPosLlh.Status = PROCESSED; // never update old data
}
if (GpsVelNed.Status == VALID) // valid packet
{
GPSInfo.veleast = GpsVelNed.VEL_E;
GPSInfo.velnorth = GpsVelNed.VEL_N;
GPSInfo.veltop = -GpsVelNed.VEL_D;
GPSInfo.velground = GpsVelNed.GSpeed;
GpsVelNed.Status = PROCESSED; // never update old data
}
if ((GpsSol.Status != INVALID) && (GpsPosLlh.Status != INVALID) && (GpsVelNed.Status != INVALID))
{
GPSInfo.status = VALID; // set valid if data are updated
}
}
142,21 → 145,21
switch(c)
{
case UBX_ID_POSLLH: // geodetic position
ubxP = (int8_t *)&GpsPosLlh; // data start pointer
ubxEp = (int8_t *)(&GpsPosLlh + 1); // data end pointer
ubxSp = (int8_t *)&GpsPosLlh.Status; // status pointer
ubxP = (int8_t *)&UbxPosLlh; // data start pointer
ubxEp = (int8_t *)(&UbxPosLlh + 1); // data end pointer
ubxSp = (int8_t *)&UbxPosLlh.Status; // status pointer
break;
case UBX_ID_SOL: // navigation solution
ubxP = (int8_t *)&GpsSol; // data start pointer
ubxEp = (int8_t *)(&GpsSol + 1); // data end pointer
ubxSp = (int8_t *)&GpsSol.Status; // status pointer
ubxP = (int8_t *)&UbxSol; // data start pointer
ubxEp = (int8_t *)(&UbxSol + 1); // data end pointer
ubxSp = (int8_t *)&UbxSol.Status; // status pointer
break;
case UBX_ID_VELNED: // velocity vector in tangent plane
ubxP = (int8_t *)&GpsVelNed; // data start pointer
ubxEp = (int8_t *)(&GpsVelNed + 1); // data end pointer
ubxSp = (int8_t *)&GpsVelNed.Status; // status pointer
ubxP = (int8_t *)&UbxVelNed; // data start pointer
ubxEp = (int8_t *)(&UbxVelNed + 1); // data end pointer
ubxSp = (int8_t *)&UbxVelNed.Status; // status pointer
break;
default: // unsupported identifier
184,7 → 187,7
ckb += cka;
// if the old data are not processed so far then break parsing now
// to avoid writing new data in ISR during reading by another function
if ( *ubxSp == VALID )
if ( *ubxSp == NEWDATA )
{
UpdateGPSInfo(); //update GPS info respectively
ubxstate = UBXSTATE_IDLE;
215,8 → 218,7
case UBXSTATE_CKB:
if (c == ckb)
{
*ubxSp = VALID; // new data are valid
ROT_FLASH;
*ubxSp = NEWDATA; // new data are valid
UpdateGPSInfo(); //update GPS info respectively
GPSTimeout = 255;
}

/branches/V0.70d Code Redesign killagreg/ubx.h
3,11 → 3,15
#include <inttypes.h>
#define INVALID 0x00
#define VALID 0x01
#define PROCESSED 0x02
typedef enum
{
INVALID,
NEWDATA,
PROCESSED
} Status_t;
// Satfix types for GPSData.satfix
#define SATFIX_NONE 0x00
#define SATFIX_DEADRECKOING 0x01
#define SATFIX_2D 0x02
14,6 → 18,11
#define SATFIX_3D 0x03
#define SATFIX_GPS_DEADRECKOING 0x04
#define SATFIX_TIMEONLY 0x05
// Flags for interpretation of the GPSData.flags
#define FLAG_GPSFIXOK 0x01 // (i.e. within DOP & ACC Masks)
#define FLAG_DIFFSOLN 0x02 // (is DGPS used)
#define FLAG_WKNSET 0x04 // (is Week Number valid)
#define FLAG_TOWSET 0x08 // (is Time of Week valid)
/* enable the UBX protocol at the gps receiver with the following messages enabled
23,7 → 32,7
typedef struct
{
uint8_t status; // status of data: invalid | valid
uint8_t flags; // flags
uint8_t satnum; // number of satelites
uint8_t satfix; // type of satfix
int32_t longitude; // in 1e-07 deg
35,7 → 44,7
int32_t veltop; // in cm/s
uint32_t velground; // 2D ground speed in cm/s
uint32_t VAcc; // in cm/s 3d velocity accuracy
uint32_t updatetime; // ms
Status_t status; // status of data: invalid | valid
} GPS_INFO_t;
//here you will find the current gps info

/branches/V0.70d Code Redesign killagreg/version.txt
132,12 → 132,33
V0.69k H.Buss 31.05.2008
- Bug in SPI.C behoben
- in 0.69h war ein Bug, der zu ungewollten Loopings führen konnte
V0.69L H.Buss 14.06.2008
- feinere Cam-Servo-Auflösung
V0.70a H.Buss 01.07.2008
- Unterstützung der V1.3-Hardware mit automatischem Hardware-Gyro-Abgleich
Anpassungen bzgl. V0.69k
G.Stobrawa 17.07.2008:
V0.70b H.Buss 14.07.2008
- flexible Einstellungsmöglichkeit von J16 und J17 (Transistorausgänge)
- eigene Parameter für GPS-Naviboard
- eigener Parameter für ExternalControl (war vorher UserParameter1 bzw. 8)
- neue Parameter im EEPROM-Datensatz: J16Bitmask, J16Timing, ExternalControl, Navi...
- MikroKopterFlags eingeführt, damit das Navi den Status des MKs kennt
- KopterTool-Kompatibilität auf 8 erhöht
V0.70c H.Buss 30.07.2008
- Parameter der Datenfusion leicht modifiziert
- EEPROM-Parameter für Looping-Umschlag angepasst (von 100 auf 85)
- MaxStick wird auf 100 begrenzt
V0.70d H.Buss 02.08.2008
- Transistorausgänge: das oberste Bit der Blinkmaske (im KopterTool linkes Bit) gibt nun den Zustand des Ausgangs im Schalterbetrieb an
Anpassungen bzgl. V0.70d
G.Stobrawa 02.08.2008:
- Code stärker modularisiert und restrukturiert
- viele Kommentare zur Erklärug eingefügt
- konsequent englische Variablennamen
144,18 → 165,16
- 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
- Makefile: EXT=NAVICTRL Unterstützung der SPI Communikation zum Naviboard
- Makefile: EXT nicht definiert Unerstützung des MK3MAG direct an FC und Conrad UBLOX Modul
- Makefile: EXT=MK3MAG Unerstützung des MK3MAG/CMPS03 direkt an FC und Conrad UBLOX Modul
- Makefile: EXT=KILLAGREG Unterstützung vom KillagregBoard mit MM3 und Conrad UBLOX Modul
- Ausertung des UBX-Pprotocols an 1. oder 2. Uart
- Ausertung des UBX-Protocols an 1. oder 2. Uart
- GPS-Hold-Funktion hinzugefügt
- GPS-Home-Funktion hinzugefügt (wird beim Motorstart gelernt, und bei Motorenstop wieder gelöscht)
- Poti3 steuert die GPS Funktionen (Poti3 < 70:GPS inaktiv, 70<=Poti3<160: GPS Hold, 160<=Poti3: GPS Home)
- LED Steuerung an J16, parametrierbar durch die User Parameter 7 & 8. UserParam7 legt die LEDOnTime in Vielfachen von 2ms fest
und UserParam 8 die LEDOffTime.
- Zusätzliche Punkte im Menü des KopterTool zur Anzeige des GPS-Status und der MM3-Kalibierparameter