Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1322 → Rev 1323

/beta/Code Redesign killagreg/analog.c
73,6 → 73,7
volatile int16_t AdAirPressure = 1023;
volatile int32_t ReadingHeight = 0;
volatile int16_t ReadingVario = 0;
volatile int32_t SumHeight = 0;
volatile uint16_t MeasurementCounter = 0;
volatile uint8_t ADReady = 1;
 
220,7 → 221,6
static int32_t filtergyronick, filtergyroroll;
static int32_t tmpAirPressure = 0;
static uint8_t AirPressCount = 0;
static int32_t SumHeight = 0;
 
// state machine
switch(state++)
337,7 → 337,6
case 9:
AdAirPressure = ADC; // update meassured air pressure, changes -523 counts per OPA_OFFSET_STEP
tmpAirPressure += AdAirPressure;
#define SM_FILTER 16
if(++AirPressCount >= AIR_PRESSURE_SCALE)
{
#define AIRPRESSURE_FILTER_TIME 8
/beta/Code Redesign killagreg/analog.h
11,6 → 11,8
extern volatile int16_t FilterHiResGyroNick, FilterHiResGyroRoll;
extern volatile int16_t AdValueAccRoll, AdValueAccNick, AdValueAccTop, AdValueAccZ;
#define AIR_PRESSURE_SCALE 18 // 1 ADC counts corresponds approx. to 18 cm
#define SM_FILTER 16
extern volatile int32_t SumHeight; // filter for variometer
extern volatile int32_t AirPressure; // gets less with growing height
extern volatile int32_t StartAirPressure; // air pressure at ground
extern volatile int32_t ReadingHeight; // height according to air pressure (altimeter in steps of 1cm)
/beta/Code Redesign killagreg/eeprom.c
118,6 → 118,7
ParamSet.Height_ACC_Effect = 30;
ParamSet.Height_HooverBand = 5;
ParamSet.Height_GPS_Z = 64;
ParamSet.Height_StickNeutralPoint = 0; // Value : 0-250 (0 = Hoover-Estimation)
ParamSet.Height_Gain = 20;
ParamSet.StickP = 14;
ParamSet.StickD = 16;
127,6 → 128,8
ParamSet.CompassYawEffect = 128;
ParamSet.GyroP = 80;
ParamSet.GyroI = 150;
ParamSet.GyroYawP = 80;
ParamSet.GyroYawI = 150;
ParamSet.LowVoltageWarning = 33; // automatic cell detection for values < 50
ParamSet.EmergencyGas = 35;
ParamSet.EmergencyGasDuration = 30;
223,6 → 226,7
ParamSet.Height_ACC_Effect = 30;
ParamSet.Height_HooverBand = 5;
ParamSet.Height_GPS_Z = 64;
ParamSet.Height_StickNeutralPoint = 0; // Value : 0-250 (0 = Hoover-Estimation)
ParamSet.Height_Gain = 15;
ParamSet.StickP = 10;
ParamSet.StickD = 16;
232,6 → 236,8
ParamSet.CompassYawEffect = 128;
ParamSet.GyroP = 90;
ParamSet.GyroI = 120;
ParamSet.GyroYawP = 90;
ParamSet.GyroYawI = 120;
ParamSet.LowVoltageWarning = 33; // auto cell detection for values < 50
ParamSet.EmergencyGas = 35;
ParamSet.EmergencyGasDuration = 30;
298,7 → 304,7
{
ParamSet.GyroD = 5;
ParamSet.DriftComp = 0;
ParamSet.GyroAccFactor = 27; // Wert : 1-64
ParamSet.GyroAccFactor = 27; // Value : 1-64
ParamSet.AngleTurnOverNick = 78;
ParamSet.AngleTurnOverRoll = 78;
}
306,7 → 312,7
{
ParamSet.GyroD = 3;
ParamSet.DriftComp = 32;
ParamSet.GyroAccFactor = 30; // Wert : 1-64
ParamSet.GyroAccFactor = 30; // Value : 1-64
ParamSet.AngleTurnOverNick = 85;
ParamSet.AngleTurnOverRoll = 85;
}
328,6 → 334,7
ParamSet.Height_ACC_Effect = 30;
ParamSet.Height_HooverBand = 5;
ParamSet.Height_GPS_Z = 64;
ParamSet.Height_StickNeutralPoint = 0; // Value : 0-250 (0 = Hoover-Estimation)
ParamSet.Height_Gain = 15;
ParamSet.StickP = 8;
ParamSet.StickD = 16;
337,6 → 344,8
ParamSet.CompassYawEffect = 128;
ParamSet.GyroP = 100;
ParamSet.GyroI = 120;
ParamSet.GyroYawP = 100;
ParamSet.GyroYawI = 120;
ParamSet.LowVoltageWarning = 33; // auto cell detection for values < 50
ParamSet.EmergencyGas = 35;
ParamSet.EmergencyGasDuration = 20;
/beta/Code Redesign killagreg/eeprom.h
89,97 → 89,99
#define CH_POTI3 6
#define CH_POTI4 7
 
#define EEPARAM_REVISION 79 // is count up, if paramater stucture has changed (compatibility)
#define EEMIXER_REVISION 1 // is count up, if Mixer stucture has changed (compatibility)
#define EEPARAM_REVISION 80 // is count up, if paramater stucture has changed (compatibility)
#define EEMIXER_REVISION 1 // is count up, if mixer stucture has changed (compatibility)
 
// values above 250 representing poti1 to poti4
typedef struct
{
uint8_t ChannelAssignment[8]; // see upper defines for details
uint8_t Config0; // see upper defines for bitcoding
uint8_t HeightMinGas; // Wert : 0-100
uint8_t HeightD; // Wert : 0-250
uint8_t MaxHeight; // Wert : 0-32
uint8_t HeightP; // Wert : 0-32
uint8_t Height_Gain; // Wert : 0-50
uint8_t Height_ACC_Effect; // Wert : 0-250
uint8_t Height_HooverBand; // Wert : 0-250
uint8_t Height_GPS_Z; // Wert : 0-250
uint8_t StickP; // Wert : 1-6
uint8_t StickD; // Wert : 0-64
uint8_t StickYawP; // Wert : 1-20
uint8_t GasMin; // Wert : 0-32
uint8_t GasMax; // Wert : 33-250
uint8_t GyroAccFactor; // Wert : 1-64
uint8_t CompassYawEffect; // Wert : 0-32
uint8_t GyroP; // Wert : 10-250
uint8_t GyroI; // Wert : 0-250
uint8_t GyroD; // Wert : 0-250
uint8_t LowVoltageWarning; // Wert : 0-250
uint8_t EmergencyGas; // Wert : 0-250 //Gaswert bei Empängsverlust
uint8_t EmergencyGasDuration; // Wert : 0-250 // Zeitbis auf EmergencyGas geschaltet wird, wg. Rx-Problemen
uint8_t UfoArrangement; // x oder + Formation
uint8_t IFactor; // Wert : 0-250
uint8_t UserParam1; // Wert : 0-250
uint8_t UserParam2; // Wert : 0-250
uint8_t UserParam3; // Wert : 0-250
uint8_t UserParam4; // Wert : 0-250
uint8_t ServoNickControl; // Wert : 0-250 // Stellung des Nick Servos
uint8_t ServoNickComp; // Wert : 0-250 // Einfluss Nick-Gyro/Servo
uint8_t ServoNickMin; // Wert : 0-250 // Anschlag
uint8_t ServoNickMax; // Wert : 0-250 // Anschlag
uint8_t ServoRollControl; // Wert : 0-250 // Stellung des Roll Servos
uint8_t ServoRollComp; // Wert : 0-250 // Einfluss Roll-Gyro/Servo
uint8_t ServoRollMin; // Wert : 0-250 // Anschlag
uint8_t ServoRollMax; // Wert : 0-250 // Anschlag
uint8_t ServoRefresh; // Wert: 0-250 // Refreshrate of servo pwm output
uint8_t LoopGasLimit; // Wert: 0-250 max. Gas während Looping
uint8_t LoopThreshold; // Wert: 0-250 Schwelle für Stickausschlag
uint8_t LoopHysteresis; // Wert: 0-250 Hysterese für Stickausschlag
uint8_t ChannelAssignment[8]; // see upper defines for details
uint8_t Config0; // see upper defines for bitcoding
uint8_t HeightMinGas; // Value : 0-100
uint8_t HeightD; // Value : 0-250
uint8_t MaxHeight; // Value : 0-32
uint8_t HeightP; // Value : 0-32
uint8_t Height_Gain; // Value : 0-50
uint8_t Height_ACC_Effect; // Value : 0-250
uint8_t Height_HooverBand; // Value : 0-250
uint8_t Height_GPS_Z; // Value : 0-250
uint8_t Height_StickNeutralPoint; // Value : 0-250
uint8_t StickP; // Value : 1-6
uint8_t StickD; // Value : 0-64
uint8_t StickYawP; // Value : 1-20
uint8_t GasMin; // Value : 0-32
uint8_t GasMax; // Value : 33-250
uint8_t GyroAccFactor; // Value : 1-64
uint8_t CompassYawEffect; // Value : 0-32
uint8_t GyroP; // Value : 10-250
uint8_t GyroI; // Value : 0-250
uint8_t GyroD; // Value : 0-250
uint8_t GyroYawP; // Value : 10-250
uint8_t GyroYawI; // Value : 0-250
uint8_t LowVoltageWarning; // Value : 0-250
uint8_t EmergencyGas; // Value : 0-250 //Gaswert bei Empängsverlust
uint8_t EmergencyGasDuration; // Value : 0-250 // Zeitbis auf EmergencyGas geschaltet wird, wg. Rx-Problemen
uint8_t UfoArrangement; // x or + Formation
uint8_t IFactor; // Value : 0-250
uint8_t UserParam1; // Value : 0-250
uint8_t UserParam2; // Value : 0-250
uint8_t UserParam3; // Value : 0-250
uint8_t UserParam4; // Value : 0-250
uint8_t ServoNickControl; // Value : 0-250 // Stellung des Nick Servos
uint8_t ServoNickComp; // Value : 0-250 // Einfluss Nick-Gyro/Servo
uint8_t ServoNickMin; // Value : 0-250 // Anschlag
uint8_t ServoNickMax; // Value : 0-250 // Anschlag
uint8_t ServoRollControl; // Value : 0-250 // Stellung des Roll Servos
uint8_t ServoRollComp; // Value : 0-250 // Einfluss Roll-Gyro/Servo
uint8_t ServoRollMin; // Value : 0-250 // Anschlag
uint8_t ServoRollMax; // Value : 0-250 // Anschlag
uint8_t ServoRefresh; // Value: 0-250 // Refreshrate of servo pwm output
uint8_t LoopGasLimit; // Value: 0-250 max. Gas während Looping
uint8_t LoopThreshold; // Value: 0-250 Schwelle für Stickausschlag
uint8_t LoopHysteresis; // Value: 0-250 Hysterese für Stickausschlag
// Axis coupling
uint8_t AxisCoupling1; // Wert: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung)
uint8_t AxisCoupling2; // Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
uint8_t AxisCouplingYawCorrection;// Wert: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
uint8_t AngleTurnOverNick; // Wert: 0-250 180°-Punkt
uint8_t AngleTurnOverRoll; // Wert: 0-250 180°-Punkt
uint8_t GyroAccTrim; // 1/k (Koppel_ACC_Wirkung)
uint8_t DriftComp; // limit for gyrodrift compensation
uint8_t DynamicStability; // PID limit for Attitude controller
uint8_t UserParam5; // Wert : 0-250
uint8_t UserParam6; // Wert : 0-250
uint8_t UserParam7; // Wert : 0-250
uint8_t UserParam8; // Wert : 0-250´
uint8_t AxisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung)
uint8_t AxisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
uint8_t AxisCouplingYawCorrection; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden
uint8_t AngleTurnOverNick; // Value: 0-250 180°-Punkt
uint8_t AngleTurnOverRoll; // Value: 0-250 180°-Punkt
uint8_t GyroAccTrim; // 1/k (Koppel_ACC_Wirkung)
uint8_t DriftComp; // limit for gyrodrift compensation
uint8_t DynamicStability; // PID limit for Attitude controller
uint8_t UserParam5; // Value : 0-250
uint8_t UserParam6; // Value : 0-250
uint8_t UserParam7; // Value : 0-250
uint8_t UserParam8; // Value : 0-250´
// Output
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 J16Bitmask_Warning; // for the J16 Outout
uint8_t J17Bitmask_Warning; // for the J17 Outout
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 J16Bitmask_Warning; // for the J16 Outout
uint8_t J17Bitmask_Warning; // for the J17 Outout
// NaviCtrl
uint8_t NaviGpsModeControl; // Parameters for the Naviboard
uint8_t NaviGpsGain; // overall gain for GPS-PID controller
uint8_t NaviGpsP; // P gain for GPS-PID controller
uint8_t NaviGpsI; // I gain for GPS-PID controller
uint8_t NaviGpsD; // D gain for GPS-PID controller
uint8_t NaviGpsPLimit; // P limit for GPS-PID controller
uint8_t NaviGpsILimit; // I limit for GPS-PID controller
uint8_t NaviGpsDLimit; // D limit for GPS-PID controller
uint8_t NaviGpsACC; // ACC gain for GPS-PID controller
uint8_t NaviGpsMinSat; // number of sattelites neccesary for GPS functions
uint8_t NaviStickThreshold; // activation threshild for detection of manual stick movements
uint8_t NaviWindCorrection; // streng of wind course correction
uint8_t NaviSpeedCompensation; // D gain fefore position hold login
uint8_t NaviOperatingRadius; // Radius limit in m around start position for GPS flights
uint8_t NaviAngleLimitation; // limitation of attitude angle controlled by the gps algorithm
uint8_t NaviPHLoginTime; // position hold logintimeout
uint8_t NaviGpsModeControl; // Parameters for the Naviboard
uint8_t NaviGpsGain; // overall gain for GPS-PID controller
uint8_t NaviGpsP; // P gain for GPS-PID controller
uint8_t NaviGpsI; // I gain for GPS-PID controller
uint8_t NaviGpsD; // D gain for GPS-PID controller
uint8_t NaviGpsPLimit; // P limit for GPS-PID controller
uint8_t NaviGpsILimit; // I limit for GPS-PID controller
uint8_t NaviGpsDLimit; // D limit for GPS-PID controller
uint8_t NaviGpsACC; // ACC gain for GPS-PID controller
uint8_t NaviGpsMinSat; // number of sattelites neccesary for GPS functions
uint8_t NaviStickThreshold; // activation threshild for detection of manual stick movements
uint8_t NaviWindCorrection; // streng of wind course correction
uint8_t NaviSpeedCompensation; // D gain fefore position hold login
uint8_t NaviOperatingRadius; // Radius limit in m around start position for GPS flights
uint8_t NaviAngleLimitation; // limitation of attitude angle controlled by the gps algorithm
uint8_t NaviPHLoginTime; // position hold logintimeout
// extern control
uint8_t ExternalControl; // for serial control
uint8_t ExternalControl; // for serial control
// config
uint8_t Config1; // see upper defines for bitcoding
uint8_t ServoCompInvert; // Bitfield: 0x01 = Nick invert, 0x02 = Roll invert // WICHTIG!!! am Ende lassen
uint8_t Config2; // see upper defines for bitcoding
uint8_t Reserved[3];
uint8_t Config1; // see upper defines for bitcoding
uint8_t ServoCompInvert; // Bitfield: 0x01 = Nick invert, 0x02 = Roll invert // WICHTIG!!! am Ende lassen
uint8_t Config2; // see upper defines for bitcoding
int8_t Name[12];
} paramset_t;
 
