| /beta/Code Redesign killagreg/eeprom.c |
|---|
| 120,9 → 120,9 |
| /***************************************************/ |
| /* Default Values for parameter set 1 */ |
| /* Common Default Values for parameter sets */ |
| /***************************************************/ |
| void ParamSet_DefaultSet1(void) // sport |
| void ParamSet_CommonDefaults(void) |
| { |
| ParamSet.Revision = EEPARAM_REVISION; |
| if(BoardRelease >= 20) |
| 141,6 → 141,7 |
| ParamSet.AngleTurnOverNick = 85; |
| ParamSet.AngleTurnOverRoll = 85; |
| } |
| ParamSet.Config0 = CFG0_AXIS_COUPLING_ACTIVE | CFG0_COMPASS_ACTIVE | CFG0_GPS_ACTIVE | CFG0_HEIGHT_SWITCH;//CFG0_HEIGHT_CONTROL | CFG0_COMPASS_FIX; |
| ParamSet.Config1 = 0; |
| ParamSet.Config2 = CFG2_HEIGHT_LIMIT | CFG2_VARIO_BEEP;//|CFG2_SENSITIVE_RC; |
| 153,22 → 154,13 |
| 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; |
| ParamSet.StickYawP = 12; |
| ParamSet.GasMin = 8; |
| ParamSet.GasMax = 230; |
| ParamSet.CompassYawEffect = 128; |
| ParamSet.GyroP = 80; |
| ParamSet.GyroI = 150; |
| ParamSet.GyroYawP = 80; |
| ParamSet.GyroYawI = 150; |
| ParamSet.GyroStability = 6; // Value : 1-8 |
| ParamSet.LowVoltageWarning = 33; // auto cell detection for values < 50 |
| ParamSet.EmergencyGas = 45; |
| ParamSet.EmergencyGasDuration = 90; |
| ParamSet.Receiver = RECEIVER_SPEKTRUM; |
| ParamSet.IFactor = 32; |
| ParamSet.UserParam1 = 0; |
| ParamSet.UserParam2 = 0; |
| ParamSet.UserParam3 = 0; |
| 178,15 → 170,17 |
| ParamSet.UserParam7 = 0; |
| ParamSet.UserParam8 = 0; |
| ParamSet.ServoCompInvert = 1; |
| ParamSet.ServoRefresh = 6; |
| ParamSet.ServoRefresh = 5; |
| ParamSet.ServoNickControl = 100; |
| ParamSet.ServoNickComp = 40; |
| ParamSet.ServoNickMin = 0; |
| ParamSet.ServoNickMax = 247; |
| ParamSet.ServoNickMin = 15; |
| ParamSet.ServoNickMax = 255; |
| ParamSet.ServoRollControl = 100; |
| ParamSet.ServoRollComp = 40; |
| ParamSet.ServoRollMin = 0; |
| ParamSet.ServoRollMax = 247; |
| ParamSet.ServoManualControlSpeed = 30; |
| ParamSet.CamOrientation = 0; |
| ParamSet.Servo3 = 125; |
| ParamSet.Servo4 = 125; |
| ParamSet.Servo5 = 125; |
| 193,17 → 187,12 |
| ParamSet.LoopGasLimit = 50; |
| ParamSet.LoopThreshold = 90; |
| ParamSet.LoopHysteresis = 50; |
| ParamSet.AxisCoupling1 = 90; |
| ParamSet.AxisCoupling2 = 80; |
| ParamSet.AxisCouplingYawCorrection = 1; |
| ParamSet.GyroAccTrim = 16; |
| ParamSet.DynamicStability = 100; |
| ParamSet.J16Bitmask = 95; |
| ParamSet.J17Bitmask = 243; |
| ParamSet.J16Bitmask_Warning = 0xAA; |
| ParamSet.J17Bitmask_Warning = 0xAA; |
| ParamSet.J16Timing = 15; |
| ParamSet.J17Timing = 15; |
| ParamSet.J16Timing = 20; |
| ParamSet.J17Timing = 20; |
| ParamSet.NaviGpsModeControl = 254; |
| ParamSet.NaviGpsGain = 100; |
| ParamSet.NaviGpsP = 90; |
| 222,6 → 211,30 |
| ParamSet.NaviPHLoginTime = 2; |
| ParamSet.OrientationAngle = 0; |
| ParamSet.OrientationModeControl = 0; |
| ParamSet.MotorSafetySwitch = 0; |
| } |
| /***************************************************/ |
| /* Default Values for parameter set 1 */ |
| /***************************************************/ |
| void ParamSet_DefaultSet1(void) // sport |
| { |
| ParamSet_CommonDefaults(); |
| ParamSet.StickP = 14; |
| ParamSet.StickD = 16; |
| ParamSet.StickYawP = 12; |
| ParamSet.GyroP = 80; |
| ParamSet.GyroI = 150; |
| ParamSet.GyroYawP = 80; |
| ParamSet.GyroYawI = 150; |
| ParamSet.GyroStability = 6; // Value : 1-8 |
| ParamSet.IFactor = 32; |
| ParamSet.AxisCoupling1 = 90; |
| ParamSet.AxisCoupling2 = 80; |
| ParamSet.AxisCouplingYawCorrection = 1; |
| ParamSet.GyroAccTrim = 16; |
| ParamSet.DynamicStability = 100; |
| memcpy(ParamSet.Name, "Sport\0",6); |
| ParamSet.crc = RAM_Checksum((uint8_t*)(&ParamSet), sizeof(ParamSet)-1); |
| } |
| 232,104 → 245,22 |
| /***************************************************/ |
| void ParamSet_DefaultSet2(void) // normal |
| { |
| ParamSet.Revision = EEPARAM_REVISION; |
| if(BoardRelease >= 20) |
| { |
| ParamSet.GyroD = 10; |
| ParamSet.DriftComp = 0; |
| ParamSet.GyroAccFactor = 27; |
| ParamSet.AngleTurnOverNick = 78; |
| ParamSet.AngleTurnOverRoll = 78; |
| } |
| else |
| { |
| ParamSet.GyroD = 3; |
| ParamSet.DriftComp = 32; |
| ParamSet.GyroAccFactor = 30; |
| ParamSet.AngleTurnOverNick = 85; |
| ParamSet.AngleTurnOverRoll = 85; |
| } |
| ParamSet.Config0 = CFG0_AXIS_COUPLING_ACTIVE | CFG0_COMPASS_ACTIVE | CFG0_GPS_ACTIVE | CFG0_HEIGHT_SWITCH;//CFG0_HEIGHT_CONTROL | CFG0_COMPASS_FIX; |
| ParamSet.Config1 = 0; |
| ParamSet.Config2 = CFG2_HEIGHT_LIMIT | CFG2_VARIO_BEEP;//|CFG2_SENSITIVE_RC; |
| ParamSet.HeightMinGas = 30; |
| ParamSet.MaxHeight = 255; |
| ParamSet.HeightP = 15; |
| ParamSet.HeightD = 30; |
| ParamSet.Height_ACC_Effect = 0; |
| ParamSet.Height_HoverBand = 8; |
| ParamSet.Height_GPS_Z = 64; |
| ParamSet.Height_StickNeutralPoint = 0; // Value : 0-250 (0 = Hoover-Estimation) |
| ParamSet.Height_Gain = 15; |
| ParamSet_CommonDefaults(); |
| ParamSet.StickP = 10; |
| ParamSet.StickD = 16; |
| ParamSet.StickYawP = 6; |
| ParamSet.GasMin = 8; |
| ParamSet.GasMax = 230; |
| ParamSet.CompassYawEffect = 128; |
| ParamSet.GyroP = 90; |
| ParamSet.GyroI = 120; |
| ParamSet.GyroYawP = 90; |
| ParamSet.GyroYawI = 120; |
| ParamSet.GyroStability = 6; // Value : 1-8 |
| ParamSet.LowVoltageWarning = 33; // auto cell detection for values < 50 |
| ParamSet.EmergencyGas = 45; |
| ParamSet.EmergencyGasDuration = 90; |
| ParamSet.Receiver = RECEIVER_SPEKTRUM; |
| ParamSet.IFactor = 32; |
| ParamSet.UserParam1 = 0; |
| ParamSet.UserParam2 = 0; |
| ParamSet.UserParam3 = 0; |
| ParamSet.UserParam4 = 0; |
| ParamSet.UserParam5 = 0; |
| ParamSet.UserParam6 = 0; |
| ParamSet.UserParam7 = 0; |
| ParamSet.UserParam8 = 0; |
| ParamSet.ServoCompInvert = 1; |
| ParamSet.ServoRefresh = 6; |
| ParamSet.ServoNickControl = 100; |
| ParamSet.ServoNickComp = 40; |
| ParamSet.ServoNickMin = 0; |
| ParamSet.ServoNickMax = 247; |
| ParamSet.ServoRollControl = 100; |
| ParamSet.ServoRollComp = 40; |
| ParamSet.ServoRollMin = 0; |
| ParamSet.ServoRollMax = 247; |
| ParamSet.Servo3 = 125; |
| ParamSet.Servo4 = 125; |
| ParamSet.Servo5 = 125; |
| ParamSet.LoopGasLimit = 50; |
| ParamSet.LoopThreshold = 90; |
| ParamSet.LoopHysteresis = 50; |
| ParamSet.AxisCoupling1 = 90; |
| ParamSet.AxisCoupling2 = 80; |
| ParamSet.AxisCouplingYawCorrection = 60; |
| ParamSet.GyroAccTrim = 32; |
| ParamSet.DynamicStability = 75; |
| ParamSet.J16Bitmask = 95; |
| ParamSet.J17Bitmask = 243; |
| ParamSet.J16Bitmask_Warning = 0xAA; |
| ParamSet.J17Bitmask_Warning = 0xAA; |
| ParamSet.J16Timing = 20; |
| ParamSet.J17Timing = 20; |
| ParamSet.NaviGpsModeControl = 254; |
| ParamSet.NaviGpsGain = 100; |
| ParamSet.NaviGpsP = 90; |
| ParamSet.NaviGpsI = 90; |
| ParamSet.NaviGpsD = 90; |
| ParamSet.NaviGpsPLimit = 75; |
| ParamSet.NaviGpsILimit = 75; |
| ParamSet.NaviGpsDLimit = 75; |
| ParamSet.NaviGpsACC = 0; |
| ParamSet.NaviGpsMinSat = 6; |
| ParamSet.NaviStickThreshold = 8; |
| ParamSet.NaviWindCorrection = 90; |
| ParamSet.NaviSpeedCompensation = 30; |
| ParamSet.NaviOperatingRadius = 100; |
| ParamSet.NaviAngleLimitation = 100; |
| ParamSet.NaviPHLoginTime = 2; |
| ParamSet.OrientationAngle = 0; |
| ParamSet.OrientationModeControl = 0; |
| memcpy(ParamSet.Name, "Normal\0", 7); |
| ParamSet.crc = RAM_Checksum((uint8_t*)(&ParamSet), sizeof(ParamSet)-1); |
| } |
| 340,104 → 271,22 |
| /***************************************************/ |
| void ParamSet_DefaultSet3(void) // beginner |
| { |
| ParamSet.Revision = EEPARAM_REVISION; |
| if(BoardRelease >= 20) |
| { |
| ParamSet.GyroD = 10; |
| ParamSet.DriftComp = 0; |
| ParamSet.GyroAccFactor = 27; // Value : 1-64 |
| ParamSet.AngleTurnOverNick = 78; |
| ParamSet.AngleTurnOverRoll = 78; |
| } |
| else |
| { |
| ParamSet.GyroD = 3; |
| ParamSet.DriftComp = 32; |
| ParamSet.GyroAccFactor = 30; // Value : 1-64 |
| ParamSet.AngleTurnOverNick = 85; |
| ParamSet.AngleTurnOverRoll = 85; |
| } |
| ParamSet.Config0 = CFG0_AXIS_COUPLING_ACTIVE | CFG0_COMPASS_ACTIVE | CFG0_GPS_ACTIVE | CFG0_HEIGHT_SWITCH;//CFG0_HEIGHT_CONTROL | CFG0_COMPASS_FIX | CFG0_ROTARY_RATE_LIMITER; |
| ParamSet.Config1 = 0; |
| ParamSet.Config2 = CFG2_HEIGHT_LIMIT | CFG2_VARIO_BEEP;//|CFG2_SENSITIVE_RC; |
| ParamSet.HeightMinGas = 30; |
| ParamSet.MaxHeight = 255; |
| ParamSet.HeightP = 15; |
| ParamSet.HeightD = 30; |
| ParamSet.Height_ACC_Effect = 0; |
| ParamSet.Height_HoverBand = 8; |
| ParamSet.Height_GPS_Z = 64; |
| ParamSet.Height_StickNeutralPoint = 0; // Value : 0-250 (0 = Hoover-Estimation) |
| ParamSet.Height_Gain = 15; |
| ParamSet_CommonDefaults(); |
| ParamSet.StickP = 8; |
| ParamSet.StickD = 16; |
| ParamSet.StickYawP = 6; |
| ParamSet.GasMin = 8; |
| ParamSet.GasMax = 230; |
| ParamSet.CompassYawEffect = 128; |
| ParamSet.GyroP = 100; |
| ParamSet.GyroI = 120; |
| ParamSet.GyroYawP = 100; |
| ParamSet.GyroYawI = 120; |
| ParamSet.GyroStability = 6; // Value : 1-8 |
| ParamSet.LowVoltageWarning = 33; // auto cell detection for values < 50 |
| ParamSet.EmergencyGas = 45; |
| ParamSet.EmergencyGasDuration = 90; |
| ParamSet.Receiver = RECEIVER_SPEKTRUM; |
| ParamSet.IFactor = 16; |
| ParamSet.UserParam1 = 0; |
| ParamSet.UserParam2 = 0; |
| ParamSet.UserParam3 = 0; |
| ParamSet.UserParam4 = 0; |
| ParamSet.UserParam5 = 0; |
| ParamSet.UserParam6 = 0; |
| ParamSet.UserParam7 = 0; |
| ParamSet.UserParam8 = 0; |
| ParamSet.ServoCompInvert = 1; |
| ParamSet.ServoRefresh = 6; |
| ParamSet.ServoNickControl = 100; |
| ParamSet.ServoNickComp = 40; |
| ParamSet.ServoNickMin = 0; |
| ParamSet.ServoNickMax = 247; |
| ParamSet.ServoRollControl = 100; |
| ParamSet.ServoRollComp = 40; |
| ParamSet.ServoRollMin = 0; |
| ParamSet.ServoRollMax = 247; |
| ParamSet.Servo3 = 125; |
| ParamSet.Servo4 = 125; |
| ParamSet.Servo5 = 125; |
| ParamSet.LoopGasLimit = 50; |
| ParamSet.LoopThreshold = 90; |
| ParamSet.LoopHysteresis = 50; |
| ParamSet.AxisCoupling1 = 90; |
| ParamSet.AxisCoupling2 = 80; |
| ParamSet.AxisCouplingYawCorrection = 70; |
| ParamSet.GyroAccTrim = 32; |
| ParamSet.DynamicStability = 70; |
| ParamSet.J16Bitmask = 95; |
| ParamSet.J17Bitmask = 243; |
| ParamSet.J16Bitmask_Warning = 0xAA; |
| ParamSet.J17Bitmask_Warning = 0xAA; |
| ParamSet.J16Timing = 30; |
| ParamSet.J17Timing = 30; |
| ParamSet.NaviGpsModeControl = 254; |
| ParamSet.NaviGpsGain = 100; |
| ParamSet.NaviGpsP = 90; |
| ParamSet.NaviGpsI = 90; |
| ParamSet.NaviGpsD = 90; |
| ParamSet.NaviGpsPLimit = 75; |
| ParamSet.NaviGpsILimit = 75; |
| ParamSet.NaviGpsDLimit = 75; |
| ParamSet.NaviGpsACC = 0; |
| ParamSet.NaviGpsMinSat = 6; |
| ParamSet.NaviStickThreshold = 8; |
| ParamSet.NaviWindCorrection = 90; |
| ParamSet.NaviSpeedCompensation = 30; |
| ParamSet.NaviOperatingRadius = 100; |
| ParamSet.NaviAngleLimitation = 100; |
| ParamSet.NaviPHLoginTime = 2; |
| ParamSet.OrientationAngle = 0; |
| ParamSet.OrientationModeControl = 0; |
| memcpy(ParamSet.Name, "Beginner\0", 9); |
| ParamSet.crc = RAM_Checksum((uint8_t*)(&ParamSet), sizeof(ParamSet)-1); |
| } |
| 629,6 → 478,45 |
| } |
| /***************************************************/ |
| /* Set default parameter set */ |
| /***************************************************/ |
| void SetDefaultParameter(uint8_t set, uint8_t restore_channels) |
| { |
| if(set > 5) set = 5; |
| else if(set < 1) set = 1; |
| switch(set) |
| { |
| case 1: |
| ParamSet_DefaultSet1(); // Fill ParamSet Structure to default parameter set 1 (Sport) |
| break; |
| case 2: |
| ParamSet_DefaultSet2(); // Kamera |
| break; |
| case 3: |
| ParamSet_DefaultSet3(); // Beginner |
| break; |
| default: |
| ParamSet_DefaultSet3(); // Beginner |
| break; |
| } |
| if(restore_channels) |
| { |
| uint8_t crc; |
| // 1st check for a valid channel backup in eeprom |
| crc = EEProm_Checksum(EEPROM_ADR_CHANNELS, sizeof(ParamSet.ChannelAssignment)); |
| if(crc == eeprom_read_byte((uint8_t*)(EEPROM_ADR_CHANNELS + sizeof(ParamSet.ChannelAssignment))) ) |
| { |
| eeprom_read_block((void *)ParamSet.ChannelAssignment, (void*)(EEPROM_ADR_CHANNELS), sizeof(ParamSet.ChannelAssignment)); |
| } |
| else ParamSet_DefaultStickMapping(); |
| } |
| else ParamSet_DefaultStickMapping(); |
| ParamSet_WriteToEEProm(set); |
| } |
| /***************************************************/ |
| /* Initialize EEPROM Parameter Sets */ |
| /***************************************************/ |
| void ParamSet_Init(void) |
| /beta/Code Redesign killagreg/eeprom.h |
|---|
| 5,7 → 5,7 |
| #include "twimaster.h" |
| #define EEPARAM_REVISION 84 // is count up, if paramater stucture has changed (compatibility) |
| #define EEPARAM_REVISION 85 // is count up, if paramater stucture has changed (compatibility) |
| #define EEMIXER_REVISION 1 // is count up, if mixer stucture has changed (compatibility) |
| #define EEBLCONFIG_REVISON 35 // is count up, if blconfig stucture has changed (compatibility to BLs!) |
| 151,7 → 151,9 |
| uint8_t ServoRollComp; // Value : 0-247 // Einfluss Roll-Gyro/Servo |
| uint8_t ServoRollMin; // Value : 0-247 // Anschlag |
| uint8_t ServoRollMax; // Value : 0-247 // Anschlag |
| uint8_t ServoRefresh; // Value: 0-247 // Refreshrate of servo pwm output |
| uint8_t ServoRefresh; // Value : 0-247 // Refreshrate of servo pwm output |
| uint8_t ServoManualControlSpeed; // |
| uint8_t CamOrientation; // |
| uint8_t Servo3; // Value: 0-247 servo value or mapping |
| uint8_t Servo4; // Value: 0-247 servo value or mapping |
| uint8_t Servo5; // Value: 0-247 servo value or mapping |
| 200,6 → 202,7 |
| //---CareFree--------------------------------------------- |
| uint8_t OrientationAngle; // Where is the front-direction in steps of 15°? |
| uint8_t OrientationModeControl; // switch for CareFre |
| uint8_t MotorSafetySwitch; |
| // config |
| uint8_t Config1; // see upper defines for bitcoding |
| uint8_t ServoCompInvert; // Bitfield: 0x01 = Nick invert, 0x02 = Roll invert // WICHTIG!!! am Ende lassen |
| 215,7 → 218,7 |
| extern uint8_t RAM_Checksum(uint8_t* pBuffer, uint16_t len); |
| extern void ParamSet_Init(void); |
| extern void SetDefaultParameter(uint8_t set, uint8_t restore_channels); |
| extern uint8_t ParamSet_ReadFromEEProm(uint8_t setnumber); |
| extern uint8_t ParamSet_WriteToEEProm(uint8_t setnumber); |
| /beta/Code Redesign killagreg/fc.c |
|---|
| 208,6 → 208,11 |
| DebugOut.Analog[31] = GPSStickRoll; |
| if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 0x01; |
| else DebugOut.Status[1] &= ~0x01; |
| if(Capacity.MinOfMaxPWM < 250/* && ModelIsFlying > 500*/) |
| { |
| BeepTime = 1000; |
| DebugOut.Analog[25]++; |
| } |
| } |
| /************************************************************************/ |
| 612,11 → 617,13 |
| } |
| if(twi_state != TWI_STATE_IDLE) |
| { |
| DebugOut.Analog[16]++; // transmission was not finished yet |
| twi_state = TWI_STATE_IDLE; |
| } |
| //Start I2C Interrupt Mode |
| motor_write = 0; |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| else |
| { |
| motor_write = 0; |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| } |
| } |
| 944,11 → 951,12 |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // gas stick is down |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| DebugOut.Analog[16] = ParamSet.MotorSafetySwitch; |
| if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < -85) |
| { |
| if(!(FCFlags&FCFLAG_MOTOR_RUN)) // motors are not running |
| { |
| if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
| if((PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) && ( (PPM_in[ParamSet.MotorSafetySwitch] < -75) || (ParamSet.MotorSafetySwitch == 0)) ) |
| { |
| // gas/yaw joystick is bottom right |
| // _________ |
| 986,7 → 994,7 |
| } |
| else // motors are running |
| { |
| if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
| if((PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) && ( (PPM_in[ParamSet.MotorSafetySwitch] < -75) || (ParamSet.MotorSafetySwitch == 0)) ) |
| { |
| // gas/yaw joystick is bottom left |
| // _________ |
| 1934,7 → 1942,7 |
| }// EOF ParamSet.Config0 & CFG0_AIRPRESS_SENSOR |
| // limit gas to parameter setting |
| LIMIT_MIN_MAX(GasMixFraction, (ParamSet.GasMin + 10) * STICK_GAIN, (ParamSet.GasMax - 20) * STICK_GAIN); |
| LIMIT_MIN_MAX(GasMixFraction, (int16_t)(ParamSet.GasMin + 10) * STICK_GAIN, (int16_t)(ParamSet.GasMax - 20) * STICK_GAIN); |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // all BL-Ctrl connected? |
| 1945,7 → 1953,7 |
| if( (ModelIsFlying > 1) && (ModelIsFlying < 50) && (GasMixFraction > 0) ) |
| { |
| ModelIsFlying = 1; // keep within lift off condition |
| GasMixFraction = (ParamSet.GasMin + 10) * STICK_GAIN; // reduce gas to min to avoid lift of |
| GasMixFraction = (int16_t)(ParamSet.GasMin + 10) * STICK_GAIN; // reduce gas to min to avoid lift of |
| } |
| } |
| 2023,7 → 2031,7 |
| if(tmp > MotorValue[i]) tmp = (MotorValue[i] + tmp) / 2; |
| else tmp = 2 * tmp - MotorValue[i]; |
| LIMIT_MIN_MAX(tmp, 4 * ParamSet.GasMin, 4 * ParamSet.GasMax); |
| LIMIT_MIN_MAX(tmp, 4 * (int16_t)ParamSet.GasMin, 4 * (int16_t)ParamSet.GasMax); |
| Motor[i].SetPoint = tmp / 4; |
| Motor[i].SetPointLowerBits = (tmp % 4)<<1; // remaining bits (3 bits total) |
| MotorValue[i] = tmp; |
| /beta/Code Redesign killagreg/main.c |
|---|
| 359,6 → 359,7 |
| // allow Serial Data Transmit if motors must not updated at this time |
| if( !UpdateMotor ) |
| { |
| Servo_Calculate(); |
| USART0_TransmitTxData(); |
| USART0_ProcessRxData(); |
| /beta/Code Redesign killagreg/makefile |
|---|
| 1,12 → 1,12 |
| #-------------------------------------------------------------------- |
| # MCU name |
| #MCU = atmega644p |
| MCU = atmega1284p |
| MCU = atmega644p |
| #MCU = atmega1284p |
| F_CPU = 20000000 |
| #------------------------------------------------------------------- |
| VERSION_MAJOR = 0 |
| VERSION_MINOR = 80 |
| VERSION_PATCH = 7 |
| VERSION_MINOR = 81 |
| VERSION_PATCH = 0 |
| VERSION_SERIAL_MAJOR = 11 # Serial Protocol Major Version |
| VERSION_SERIAL_MINOR = 0 # Serial Protocol Minor Version |
| /beta/Code Redesign killagreg/menu.c |
|---|
| 227,7 → 227,7 |
| case 12:// Servo Menu Item |
| LCD_printfxy(0,0,"Servo" ); |
| LCD_printfxy(0,1,"Setpoint %3i",FCParam.ServoNickControl); |
| LCD_printfxy(0,2,"Position: %3i",ServoNickValue); |
| LCD_printfxy(0,2,"Position: %3i",ServoNickValue/4); |
| LCD_printfxy(0,3,"Range:%3i-%3i",ParamSet.ServoNickMin, ParamSet.ServoNickMax); |
| break; |
| case 13://Extern Control |
| /beta/Code Redesign killagreg/timer2.c |
|---|
| 56,11 → 56,18 |
| #include "uart0.h" |
| #include "main.h" |
| #include "rc.h" |
| #include "mymath.h" |
| volatile int16_t ServoNickValue = 0; |
| volatile int16_t ServoRollValue = 0; |
| volatile uint8_t ServoActive = 0; |
| volatile uint8_t CalculateServoSignals = 1; |
| #define MULTIPLYER 4 |
| volatile uint16_t RemainingPulse = 0; |
| volatile int16_t ServoNickOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
| volatile int16_t ServoRollOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
| #define HEF4017R_ON PORTC |= (1<<PORTC6) |
| #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
| 135,7 → 142,79 |
| /*****************************************************/ |
| /* Control Servo Position */ |
| /*****************************************************/ |
| void Servo_Calculate(void) |
| { |
| int32_t cos, sin; |
| int16_t nick, roll, angle; |
| if(CalculateServoSignals == 0) return; |
| angle = ParamSet.OrientationAngle * 15; // to degree |
| sin = c_sin_8192(angle)/1024; // -8 ... 8 |
| cos = c_cos_8192(angle)/1024; // -8 ... 8 |
| if(CalculateServoSignals == 1) |
| { |
| nick = (cos * IntegralGyroNick) / 128L - (sin * IntegralGyroRoll) / 128L; |
| DebugOut.Analog[18] = nick; |
| nick = ((int32_t)ParamSet.ServoNickComp * nick) / 512L; |
| //DebugOut.Analog[18] = nick; |
| ServoNickOffset += ((int16_t)FCParam.ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / ParamSet.ServoManualControlSpeed; |
| ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765) |
| if(ParamSet.ServoCompInvert & 0x01) |
| { // inverting movement of servo |
| ServoNickValue += nick; |
| } |
| else |
| { // non inverting movement of servo |
| ServoNickValue -= nick; |
| } |
| // limit servo value to its parameter range definition |
| if(ServoNickValue < ((int16_t)ParamSet.ServoNickMin * MULTIPLYER) ) |
| { |
| ServoNickValue = (int16_t)ParamSet.ServoNickMin * MULTIPLYER; |
| } |
| else |
| if(ServoNickValue > ((int16_t)ParamSet.ServoNickMax * MULTIPLYER) ) |
| { |
| ServoNickValue = (int16_t)ParamSet.ServoNickMax * MULTIPLYER; |
| } |
| if(BoardRelease < 20) CalculateServoSignals = 0; // no roll servo output |
| else CalculateServoSignals++; // calc roll servo on next loop |
| } |
| else |
| { |
| roll = (cos * IntegralGyroRoll) / 128L + (sin * IntegralGyroNick) / 128L; |
| roll = ((int32_t)ParamSet.ServoRollComp * roll) / 512L; |
| ServoRollOffset += ((int16_t)FCParam.ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / ParamSet.ServoManualControlSpeed; |
| ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
| if(ParamSet.ServoCompInvert & 0x02) |
| { // inverting movement of servo |
| ServoRollValue += roll; |
| } |
| else |
| { // non inverting movement of servo |
| ServoRollValue -= roll; |
| } |
| // limit servo value to its parameter range definition |
| if(ServoRollValue < ((int16_t)ParamSet.ServoRollMin * MULTIPLYER) ) |
| { |
| ServoRollValue = (int16_t)ParamSet.ServoRollMin * MULTIPLYER; |
| } |
| else |
| if(ServoRollValue > ((int16_t)ParamSet.ServoRollMax * MULTIPLYER) ) |
| { |
| ServoRollValue = (int16_t)ParamSet.ServoRollMax * MULTIPLYER; |
| } |
| CalculateServoSignals = 0; // done |
| } |
| } |
| ISR(TIMER2_COMPA_vect) |
| { |
| 147,7 → 226,6 |
| #define IRS_RUNTIME 127 |
| #define PPM_STOPPULSE 188 |
| //#define PPM_FRAMELEN 14063 |
| #define PPM_FRAMELEN (1757 * ParamSet.ServoRefresh) // 22.5 ms / 8 Channels = 2.8125ms per Servo Channel |
| #define MINSERVOPULSE 375 |
| #define MAXSERVOPULSE 1500 |
| 154,13 → 232,9 |
| #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE) |
| static uint8_t PulseOutput = 0; |
| static uint16_t RemainingPulse = 0; |
| static uint16_t ServoFrameTime = 0; |
| static uint8_t ServoIndex = 0; |
| #define MULTIPLYER 4 |
| static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center position |
| static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center position |
| if(BoardRelease < 20) |
| { |
| 174,31 → 248,8 |
| TCCR2A &= ~(1<<COM2A0);// make a high pulse |
| RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
| ServoNickOffset = (ServoNickOffset * 3 + (int16_t)FCParam.ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
| ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
| if(ParamSet.ServoCompInvert & 0x01) |
| { // inverting movement of servo |
| ServoNickValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
| } |
| else |
| { // non inverting movement of servo |
| ServoNickValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
| } |
| // limit servo value to its parameter range definition |
| if(ServoNickValue < ((int16_t)ParamSet.ServoNickMin * MULTIPLYER) ) |
| { |
| ServoNickValue = (int16_t)ParamSet.ServoNickMin * MULTIPLYER; |
| } |
| else |
| if(ServoNickValue > ((int16_t)ParamSet.ServoNickMax * MULTIPLYER) ) |
| { |
| ServoNickValue = (int16_t)ParamSet.ServoNickMax * MULTIPLYER; |
| } |
| RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
| ServoNickValue /= MULTIPLYER; |
| // range servo pulse width |
| if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
| else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
| 209,6 → 260,7 |
| { |
| TCCR2A |= (1<<COM2A0); // make a low pulse |
| RemainingPulse = PPM_FRAMELEN - ServoFrameTime; |
| CalculateServoSignals = 1; |
| } |
| // set pulse output active |
| PulseOutput = 1; |
| 237,55 → 289,11 |
| switch(ServoIndex) // map servo channels |
| { |
| case 1: // Nick Compensation Servo |
| ServoNickOffset = (ServoNickOffset * 3 + (int16_t)FCParam.ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
| ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
| if(ParamSet.ServoCompInvert & 0x01) |
| { // inverting movement of servo |
| ServoNickValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
| } |
| else |
| { // non inverting movement of servo |
| ServoNickValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
| } |
| // limit servo value to its parameter range definition |
| if(ServoNickValue < ((int16_t)ParamSet.ServoNickMin * MULTIPLYER) ) |
| { |
| ServoNickValue = (int16_t)ParamSet.ServoNickMin * MULTIPLYER; |
| } |
| else |
| if(ServoNickValue > ((int16_t)ParamSet.ServoNickMax * MULTIPLYER) ) |
| { |
| ServoNickValue = (int16_t)ParamSet.ServoNickMax * MULTIPLYER; |
| } |
| RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
| ServoNickValue /= MULTIPLYER; |
| break; |
| case 2: // Roll Compensation Servo |
| ServoRollOffset = (ServoRollOffset * 3 + (int16_t)FCParam.ServoRollControl * MULTIPLYER) / 4; // lowpass offset |
| ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
| if(ParamSet.ServoCompInvert & 0x02) |
| { // inverting movement of servo |
| ServoRollValue += (int16_t)( ( (int32_t)ParamSet.ServoRollComp * MULTIPLYER * (IntegralGyroRoll / 128L ) ) / (256L) ); |
| } |
| else |
| { // non inverting movement of servo |
| ServoRollValue -= (int16_t)( ( (int32_t)ParamSet.ServoRollComp * MULTIPLYER * (IntegralGyroRoll / 128L ) ) / (256L) ); |
| } |
| // limit servo value to its parameter range definition |
| if(ServoRollValue < ((int16_t)ParamSet.ServoRollMin * MULTIPLYER) ) |
| { |
| ServoRollValue = (int16_t)ParamSet.ServoRollMin * MULTIPLYER; |
| } |
| else |
| if(ServoRollValue > ((int16_t)ParamSet.ServoRollMax * MULTIPLYER) ) |
| { |
| ServoRollValue = (int16_t)ParamSet.ServoRollMax * MULTIPLYER; |
| } |
| RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoRollValue to center position |
| ServoRollValue /= MULTIPLYER; |
| break; |
| case 3: |
| 323,7 → 331,11 |
| if(ServoActive && RC_Quality > 180) HEF4017R_OFF; // disable HEF4017 reset |
| else HEF4017R_ON; // enable reset |
| ServoIndex++; // change to next servo channel |
| if(ServoIndex > ParamSet.ServoRefresh) ServoIndex = 0; // reset to the sync gap |
| if(ServoIndex > ParamSet.ServoRefresh) |
| { |
| CalculateServoSignals = 1; |
| ServoIndex = 0; // reset to the sync gap |
| } |
| } |
| // set pulse output active |
| PulseOutput = 1; |
| /beta/Code Redesign killagreg/timer2.h |
|---|
| 9,6 → 9,7 |
| void TIMER2_Init(void); |
| void Servo_On(void); |
| void Servo_Off(void); |
| void Servo_Calculate(void); |
| #endif //_TIMER2_H |
| /beta/Code Redesign killagreg/uart0.c |
|---|
| 541,16 → 541,33 |
| break; |
| case 'q':// request settings |
| if(pRxData[0] == 0xFF) |
| Debug("Setting = %d", pRxData[0]); |
| if((10 <= pRxData[0]) && (pRxData[0] < 20)) |
| { |
| pRxData[0] = GetActiveParamSet(); |
| tempchar1 = pRxData[0] - 10; |
| if(tempchar1< 1) tempchar1 = 1; // limit to 1 |
| else if(tempchar1 > 5) tempchar1 = 5; // limit to 5 |
| SetDefaultParameter(tempchar1, 1); |
| } |
| // limit settings range |
| if(pRxData[0] < 1) pRxData[0] = 1; // limit to 1 |
| else if(pRxData[0] > 5) pRxData[0] = 5; // limit to 5 |
| // load requested parameter set |
| ParamSet_ReadFromEEProm(pRxData[0]); |
| tempchar1 = pRxData[0]; |
| else if((20 <= pRxData[0]) && (pRxData[0] < 30)) |
| { |
| tempchar1 = pRxData[0] - 20; |
| if(tempchar1< 1) tempchar1 = 1; // limit to 1 |
| else if(tempchar1 > 5) tempchar1 = 5; // limit to 5 |
| SetDefaultParameter(tempchar1, 0); |
| } |
| else |
| { |
| tempchar1 = pRxData[0]; |
| if(tempchar1 == 0xFF) |
| { |
| tempchar1 = GetActiveParamSet(); |
| } |
| if(tempchar1< 1) tempchar1 = 1; // limit to 1 |
| else if(tempchar1 > 5) tempchar1 = 5; // limit to 5 |
| // load requested parameter set |
| ParamSet_ReadFromEEProm(tempchar1); |
| } |
| while(!txd_complete); // wait for previous frame to be sent |
| SendOutData('Q', FC_ADDRESS,2, &tempchar1, sizeof(tempchar1), (uint8_t *) &ParamSet, sizeof(ParamSet) - 1); |
| Debug("Reading Setting %d", tempchar1) |
| /beta/Code Redesign killagreg/uart0.h |
|---|
| 4,8 → 4,8 |
| #include "printf_P.h" |
| // must be at least 4('#'+Addr+'CmdID'+'\r')+ (80 * 4)/3 = 111 bytes |
| #define TXD_BUFFER_LEN 160 |
| #define RXD_BUFFER_LEN 160 |
| #define TXD_BUFFER_LEN 170 |
| #define RXD_BUFFER_LEN 170 |
| #include <inttypes.h> |
| /beta/Code Redesign killagreg/version.txt |
|---|
| 416,10 → 416,14 |
| - bei I2C-Fehlern wurden die Counter zurück gesetzt und für einige ms die Interrupts angehalten - das ist jetzt behoben |
| - Nur I2C-Daten senden, wenn das alte Paket komplett raus ist |
| 0.81a |
| - MotorSafetySwitch - Verriegelt das Ein/Ausschalten |
| - ServoManualControlSpeed - Verlangsamt das Cam-Servo |
| - CamOrientation - für verdrehte Kamera-Servos |
| Anpassungen bzgl. V0.80h |
| G.Stobrawa 2.9.2010 |
| G.Stobrawa 12.10.2010 |
| - Code stärker modularisiert und restrukturiert |
| - viele Kommentare zur Erklärug eingefügt |