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