/beta/Code Redesign killagreg/fc.c
47,7 → 47,8
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
162,7 → 163,7
uint8_t LoopingLeft = 0, LoopingRight = 0, LoopingDown = 0, LoopingTop = 0;
 
 
fc_param_t FCParam = {48,251,16,58,64,64,8,150,150,2,10,0,0,0,0,0,0,0,0,100,100,70,90,65,64,100,0,0,0};
fc_param_t FCParam = {48,251,16,58,64,64,8,150,150,150,150,2,10,0,0,0,0,0,0,0,0,100,100,70,90,65,64,100,0,0,0};
 
 
 
330,10 → 331,10
FCParam.KalmanMaxDrift = 0;
FCParam.KalmanMaxFusion = 32;
 
Poti1 = PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + RC_CHANNEL_OFFSET;
Poti2 = PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + RC_CHANNEL_OFFSET;
Poti3 = PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + RC_CHANNEL_OFFSET;
Poti4 = PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + RC_CHANNEL_OFFSET;
Poti1 = PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + RC_POTI_OFFSET;
Poti2 = PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + RC_POTI_OFFSET;
Poti3 = PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + RC_POTI_OFFSET;
Poti4 = PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + RC_POTI_OFFSET;
 
//Servo_On(); //enable servo output
RC_Quality = 100;
586,6 → 587,8
CHK_POTI_MM(FCParam.GyroP,ParamSet.GyroP,10,255);
CHK_POTI(FCParam.GyroI,ParamSet.GyroI);
CHK_POTI(FCParam.GyroD,ParamSet.GyroD);
CHK_POTI_MM(FCParam.GyroYawP,ParamSet.GyroYawP,10,255);
CHK_POTI(FCParam.GyroYawI,ParamSet.GyroYawI);
CHK_POTI(FCParam.IFactor,ParamSet.IFactor);
CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1);
CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2);
722,15 → 725,15
}
else MKFlags |= MKFLAG_FLY; // set fly flag
 
