/branches/V0.68d Code Redesign killagreg/analog.c |
---|
23,7 → 23,7 |
volatile int16_t StartAirPressure; |
volatile uint16_t ReadingAirPressure = 1023; |
uint8_t PressureSensorOffset; |
volatile int16_t HightD = 0; |
volatile int16_t HeightD = 0; |
volatile uint16_t MeasurementCounter = 0; |
/*****************************************************/ |
171,9 → 171,9 |
if(++average_pressure >= 5) // if 5 values are summerized for averaging |
{ |
ReadingAirPressure = ADC; // update measured air pressure |
HightD = (int16_t)(StartAirPressure - tmpAirPressure - ReadingHight); // D-Anteil = neuerWert - AlterWert |
HeightD = (int16_t)(StartAirPressure - tmpAirPressure - ReadingHeight); // D-Anteil = neuerWert - AlterWert |
AirPressure = (tmpAirPressure + 3 * AirPressure) / 4; // averaging using history |
ReadingHight = StartAirPressure - AirPressure; |
ReadingHeight = StartAirPressure - AirPressure; |
average_pressure = 0; // reset air pressure measurement counter |
tmpAirPressure = 0; |
} |
/branches/V0.68d Code Redesign killagreg/analog.h |
---|
10,7 → 10,7 |
extern volatile int32_t AirPressure; |
extern volatile uint16_t MeasurementCounter; |
extern uint8_t PressureSensorOffset; |
extern volatile int16_t HightD; |
extern volatile int16_t HeightD; |
extern volatile uint16_t ReadingAirPressure; |
extern volatile int16_t StartAirPressure; |
/branches/V0.68d Code Redesign killagreg/eeprom.c |
---|
38,12 → 38,12 |
ParamSet.ChannelAssignment[CH_POTI3] = 7; |
ParamSet.ChannelAssignment[CH_POTI4] = 8; |
ParamSet.GlobalConfig = CFG_AXIS_COUPLING_ACTIVE;//CFG_HEIGHT_CONTROL | /*CFG_HEIGHT_SWITCH |*/ CFG_COMPASS_ACTIVE | CFG_COMPASS_FIX;//0x01; |
ParamSet.Hight_MinThrust = 30; |
ParamSet.MaxHight = 251; // Wert : 0-250 251 -> Poti1 |
ParamSet.Hight_P = 10; // Wert : 0-32 |
ParamSet.Hight_D = 30; // Wert : 0-250 |
ParamSet.Hight_ACC_Effect = 30; // Wert : 0-250 |
ParamSet.Hight_Gain = 4; // Wert : 0-50 |
ParamSet.Height_MinThrust = 30; |
ParamSet.MaxHeight = 251; // Wert : 0-250 251 -> Poti1 |
ParamSet.Height_P = 10; // Wert : 0-32 |
ParamSet.Height_D = 30; // Wert : 0-250 |
ParamSet.Height_ACC_Effect = 30; // Wert : 0-250 |
ParamSet.Height_Gain = 4; // Wert : 0-50 |
ParamSet.Stick_P = 4; //2 // Wert : 1-6 |
ParamSet.Stick_D = 8; //8 // Wert : 0-64 |
ParamSet.Yaw_P = 12; // Wert : 1-20 |
101,12 → 101,12 |
ParamSet.ChannelAssignment[CH_POTI3] = 7; |
ParamSet.ChannelAssignment[CH_POTI4] = 8; |
ParamSet.GlobalConfig = CFG_AXIS_COUPLING_ACTIVE;//CFG_HEIGHT_CONTROL | /*CFG_HEIGHT_SWITCH |*/ CFG_COMPASS_ACTIVE;//0x01; |
ParamSet.Hight_MinThrust = 30; |
ParamSet.MaxHight = 251; // Wert : 0-250 251 -> Poti1 |
ParamSet.Hight_P = 10; // Wert : 0-32 |
ParamSet.Hight_D = 30; // Wert : 0-250 |
ParamSet.Hight_ACC_Effect = 30; // Wert : 0-250 |
ParamSet.Hight_Gain = 3; // Wert : 0-50 |
ParamSet.Height_MinThrust = 30; |
ParamSet.MaxHeight = 251; // Wert : 0-250 251 -> Poti1 |
ParamSet.Height_P = 10; // Wert : 0-32 |
ParamSet.Height_D = 30; // Wert : 0-250 |
ParamSet.Height_ACC_Effect = 30; // Wert : 0-250 |
ParamSet.Height_Gain = 3; // Wert : 0-50 |
ParamSet.Stick_P = 3; //2 // Wert : 1-6 |
ParamSet.Stick_D = 4; //8 // Wert : 0-64 |
ParamSet.Yaw_P = 6; // Wert : 1-20 |
164,12 → 164,12 |
ParamSet.ChannelAssignment[CH_POTI3] = 7; |
ParamSet.ChannelAssignment[CH_POTI4] = 8; |
ParamSet.GlobalConfig = CFG_ROTARY_RATE_LIMITER | CFG_AXIS_COUPLING_ACTIVE;///*CFG_HEIGHT_SWITCH |*/ CFG_COMPASS_ACTIVE;//0x01; |
ParamSet.Hight_MinThrust = 30; |
ParamSet.MaxHight = 251; // Wert : 0-250 251 -> Poti1 |
ParamSet.Hight_P = 10; // Wert : 0-32 |
ParamSet.Hight_D = 30; // Wert : 0-250 |
ParamSet.Hight_ACC_Effect = 30; // Wert : 0-250 |
ParamSet.Hight_Gain = 2; // Wert : 0-50 |
ParamSet.Height_MinThrust = 30; |
ParamSet.MaxHeight = 251; // Wert : 0-250 251 -> Poti1 |
ParamSet.Height_P = 10; // Wert : 0-32 |
ParamSet.Height_D = 30; // Wert : 0-250 |
ParamSet.Height_ACC_Effect = 30; // Wert : 0-250 |
ParamSet.Height_Gain = 2; // Wert : 0-50 |
ParamSet.Stick_P = 2; //2 // Wert : 1-6 |
ParamSet.Stick_D = 4; //8 // Wert : 0-64 |
ParamSet.Yaw_P = 6; // Wert : 1-20 |
/branches/V0.68d Code Redesign killagreg/eeprom.h |
---|
54,12 → 54,12 |
{ |
uint8_t ChannelAssignment[8]; // see upper defines for details |
uint8_t GlobalConfig; // see upper defines for bitcoding |
uint8_t Hight_MinThrust; // Wert : 0-100 |
uint8_t Hight_D; // Wert : 0-250 |
uint8_t MaxHight; // Wert : 0-32 |
uint8_t Hight_P; // Wert : 0-32 |
uint8_t Hight_Gain; // Wert : 0-50 |
uint8_t Hight_ACC_Effect; // Wert : 0-250 |
uint8_t Height_MinThrust; // Wert : 0-100 |
uint8_t Height_D; // Wert : 0-250 |
uint8_t MaxHeight; // Wert : 0-32 |
uint8_t Height_P; // Wert : 0-32 |
uint8_t Height_Gain; // Wert : 0-50 |
uint8_t Height_ACC_Effect; // Wert : 0-250 |
uint8_t Stick_P; // Wert : 1-6 |
uint8_t Stick_D; // Wert : 0-64 |
uint8_t Yaw_P; // Wert : 1-20 |
/branches/V0.68d Code Redesign killagreg/fc.c |
---|
123,13 → 123,13 |
int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickThrust = 0; |
int16_t MaxStickPitch = 0, MaxStickRoll = 0, MaxStickYaw = 0; |
// stick values derived by uart inputs |
int16_t ExternStickPitch = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHightValue = -20; |
int16_t ExternStickPitch = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20; |
int16_t ReadingHight = 0; |
int16_t SetPointHight = 0; |
int16_t ReadingHeight = 0; |
int16_t SetPointHeight = 0; |
int16_t AttitudeCorrectionRoll = 0, AttitudeCorrectionPitch = 0; |
173,7 → 173,7 |
CalibMean(); |
Delay_ms_Mess(100); |
CalibMean(); |
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Hight Control activated? |
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Height Control activated? |
{ |
if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset(); |
} |
203,13 → 203,13 |
Reading_GyroRoll = 0; |
Reading_GyroYaw = 0; |
StartAirPressure = AirPressure; |
HightD = 0; |
HeightD = 0; |
Reading_Integral_Top = 0; |
CompassCourse = CompassHeading; |
BeepTime = 50; |
TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
ExternHightValue = 0; |
ExternHeightValue = 0; |
GPS_Neutral(); |
} |
409,10 → 409,10 |
{ |
#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.MaxHight,ParamSet.MaxHight,0,255); |
CHK_POTI(FCParam.Hight_D,ParamSet.Hight_D,0,100); |
CHK_POTI(FCParam.Hight_P,ParamSet.Hight_P,0,100); |
CHK_POTI(FCParam.Hight_ACC_Effect,ParamSet.Hight_ACC_Effect,0,255); |
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); |
449,8 → 449,8 |
static uint16_t RcLostTimer; |
static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0; |
static uint16_t Modell_Is_Flying = 0; |
static uint8_t HightControlActive = 0; |
static int16_t HightControlThrust = 0; |
static uint8_t HeightControlActive = 0; |
static int16_t HeightControlThrust = 0; |
static int8_t TimerDebugOut = 0; |
static int8_t StoreNewCompassCourse = 0; |
static int32_t CorrectionPitch, CorrectionRoll; |
681,8 → 681,8 |
if(DubWiseKeys[0] & 8) ExternStickYaw = 50;else |
if(DubWiseKeys[0] & 4) ExternStickYaw =-50;else ExternStickYaw = 0; |
if(DubWiseKeys[0] & 2) ExternHightValue++; |
if(DubWiseKeys[0] & 16) ExternHightValue--; |
if(DubWiseKeys[0] & 2) ExternHeightValue++; |
if(DubWiseKeys[0] & 16) ExternHeightValue--; |
StickPitch += ExternStickPitch / 8; |
StickRoll += ExternStickRoll / 8; |
697,7 → 697,7 |
StickPitch += (int16_t) ExternControl.Pitch * (int16_t) ParamSet.Stick_P; |
StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P; |
StickYaw += ExternControl.Yaw; |
ExternHightValue = (int16_t) ExternControl.Hight * (int16_t)ParamSet.Hight_Gain; |
ExternHeightValue = (int16_t) ExternControl.Height * (int16_t)ParamSet.Height_Gain; |
if(ExternControl.Thrust < StickThrust) StickThrust = ExternControl.Thrust; |
} |
// disable I part of gyro control feedback |
1078,7 → 1078,7 |
DebugOut.Analog[2] = Mean_AccPitch; |
DebugOut.Analog[3] = Mean_AccRoll; |
DebugOut.Analog[4] = Reading_GyroYaw; |
DebugOut.Analog[5] = ReadingHight; |
DebugOut.Analog[5] = ReadingHeight; |
DebugOut.Analog[6] = (Reading_Integral_Top / 512); |
DebugOut.Analog[8] = CompassHeading; |
DebugOut.Analog[9] = UBat; |
1098,7 → 1098,7 |
DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7]; |
DebugOut.Analog[9] = Reading_GyroPitch; |
DebugOut.Analog[9] = SetPointHight; |
DebugOut.Analog[9] = SetPointHeight; |
DebugOut.Analog[10] = Reading_IntegralGyroYaw / 128; |
DebugOut.Analog[11] = CompassCourse; |
DebugOut.Analog[10] = FCParam.Gyro_I; |
1105,8 → 1105,8 |
DebugOut.Analog[10] = ParamSet.Gyro_I; |
DebugOut.Analog[9] = CompassOffCourse; |
DebugOut.Analog[10] = ThrustMixFraction; |
DebugOut.Analog[3] = HightD * 32; |
DebugOut.Analog[4] = HightControlThrust; |
DebugOut.Analog[3] = HeightD * 32; |
DebugOut.Analog[4] = HeightControlThrust; |
*/ |
} |
1134,7 → 1134,7 |
if(Reading_GyroYaw < -MAX_SENSOR) Reading_GyroYaw = -MAX_SENSOR; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Hight Control |
// Height Control |
// The higth control algorithm reduces the thrust but does not increase the thrust. |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// If hight control is activated and no emergency landing is active |
1144,45 → 1144,45 |
// if hight control is activated by an rc channel |
if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH) |
{ // check if parameter is less than activation threshold |
if(FCParam.MaxHight < 50) |
if(FCParam.MaxHeight < 50) |
{ |
SetPointHight = ReadingHight - 20; // update SetPoint with current reading |
HightControlActive = 0; // disable hight control |
SetPointHeight = ReadingHeight - 20; // update SetPoint with current reading |
HeightControlActive = 0; // disable hight control |
} |
else HightControlActive = 1; // enable hight control |
else HeightControlActive = 1; // enable hight control |
} |
else // no switchable hight control |
{ |
SetPointHight = ((int16_t) ExternHightValue + (int16_t) FCParam.MaxHight) * (int16_t)ParamSet.Hight_Gain - 20; |
HightControlActive = 1; |
SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20; |
HeightControlActive = 1; |
} |
// get current hight |
h = ReadingHight; |
h = ReadingHeight; |
// if current hight is above the setpoint reduce thrust |
if((h > SetPointHight) && HightControlActive) |
if((h > SetPointHeight) && HeightControlActive) |
{ |
// hight difference -> P control part |
h = ((h - SetPointHight) * (int16_t) FCParam.Hight_P) / 16; |
h = ((h - SetPointHeight) * (int16_t) FCParam.Height_P) / 16; |
h = ThrustMixFraction - h; // reduce gas |
// higth gradient --> D control part |
h -= (HightD * FCParam.Hight_D) / 8; // D control part |
h -= (HeightD * FCParam.Height_D) / 8; // D control part |
// acceleration sensor effect |
tmp_int = ((Reading_Integral_Top / 512) * (int32_t) FCParam.Hight_ACC_Effect) / 32; |
tmp_int = ((Reading_Integral_Top / 512) * (int32_t) FCParam.Height_ACC_Effect) / 32; |
if(tmp_int > 50) tmp_int = 50; |
if(tmp_int < -50) tmp_int = -50; |
h -= tmp_int; |
// update hight control thrust |
HightControlThrust = (HightControlThrust*15 + h) / 16; |
HeightControlThrust = (HeightControlThrust*15 + h) / 16; |
// limit thrust reduction |
if(HightControlThrust < ParamSet.Hight_MinThrust) |
if(HeightControlThrust < ParamSet.Height_MinThrust) |
{ |
if(ThrustMixFraction >= ParamSet.Hight_MinThrust) HightControlThrust = ParamSet.Hight_MinThrust; |
if(ThrustMixFraction >= ParamSet.Height_MinThrust) HeightControlThrust = ParamSet.Height_MinThrust; |
// allows landing also if thrust stick is reduced below min thrust on hight control |
if(ThrustMixFraction < ParamSet.Hight_MinThrust) HightControlThrust = ThrustMixFraction; |
if(ThrustMixFraction < ParamSet.Height_MinThrust) HeightControlThrust = ThrustMixFraction; |
} |
// limit thrust to stick setting |
if(HightControlThrust > ThrustMixFraction) HightControlThrust = ThrustMixFraction; |
ThrustMixFraction = HightControlThrust; |
if(HeightControlThrust > ThrustMixFraction) HeightControlThrust = ThrustMixFraction; |
ThrustMixFraction = HeightControlThrust; |
} |
} |
// limit thrust to parameter setting |
/branches/V0.68d Code Redesign killagreg/fc.h |
---|
7,10 → 7,10 |
typedef struct |
{ |
uint8_t Hight_D; |
uint8_t MaxHight; |
uint8_t Hight_P; |
uint8_t Hight_ACC_Effect; |
uint8_t Height_D; |
uint8_t MaxHeight; |
uint8_t Height_P; |
uint8_t Height_ACC_Effect; |
uint8_t CompassYawEffect; |
uint8_t Gyro_P; |
uint8_t Gyro_I; |
53,8 → 53,8 |
extern volatile int16_t CompassOffCourse; |
// hight control |
extern int ReadingHight; |
extern int SetPointHight; |
extern int ReadingHeight; |
extern int SetPointHeight; |
// mean accelarations |
extern volatile int16_t Mean_AccPitch, Mean_AccRoll, Mean_AccTop; |
/branches/V0.68d Code Redesign killagreg/menu.c |
---|
96,11 → 96,11 |
LCD_printfxy(0,2,"Setting: %d ", GetActiveParamSet()); |
LCD_printfxy(0,3,"(c) Holger Buss"); |
break; |
case 1:// Hight Control Menu Item |
case 1:// Height Control Menu Item |
if(ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) |
{ |
LCD_printfxy(0,0,"Hight: %5i",ReadingHight); |
LCD_printfxy(0,1,"Set Point: %5i",SetPointHight); |
LCD_printfxy(0,0,"Height: %5i",ReadingHeight); |
LCD_printfxy(0,1,"Set Point: %5i",SetPointHeight); |
LCD_printfxy(0,2,"Air Press.: %5i",ReadingAirPressure); |
LCD_printfxy(0,3,"Offset : %5i",PressureSensorOffset); |
} |
107,7 → 107,7 |
else |
{ |
LCD_printfxy(0,1,"No "); |
LCD_printfxy(0,2,"Hight Control"); |
LCD_printfxy(0,2,"Height Control"); |
} |
break; |
148,7 → 148,7 |
LCD_printfxy(0,0,"ACC - Sensor"); |
LCD_printfxy(0,1,"Pitch %4i (%3i)",AdValueAccPitch, NeutralAccX); |
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueAccRoll, NeutralAccY); |
LCD_printfxy(0,3,"Hight %4i (%3i)",Mean_AccTop, (int)NeutralAccZ); |
LCD_printfxy(0,3,"Height %4i (%3i)",Mean_AccTop, (int)NeutralAccZ); |
break; |
case 7:// Accumulator Voltage / Remote Control Level |
LCD_printfxy(0,1,"Voltage: %5i",UBat); |
176,7 → 176,7 |
LCD_printfxy(0,0,"ExternControl " ); |
LCD_printfxy(0,1,"Pi:%4i Ro:%4i ",ExternControl.Pitch, ExternControl.Roll); |
LCD_printfxy(0,2,"Gs:%4i Ya:%4i ",ExternControl.Thrust, ExternControl.Yaw); |
LCD_printfxy(0,3,"Hi:%4i Cf:%4i ",ExternControl.Hight, ExternControl.Config); |
LCD_printfxy(0,3,"Hi:%4i Cf:%4i ",ExternControl.Height, ExternControl.Config); |
break; |
case 12://GPS Lat/Lon coords |
if (GPSInfo.status == INVALID) |
/branches/V0.68d Code Redesign killagreg/rc.c |
---|
15,8 → 15,8 |
#include "rc.h" |
#include "main.h" |
volatile int16_t PPM_in[11]; |
volatile int16_t PPM_diff[11]; |
volatile int16_t PPM_in[15]; //PPM24 supports 12 channels per frame |
volatile int16_t PPM_diff[15]; |
volatile uint8_t NewPpmData = 1; |
volatile uint8_t SenderOkay = 0; |
77,7 → 77,15 |
/********************************************************************/ |
/* Every time a positive edge is detected at PD6 */ |
/********************************************************************/ |
/* |
/* t-Frame |
<-----------------------------------------------------------------------> |
____ ______ _____ ________ ______ sync gap ____ |
| | | | | | | | | | | |
| | | | | | | | | | | |
___| |_| |_| |_| |_.............| |________________| |
<-----><-------><------><--------> <------> <--- |
t0 t1 t2 t4 tn t0 |
The PPM-Frame length is 22.5 ms. |
Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse. |
The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms. |
112,7 → 120,7 |
} |
else // within the PPM frame |
{ |
if(index < 10) // channel limit is 9 because of the frame length of 22.5 ms |
if(index < 14) // PPM24 supports 12 channels |
{ |
// check for valid signal length (0.8 ms < signal < 2.1984 ms) |
// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
/branches/V0.68d Code Redesign killagreg/rc.h |
---|
4,8 → 4,8 |
#include <inttypes.h> |
extern void RC_Init (void); |
extern volatile int16_t PPM_in[11]; // the RC-Signal |
extern volatile int16_t PPM_diff[11]; // the differentiated RC-Signal |
extern volatile int16_t PPM_in[15]; // the RC-Signal |
extern volatile int16_t PPM_diff[15]; // the differentiated RC-Signal |
extern volatile uint8_t NewPpmData; |
extern volatile uint8_t SenderOkay; // signal level indicator |
/branches/V0.68d Code Redesign killagreg/uart.c |
---|
56,7 → 56,7 |
"AccPitch ", |
"AccRoll ", |
"GyroYaw ", |
"ReadingHight ", //5 |
"ReadingHeight ", //5 |
"AccZ ", |
"Thrust ", |
"CompassHeading ", |
/branches/V0.68d Code Redesign killagreg/uart.h |
---|
42,7 → 42,7 |
int8_t Roll; |
int8_t Yaw; |
uint8_t Thrust; |
int8_t Hight; |
int8_t Height; |
uint8_t free; |
uint8_t Frame; |
uint8_t Config; |