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