if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + RC_CHANNEL_OFFSET) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + RC_CHANNEL_OFFSET && Poti1) Poti1--;
if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + RC_CHANNEL_OFFSET) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + RC_CHANNEL_OFFSET && Poti2) Poti2--;
if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + RC_CHANNEL_OFFSET) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + RC_CHANNEL_OFFSET && Poti3) Poti3--;
if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + RC_CHANNEL_OFFSET) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + RC_CHANNEL_OFFSET && Poti4) Poti4--;
if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + RC_POTI_OFFSET) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + RC_POTI_OFFSET && Poti1) Poti1--;
if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + RC_POTI_OFFSET) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + RC_POTI_OFFSET && Poti2) Poti2--;
if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + RC_POTI_OFFSET) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + RC_POTI_OFFSET && Poti3) Poti3--;
if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + RC_POTI_OFFSET) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + RC_POTI_OFFSET && Poti4) Poti4--;
//PPM24-Extension
if(Poti5 < PPM_in[9] + RC_CHANNEL_OFFSET) Poti5++; else if(Poti5 > PPM_in[9] + RC_CHANNEL_OFFSET && Poti5) Poti5--;
if(Poti6 < PPM_in[10] + RC_CHANNEL_OFFSET) Poti6++; else if(Poti6 > PPM_in[10] + RC_CHANNEL_OFFSET && Poti6) Poti6--;
if(Poti7 < PPM_in[11] + RC_CHANNEL_OFFSET) Poti7++; else if(Poti7 > PPM_in[11] + RC_CHANNEL_OFFSET && Poti7) Poti7--;
if(Poti8 < PPM_in[12] + RC_CHANNEL_OFFSET) Poti8++; else if(Poti8 > PPM_in[12] + RC_CHANNEL_OFFSET && Poti8) Poti8--;
if(Poti5 < PPM_in[9] + RC_POTI_OFFSET) Poti5++; else if(Poti5 > PPM_in[9] + RC_POTI_OFFSET && Poti5) Poti5--;
if(Poti6 < PPM_in[10] + RC_POTI_OFFSET) Poti6++; else if(Poti6 > PPM_in[10] + RC_POTI_OFFSET && Poti6) Poti6--;
if(Poti7 < PPM_in[11] + RC_POTI_OFFSET) Poti7++; else if(Poti7 > PPM_in[11] + RC_POTI_OFFSET && Poti7) Poti7--;
if(Poti8 < PPM_in[12] + RC_POTI_OFFSET) Poti8++; else if(Poti8 > PPM_in[12] + RC_POTI_OFFSET && Poti8) Poti8--;
//limit poti values
#define POTI_MIN 0
#define POTI_MAX 255
948,13 → 951,13
}
 
// mapping of gas
StickGas = (StickGas + PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + RC_CHANNEL_OFFSET) / 2;// shift to positive numbers
StickGas = (StickGas + PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + RC_GAS_OFFSET) / 2;// shift to positive numbers
 
// update gyro control loop factors
GyroPFactor = FCParam.GyroP + 10;
GyroIFactor = FCParam.GyroI;
GyroYawPFactor = FCParam.GyroP + 10;
GyroYawIFactor = FCParam.GyroI;
GyroYawPFactor = FCParam.GyroYawP + 10;
GyroYawIFactor = FCParam.GyroYawI;
 
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1440,9 → 1443,7
DebugOut.Analog[10] = RC_Quality;
DebugOut.Analog[11] = YawGyroHeading / GYRO_DEG_FACTOR;
DebugOut.Analog[19] = CompassCalState;
DebugOut.Analog[22] = NCGpsZ;
DebugOut.Analog[27] = (int16_t)FCParam.KalmanMaxDrift;
// DebugOut.Analog[28] = (int16_t)FCParam.KalmanMaxFusion;
DebugOut.Analog[29] = NCSerialDataOkay;
DebugOut.Analog[30] = GPSStickNick;
DebugOut.Analog[31] = GPSStickRoll;
}
1506,8 → 1507,8
}
GasMixFraction *= STICK_GAIN; // scale GasMixFraction to enlarge resolution in the motor mixer
 
// if height control is activated
if(ParamSet.Config0 & CFG0_AIRPRESS_SENSOR)
// if airpressure sensor is enabled
if((ParamSet.Config0 & CFG0_AIRPRESS_SENSOR) && !(LoopingRoll || LoopingNick) )
{
#define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging
#define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging
1518,12 → 1519,17
static int16_t HeightTrimming = 0; // rate for change of height setpoint
static uint8_t HCActive = 0;
static int16_t FilterHCGas = 0;
static int16_t StickGasHoover = 110, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023;
static int16_t StickGasHoover = RC_GAS_OFFSET, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023;
static uint32_t HooverGasFilter = 0;
static uint8_t delay = 100;
 
#define BARO_LIMIT_MAX 0x01
#define BARO_LIMIT_MIN 0x02
static uint8_t BaroFlags = 0;
static uint8_t BaroExpandActive = 0;
 
// get the current hooverpoint
DebugOut.Analog[25] = HooverGas;
DebugOut.Analog[21] = HooverGas;
DebugOut.Analog[18] = ReadingVario;
 
// if height control is activated by an rc channel
1533,28 → 1539,10
{ //height control not active
if(!delay--)
{
// measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
if( (AdAirPressure > 1000) && (OCR0A < (255 - EXPANDBARO_OPA_OFFSET_STEP)) )
{ // increase offset
ExpandBaro -= 1;
OCR0A = PressureSensorOffset - EXPANDBARO_OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
BeepTime = 300;
delay = 250;
}
// measurement of air pressure close to lower limit and
else if( (AdAirPressure < 100) && (OCR0A > EXPANDBARO_OPA_OFFSET_STEP) )
{ // decrease offset
ExpandBaro += 1;
OCR0A = PressureSensorOffset - EXPANDBARO_OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
BeepTime = 300;
delay = 250;
}
else
{
SetPointHeight = ReadingHeight; // update SetPoint with current reading
HCActive = 0; // disable height control
delay = 1;
}
 
SetPointHeight = ReadingHeight; // update SetPoint with current reading
HCActive = 0; // disable height control
delay = 1;
}
}
else
1565,19 → 1553,69
}
else // no switchable height control
{ // the height control is always active and the set point is defined by the parameter
SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain;
if( !(BaroFlags & (BARO_LIMIT_MIN|BARO_LIMIT_MAX)) )
{
SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain;
}
HCActive = 1;
}
 
// --------- barometer range expansion ------------------
if(BaroExpandActive) // delay, because of expanding the Baro-Range
{
SumHeight = ReadingHeight * SM_FILTER; // reinit filter for vario
ReadingVario = 0;
// count down
BaroExpandActive--;
}
else // expansion not active
{
// measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
if(AdAirPressure > 1000)
{ // increase offset
if(OCR0A < (255 - EXPANDBARO_OPA_OFFSET_STEP))
{
ExpandBaro -= 1;
OCR0A = PressureSensorOffset - EXPANDBARO_OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
BeepTime = 300;
BaroExpandActive = 250;
}
else
{
BaroFlags |= BARO_LIMIT_MIN;
}
}
// measurement of air pressure close to lower limit and
else if(AdAirPressure < 100 )
{ // decrease offset
if(OCR0A > EXPANDBARO_OPA_OFFSET_STEP)
{
ExpandBaro += 1;
OCR0A = PressureSensorOffset - EXPANDBARO_OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
BeepTime = 300;
BaroExpandActive = 250;
}
else
{
BaroFlags |= BARO_LIMIT_MAX;
}
}
else
{ // still ok
BaroFlags &= ~(BARO_LIMIT_MIN | BARO_LIMIT_MAX);
}
}// EOF --------- barometer range expansion ------------------
 
 
// calculate cos of nick and roll angle used for projection of the vertical hoover gas
tmp_long = IntegralGyroNick/GYRO_DEG_FACTOR; // nick angle in deg
tmp_long *= tmp_long;
tmp_long2 = IntegralGyroRoll/GYRO_DEG_FACTOR; // roll angle in deg
tmp_long2 *= tmp_long2;
 
CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // phytagoras gives effective attitude angle in deg
LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude
 
if(HCActive && !(MKFlags & MKFLAG_EMERGENCY_LANDING))
{
if((ParamSet.Config2 & CFG2_HEIGHT_LIMIT) || !(ParamSet.Config0 & CFG0_HEIGHT_SWITCH))
1603,7 → 1641,7
 
if(MKFlags & MKFLAG_FLY) // trim setpoint only when flying
{ // gas stick is above hoover point
if(StickGas > (StickGasHoover + HC_STICKTHRESHOLD) )
if(StickGas > (StickGasHoover + HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MAX))
{
if(HeightTrimmingFlag & HC_TRIM_DOWN)
{
1613,7 → 1651,7
HeightTrimmingFlag |= HC_TRIM_UP;
HeightTrimming += abs(StickGas - (StickGasHoover + HC_STICKTHRESHOLD));
} // gas stick is below hoover point
else if(StickGas < (StickGasHoover - HC_STICKTHRESHOLD) )
else if(StickGas < (StickGasHoover - HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MIN))
{
if(HeightTrimmingFlag & HC_TRIM_UP)
{
1633,7 → 1671,7
if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 500;
}
}
// trim height set point
// trim height set point if needed
if(abs(HeightTrimming) > 512)
{
SetPointHeight += (HeightTrimming * ParamSet.Height_Gain)/(( 512 * 5 ) / 2); // move setpoint
1640,18 → 1678,22
HeightTrimming = 0;
if(ParamSet.Config2 & CFG2_VARIO_BEEP) BeepTime = 75;
//update hoover gas stick value when setpoint is shifted
StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
StickGasHoover = (StickGasHoover * UBat) / LowVoltageWarning;
LIMIT_MIN_MAX(StickGasHoover, 70, 150);
}
if(!ParamSet.Height_StickNeutralPoint)
{
StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
StickGasHoover = (StickGasHoover * UBat) / LowVoltageWarning;
LIMIT_MIN_MAX(StickGasHoover, 70, 150); // reserve some range for trim up and down
}
} // EOF trimming height set point
} //if MKFlags & MKFLAG_FLY
else // not flying but height control is already active
{
SetPointHeight = ReadingHeight - 200; // setpoint should be 2 meters below actual height to avoid a take off
StickGasHoover = 110;
SetPointHeight = ReadingHeight - 400; // setpoint should be 4 meters below actual height to avoid a take off
if(ParamSet.Height_StickNeutralPoint) StickGasHoover = ParamSet.Height_StickNeutralPoint;
else StickGasHoover = RC_GAS_OFFSET;
}
 
HCGas = HooverGas; // take hoover gas (neutral point)
HCGas = HooverGas; // take hoover gas (neutral point for PD controller)
 
} //EOF alternative height control
 
1658,33 → 1700,39
if((ReadingHeight > SetPointHeight) || !(ParamSet.Config2 & CFG2_HEIGHT_LIMIT) )
{
// from this point the Heigth Control Algorithm is identical for both versions
// ------------------------- P-Part ----------------------------
HeightDeviation = (int16_t)(ReadingHeight - SetPointHeight); // positive when too high
tmp_int = (HeightDeviation * (int16_t)FCParam.HeightP) / 16; // p-part
HCGas -= tmp_int;
// ------------------------- D-Part 1: Vario Meter ----------------------------
tmp_int = ReadingVario / 8;
if(tmp_int > 8) tmp_int = 8; // limit quadratic part on upward movement to avoid to much gas reduction
if(tmp_int > 0) tmp_int = ReadingVario + (tmp_int * tmp_int) / 4;
else tmp_int = ReadingVario - (tmp_int * tmp_int) / 4;
tmp_int = (FCParam.HeightD * (int32_t)(tmp_int)) / 128L; // scale to d-gain parameter
LIMIT_MIN_MAX(tmp_int, -127, 255);
HCGas -= tmp_int;
// ------------------------ D-Part 2: ACC-Z Integral ------------------------
tmp_int = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN);
DebugOut.Analog[23] = -tmp_int;
LIMIT_MIN_MAX(tmp_int, -127, 255);
HCGas -= tmp_int;
 
// limit deviation from hoover point within the target region
if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero
if(BaroExpandActive) // baro range expanding active
{
LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point
}
HCGas = HooverGas; // hoover while expanding baro adc range
} // EOF // baro range expanding active
else // no baro range expanding
{
// ------------------------- P-Part ----------------------------
HeightDeviation = (int16_t)(ReadingHeight - SetPointHeight); // positive when too high
tmp_int = (HeightDeviation * (int16_t)FCParam.HeightP) / 16; // p-part
HCGas -= tmp_int;
// ------------------------- D-Part 1: Vario Meter ----------------------------
tmp_int = ReadingVario / 8;
if(tmp_int > 8) tmp_int = 8; // limit quadratic part on upward movement to avoid to much gas reduction
if(tmp_int > 0) tmp_int = ReadingVario + (tmp_int * tmp_int) / 4;
else tmp_int = ReadingVario - (tmp_int * tmp_int) / 4;
tmp_int = (FCParam.HeightD * (int32_t)(tmp_int)) / 128L; // scale to d-gain parameter
LIMIT_MIN_MAX(tmp_int, -127, 255);
HCGas -= tmp_int;
// ------------------------ D-Part 2: ACC-Z Integral ------------------------
tmp_int = ((ReadingIntegralTop / 128) * (int32_t) FCParam.Height_ACC_Effect) / (128 / STICK_GAIN);
LIMIT_MIN_MAX(tmp_int, -127, 255);
HCGas -= tmp_int;
 
// limit deviation from hoover point within the target region
if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero
{
LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point
}
} // EOF no baro range expanding
 
// ------------------------ D-Part 3: GpsZ ----------------------------------
tmp_int = (ParamSet.Height_GPS_Z * (int32_t)NCGpsZ)/128L;
DebugOut.Analog[23] = -tmp_int;
LIMIT_MIN_MAX(tmp_int, -127, 255);
HCGas -= tmp_int;
 
// strech control output by inverse attitude projection 1/cos
1708,13 → 1756,20
}// EOF height control active
else // HC not active
{
//update hoover gas stick value when HC is not active
StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
StickGasHoover = (StickGasHoover * UBat) / LowVoltageWarning;
LIMIT_MIN_MAX(StickGasHoover, 70, 150);
}
// update hoover gas stick value when HC is not active
if(ParamSet.Height_StickNeutralPoint)
{
StickGasHoover = ParamSet.Height_StickNeutralPoint;
}
else // take real hoover stick position
{
StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
StickGasHoover = (StickGasHoover * UBat) / LowVoltageWarning;
}
LIMIT_MIN_MAX(StickGasHoover, 70, 150); // reserve some range for trim up and down
} // EOF HC not active
 
 
// ----------------- Hoover Gas Estimation --------------------------------
// Hoover gas estimation by averaging gas control output on small z-velocities
// this is done only if height contol option is selected in global config and aircraft is flying
if((MKFlags & MKFLAG_FLY) && !(MKFlags & MKFLAG_EMERGENCY_LANDING))
1760,7 → 1815,7
HooverGasMax = 1023;
}
} //EOF only on small vertical speed
}
}// EOF ----------------- Hoover Gas Estimation --------------------------------
}// EOF ParamSet.Config0 & CFG0_HEIGHT_CONTROL
 
// limit gas to parameter setting
/beta/Code Redesign killagreg/fc.h
26,7 → 26,8
#define GYRO_DEG_FACTOR ((int16_t)(ParamSet.GyroAccFactor) * ACC_DEG_FACTOR)
 
// shift for zero centered rc channel data to Poty and thrust values
#define RC_CHANNEL_OFFSET 120
#define RC_POTI_OFFSET 110
#define RC_GAS_OFFSET 120
 
extern uint8_t RequiredMotors;
 
41,6 → 42,8
uint8_t GyroD;
uint8_t GyroP;
uint8_t GyroI;
uint8_t GyroYawP;
uint8_t GyroYawI;
uint8_t StickYawP;
uint8_t IFactor;
uint8_t UserParam1;
/beta/Code Redesign killagreg/main.c
363,6 → 363,7
{
I2C_Reset();
I2CTimeout = 5;
DebugOut.Analog[28]++; // I2C-Error
}
if((BeepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN) )
{
/beta/Code Redesign killagreg/makefile
5,7 → 5,7
#-------------------------------------------------------------------
VERSION_MAJOR = 0
VERSION_MINOR = 75
VERSION_PATCH = 12
VERSION_PATCH = 13
 
VERSION_SERIAL_MAJOR = 10 # Serial Protocol Major Version
VERSION_SERIAL_MINOR = 1 # Serial Protocol Minor Version
/beta/Code Redesign killagreg/menu.c
161,7 → 161,7
LCD_printfxy(0,0,"Height: %5i",(int16_t)(ReadingHeight/5));
LCD_printfxy(0,1,"Set Point: %5i",(int16_t)(SetPointHeight/5));
LCD_printfxy(0,2,"Air Press.:%5i",AdAirPressure);
LCD_printfxy(0,3,"Offset :%5i",PressureSensorOffset);
LCD_printfxy(0,3,"Offset :%5i",OCR0A);
}
else
{
183,9 → 183,9
break;
case 4:// Remote Control Mapping Menu Item
LCD_printfxy(0,0,"Ni:%4i Ro:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_NICK]],PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]);
LCD_printfxy(0,1,"Gs:%4i Ya:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + RC_CHANNEL_OFFSET,PPM_in[ParamSet.ChannelAssignment[CH_YAW]]);
LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + RC_CHANNEL_OFFSET, PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + RC_CHANNEL_OFFSET);
LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + RC_CHANNEL_OFFSET, PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + RC_CHANNEL_OFFSET);
LCD_printfxy(0,1,"Gs:%4i Ya:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + RC_GAS_OFFSET,PPM_in[ParamSet.ChannelAssignment[CH_YAW]]);
LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + RC_GAS_OFFSET, PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + RC_POTI_OFFSET);
LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + RC_POTI_OFFSET, PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + RC_POTI_OFFSET);
break;
case 5:// Gyro Sensor Menu Item
LCD_printfxy(0,0,"Gyro - Sensor");
/beta/Code Redesign killagreg/spectrum.c
74,9 → 74,7
D9 to D0 is the channel data (10 bit) 0xaa..0x200..0x356 for 100% transmitter-travel
*/
 
 
 
#define MAX_FRAMEGAP 68 // 7ms
#define MIN_FRAMEGAP 68 // 7ms
#define MAX_BYTEGAP 3 // 375us
 
void spectrum_parser(uint8_t c)
91,7 → 89,7
{
// wait for beginning of new frame
ReSync = 0;
SpektrumTimer = MAX_FRAMEGAP;
SpektrumTimer = MIN_FRAMEGAP;
FrameCnt = 0;
Sync = 0;
ByteHigh = 0;
114,7 → 112,7
{
// Zeichen kam vor Ablauf der 7ms Sync-Pause
// warten auf erstes Sync-Zeichen
SpektrumTimer = MAX_FRAMEGAP;
SpektrumTimer = MIN_FRAMEGAP;
FrameCnt = 0;
Sync = 0;
ByteHigh = 0;
190,7 → 188,7
else
{
// hier stimmt was nicht: neu synchronisieren
SpektrumTimer = MAX_FRAMEGAP; // next frame expexted after 7ms
SpektrumTimer = MIN_FRAMEGAP; // next frame expexted after 7ms
ReSync = 1;
FrameCnt = 0;
Frame2 = 0;
206,7 → 204,7
// nur beim ersten Frame (CH 0-7) setzen
if(!ReSync) NewPpmData = 0;
}
SpektrumTimer = MAX_FRAMEGAP; // next frame expexted after 7ms
SpektrumTimer = MIN_FRAMEGAP;
FrameCnt = 0;
Frame2 = 0;
Sync = 0;
/beta/Code Redesign killagreg/spi.c
334,7 → 334,6
FCParam.KalmanMaxDrift = FromNaviCtrl.Param.sByte[2];
NCSerialDataOkay = FromNaviCtrl.Param.Byte[3];
NCGpsZ = FromNaviCtrl.Param.sByte[4];
DebugOut.Analog[29] = NCSerialDataOkay;
break;
 
default:
/beta/Code Redesign killagreg/uart0.c
148,14 → 148,14
"VarioMeter ",
"MK3MAG CalState ",
"NickServo ", //20
"RollServo ",
"GPS-Z ",
"ACC-D ",
"GPS-D ",
"Hoovergas ", //25
"MaxHoover ",
"Kalman Max Drift",
"MinHoover ",
"Hoovergas ",
" ",
" ",
" ",
" ", //25
" ",
" ",
"I2C-Error ",
"Navi Serial Data",
"GPS Nick ", //30
"GPS Roll "
/beta/Code Redesign killagreg/version.txt
320,11 → 320,16
- Spektrum-Timing wird nun überwacht
- die FC kann nun stand-Alone als Kamera-Stabilizer eingesetzt werden, weil die Servos aktiviert werden, wenn I2C fehlt
 
0.75n H.Buss 29.09.2009
- Hoover-Stickpunkt wird bei aktivem Höhenregler nicht nachgeführt
- Hoover-Stickpunkt kann per Parameter fest eingestellt werden
- Baro-Messbereichserweiterung auch bei aktiviertem Höhenregler
- Gyro_Gier_P und Gyro_Gier_I eingeführt
- I2C_fehler-Zähler jetzt in den Analogdaten
 
Anpassungen bzgl. V0.75n
G.Stobrawa 30.9.2009:
 
Anpassungen bzgl. V0.75m
G.Stobrawa 29.9.2009:
 
- Code stärker modularisiert und restrukturiert
- viele Kommentare zur Erklärug eingefügt
- konsequent englische Variablennamen