Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1660 → Rev 1661

/beta/Code Redesign killagreg/analog.c
305,7 → 305,7
gyroyaw += ADC; // get yaw gyro voltage 2nd sample
if(BoardRelease == 10) AdValueGyroYaw = (gyroyaw + 1) / 2; // analog gain on board 1.0 is 2 times higher
else
if(BoardRelease == 20) AdValueGyroYaw = 2047 - gyroyaw; // 2 times higher than a single sample
if(BoardRelease >= 20) AdValueGyroYaw = 2047 - gyroyaw; // 2 times higher than a single sample
else AdValueGyroYaw = gyroyaw; // 2 times higher than a single sample
ad_channel = AD_ACC_ROLL;
break;
/beta/Code Redesign killagreg/fc.c
131,7 → 131,9
uint16_t ModelIsFlying = 0;
uint8_t volatile FCFlags = 0;
int8_t VarioCharacter = ' ';
uint8_t HeadFree = 0;
 
 
int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L;
 
uint8_t GyroPFactor, GyroIFactor; // the PD factors for the attitude control
643,6 → 645,9
CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability);
CHK_POTI(FCParam.ExternalControl,ParamSet.ExternalControl);
Ki = (10300/2) / ( FCParam.IFactor + 1 );
if(Poti4 > 50) HeadFree = 1; else HeadFree = 0;
if(HeadFree) {if(FCParam.AxisCoupling1 < 210) FCParam.AxisCoupling1 += 30;}
 
}
}
 
682,12 → 687,13
int16_t PDPartNick, PDPartRoll, PDPartYaw, PPartNick, PPartRoll;
static int32_t IPartNick = 0, IPartRoll = 0;
 
static int32_t SetPointYaw = 0;
static int16_t ControlHeading = 0;
static int16_t SetPointYaw = 0; // yawing derived from yaw stick value
static int32_t IntegralGyroNickError = 0, IntegralGyroRollError = 0;
static int32_t CorrectionNick, CorrectionRoll;
static uint16_t RcLostTimer;
static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0;
static uint16_t UpdateCompassCourse = 0;
static uint8_t UpdateCompassCourse = 0;
static uint8_t Calibration_Done = 0;
// high resolution motor values for smoothing of PID motor outputs
static int16_t MotorValue[MAX_MOTORS];
925,6 → 931,7
ReadingIntegralGyroRoll2 = IntegralGyroRoll;
IPartNick = 0;
IPartRoll = 0;
ControlHeading = CompassHeading; // the stick reference
}
else
{
972,12 → 979,30
// calculate Stick inputs by rc channels (P) and changing of rc channels (D)
stick_nick = (stick_nick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.StickP) / 4;
stick_nick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.StickD;
StickNick = stick_nick - GPSStickNick;
 
stick_roll = (stick_roll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.StickP) / 4;
stick_roll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.StickD;
StickRoll = stick_roll - GPSStickRoll;
 
if(HeadFree) // start reference
{
int16_t angle, sin_h, cos_h;
 
angle = ((540 + ControlHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180;
sin_h = c_sin_8192(angle)/128; // -64 ... 64
cos_h = c_cos_8192(angle)/128; // -64 ... 64
// rotate sticks virtually to global reference
StickNick = ((cos_h * stick_nick) + (sin_h * stick_roll))/64;
StickRoll = ((cos_h * stick_roll) - (sin_h * stick_nick))/64;
}
else // body frame (according to head)
{
StickNick = stick_nick;
StickRoll = stick_roll;
}
 
StickNick -= GPSStickNick;
StickRoll -= GPSStickRoll;
 
// mapping of yaw
StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
#define YAW_DEAD_RANGE 2
1388,13 → 1413,13
w = abs(IntegralGyroNick / 512);
v = abs(IntegralGyroRoll / 512);
if(v > w) w = v;
correction = w / 8 + 1;
correction = w / 8 + 2;
// calculate the deviation of the yaw gyro heading and the compass heading
if (CompassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined
else error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180;
if(abs(GyroYaw) > 128) // spinning fast
else
{
error = 0;
error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180;
error += GyroYaw / 12;
}
if(!BadCompassHeading && w < 25)
{
1407,7 → 1432,7
UpdateCompassCourse = 0;
}
}
YawGyroHeading += (error * 8) / correction;
YawGyroHeading += (error * 16) / correction;
w = (w * FCParam.CompassYawEffect) / 32;
w = FCParam.CompassYawEffect - w;
if(w >= 0)
1909,6 → 1934,7
Motor[i].SetPoint = tmp / 8;
Motor[i].SetPointLowerBits = tmp % 8; // remaining bits
MotorValue[i] = tmp;
 
}
else
{
/beta/Code Redesign killagreg/libfc644.a
Cannot display: file marked as a binary type.
svn:mime-type = application/octet-stream
/beta/Code Redesign killagreg/main.c
86,6 → 86,9
uint8_t BoardRelease = 10;
// the board release is coded via the pull up or down the 2 status LED
 
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
BoardRelease = 21;
#else
PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate
DDRB &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input
 
112,6 → 115,7
DDRB |= (1<<DDB1)|(1<<DDB0);
RED_ON;
GRN_OFF;
#endif
return BoardRelease;
}
 
/beta/Code Redesign killagreg/main.h
6,8 → 6,8
//#define ACT_S3D_SUMMENSIGNAL
 
// neue Hardware
#define RED_OFF {if((BoardRelease == 10)||(BoardRelease == 20)) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);}
#define RED_ON {if((BoardRelease == 10)||(BoardRelease == 20)) PORTB |= (1<<PORTB0); else PORTB &=~(1<<PORTB0);}
#define RED_OFF {if((BoardRelease == 10)||(BoardRelease >= 20)) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);}
#define RED_ON {if((BoardRelease == 10)||(BoardRelease >= 20)) PORTB |= (1<<PORTB0); else PORTB &=~(1<<PORTB0);}
#define RED_FLASH PORTB ^= (1<<PORTB0)
#define GRN_OFF {if(BoardRelease < 12) PORTB &=~(1<<PORTB1); else PORTB |= (1<<PORTB1);}
#define GRN_ON {if(BoardRelease < 12) PORTB |= (1<<PORTB1); else PORTB &=~(1<<PORTB1);}
/beta/Code Redesign killagreg/menu.c
160,25 → 160,28
switch(BoardRelease)
{
case 10:
LCD_printfxy(0,1,"Nick %4i(%3i.%i)",AdValueGyroNick - BiasHiResGyroNick / HIRES_GYRO_AMPLIFY, BiasHiResGyroNick / HIRES_GYRO_AMPLIFY, BiasHiResGyroNick % HIRES_GYRO_AMPLIFY);
LCD_printfxy(0,2,"Roll %4i(%3i.%i)",AdValueGyroRoll - BiasHiResGyroRoll / HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll / HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll % HIRES_GYRO_AMPLIFY);
LCD_printfxy(0,3,"Yaw %4i(%3i)",AdBiasGyroYaw - AdValueGyroYaw , AdBiasGyroYaw);
break;
LCD_printfxy(0,1,"Nick %4i(%3i.%i)",AdValueGyroNick - BiasHiResGyroNick / HIRES_GYRO_AMPLIFY, BiasHiResGyroNick / HIRES_GYRO_AMPLIFY, BiasHiResGyroNick % HIRES_GYRO_AMPLIFY);
LCD_printfxy(0,2,"Roll %4i(%3i.%i)",AdValueGyroRoll - BiasHiResGyroRoll / HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll / HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll % HIRES_GYRO_AMPLIFY);
LCD_printfxy(0,3,"Yaw %4i(%3i)",AdBiasGyroYaw - AdValueGyroYaw , AdBiasGyroYaw);
break;
 
case 13:
// divice Offests by 2 because 2 samples are added in adc isr
LCD_printfxy(0,1,"Nick %4i (%3i.%i)(%3i)",AdValueGyroNick - BiasHiResGyroNick/HIRES_GYRO_AMPLIFY, BiasHiResGyroNick / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroNick % (HIRES_GYRO_AMPLIFY * 2))/2, DacOffsetGyroNick); // division by 2 to push the reminder below 10 (15/2 = 7)
LCD_printfxy(0,2,"Roll %4i (%3i.%i)(%3i)",AdValueGyroRoll - BiasHiResGyroRoll/HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroRoll % (HIRES_GYRO_AMPLIFY * 2))/2, DacOffsetGyroRoll); // division by 2 to push the reminder below 10 (15/2 = 7)
LCD_printfxy(0,3,"Yaw %4i (%3i)(%3i)",AdBiasGyroYaw - AdValueGyroYaw , AdBiasGyroYaw/2, DacOffsetGyroYaw );
break;
 
case 11:
case 12:
case 20: // divice Offests by 2 becuse 2 samples are added in adc isr
case 20:
case 21:
default:
// divice Offests by 2 becuse 2 samples are added in adc isr
LCD_printfxy(0,1,"Nick %4i(%3i.%i)",AdValueGyroNick - BiasHiResGyroNick/HIRES_GYRO_AMPLIFY, BiasHiResGyroNick / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroNick % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7)
LCD_printfxy(0,2,"Roll %4i(%3i.%i)",AdValueGyroRoll - BiasHiResGyroRoll/HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroRoll % (HIRES_GYRO_AMPLIFY * 2)) / 2); // division by 2 to push the reminder below 10 (15/2 = 7)
LCD_printfxy(0,3,"Yaw %4i(%3i)",AdBiasGyroYaw - AdValueGyroYaw , AdBiasGyroYaw/2);
break;
 
case 13:
default: // divice Offests by 2 becuse 2 samples are added in adc isr
LCD_printfxy(0,1,"Nick %4i (%3i.%i)(%3i)",AdValueGyroNick - BiasHiResGyroNick/HIRES_GYRO_AMPLIFY, BiasHiResGyroNick / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroNick % (HIRES_GYRO_AMPLIFY * 2))/2, DacOffsetGyroNick); // division by 2 to push the reminder below 10 (15/2 = 7)
LCD_printfxy(0,2,"Roll %4i (%3i.%i)(%3i)",AdValueGyroRoll - BiasHiResGyroRoll/HIRES_GYRO_AMPLIFY, BiasHiResGyroRoll / (HIRES_GYRO_AMPLIFY * 2), (BiasHiResGyroRoll % (HIRES_GYRO_AMPLIFY * 2))/2, DacOffsetGyroRoll); // division by 2 to push the reminder below 10 (15/2 = 7)
LCD_printfxy(0,3,"Yaw %4i (%3i)(%3i)",AdBiasGyroYaw - AdValueGyroYaw , AdBiasGyroYaw/2, DacOffsetGyroYaw );
break;
}
 
break;
/beta/Code Redesign killagreg/twimaster.c
71,8 → 71,14
volatile uint8_t BLFlags = 0;
 
MotorData_t Motor[MAX_MOTORS];
BLConfig_t BLConfig[MAX_MOTORS];
 
// bit mask for witch BL the configuration should be sent
volatile uint16_t BLConfig_WriteMask = 0;
// bit mask for witch BL the configuration should be read
volatile uint16_t BLConfig_ReadMask = 0;
// buffer for BL Configuration
BLConfig_t BLConfig;
 
#define I2C_WriteByte(byte) {TWDR = byte; TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);}
#define I2C_ReceiveByte() {TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);}
#define I2C_ReceiveLastByte() {TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);}
161,6 → 167,7
while((Mixer.Motor[motor_write][MIX_GAS] <= 0) && (motor_write < MAX_MOTORS)) motor_write++;
if(motor_write >= MAX_MOTORS) // writing finished, read now
{
BLConfig_WriteMask = 0; // reset configuration bitmask
motor_write = 0; // reset motor write counter for next cycle
twi_state = TWI_STATE_MOTOR_RX;
I2C_WriteByte(TWI_BASE_ADDRESS + TW_READ + (motor_read<<1) ); // select slave address in rx mode
175,23 → 182,30
twi_state = 4; //jump over sending more data
}
// the new version has been detected
else if(!( (Motor[motor_write].SetPointLowerBits && (RequiredMotors < 7)) || (BLFlags & (BLFLAG_SET_CONFIG|BLFLAG_GET_CONFIG)) ) )
else if(!( (Motor[motor_write].SetPointLowerBits && (RequiredMotors < 7)) || BLConfig_WriteMask || BLConfig_ReadMask ) )
{ // or LowerBits are zero and no BlConfig should be sent (saves round trip time)
twi_state = 4; //jump over sending more data
}
break;
case 2: // lower bits of setpoint (higher resolution)
if (BLFlags & BLFLAG_GET_CONFIG) BLReadMode = BL_READ_CONFIG;
else BLReadMode = BL_READ_STATUS;
I2C_WriteByte((BLReadMode<<3)|(Motor[motor_write].SetPointLowerBits & 0x07)); // send read mode and the lower bits of setpoint
// transmit config only on demand and the motors are not running and only for one motor per round trip
if( (BLFlags & BLFLAG_SET_CONFIG) && (motor_write == motor_read))
{ // prepare sending of configuration
pBuff = (uint8_t*)(&BLConfig[motor_write]); // select config for motor
if ((motor_write == motor_read) && ((0x0001<<motor_read) & BLConfig_ReadMask))
{
BLReadMode = BL_READ_CONFIG; // configuration request
}
else
{
BLReadMode = BL_READ_STATUS; // normal status request
}
// send read mode and the lower bits of setpoint
I2C_WriteByte((BLReadMode<<3)|(Motor[motor_write].SetPointLowerBits & 0x07));
// configuration tranmission request?
if((0x0001<<motor_read) & BLConfig_WriteMask)
{ // redirect tx pointer to configuration data
pBuff = (uint8_t*)&BLConfig; // select config for motor
BuffLen = sizeof(BLConfig_t);
}
else
{ // jump to state for end of transmission for that motor
{ // jump to end of transmission for that motor
twi_state = 4;
}
break;
219,8 → 233,7
if(++motor_read >= MAX_MOTORS)
{ // all motors read
motor_read = 0; // restart from beginning
BLFlags &= ~(BLFLAG_GET_CONFIG|BLFLAG_SET_CONFIG);
BLReadMode = BL_READ_STATUS;
BLConfig_ReadMask = 0; // reset read configuration bitmask
if(++motor_read_temperature >= MAX_MOTORS)
{
motor_read_temperature = 0;
240,7 → 253,7
switch(BLReadMode)
{
case BL_READ_CONFIG:
pBuff = (uint8_t*)&BLConfig[motor_read];
pBuff = (uint8_t*)&BLConfig;
BuffLen = sizeof(BLConfig_t);
break;
 
290,9 → 303,8
}
if(++motor_read >= MAX_MOTORS)
{
motor_read = 0; // restart from beginning
BLFlags &= ~(BLFLAG_GET_CONFIG|BLFLAG_SET_CONFIG);
BLReadMode = BL_READ_STATUS;
motor_read = 0; // restart from beginning
BLConfig_ReadMask = 0; // reset read configuration bitmask
if(++motor_read_temperature >= MAX_MOTORS)
{
motor_read_temperature = 0;
351,7 → 363,7
 
default:
I2C_Stop(TWI_STATE_MOTOR_TX);
BLFlags &= ~BLFLAG_TX_COMPLETE;
BLFlags |= BLFLAG_TX_COMPLETE;
I2CTimeout = 10;
motor_write = 0;
motor_read = 0;
361,46 → 373,80
}
 
 
uint8_t I2C_SetBLConfig(void)
uint8_t I2C_WriteBLConfig(uint8_t motor)
{
uint8_t i;
 
if(FCFlags & FCFLAG_MOTOR_RUN) return(0); // not when motors are running!
while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
if(FCFlags & FCFLAG_MOTOR_RUN) return(0); // not when motors are running!
if(motor > MAX_MOTORS) return (0); // motor does not exist!
if(!(Motor[motor].State & MOTOR_STATE_PRESENT_MASK)) return(0); // motor does not exist!
if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) return(0); // not a new BL!
 
// check BL configuration to send
if(BLConfig.Revision != BLCONFIG_REVISION) return (0); // bad revison
i = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1);
if(i != BLConfig.crc) return(0); // bad checksum
 
 
while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
 
// prepare the bitmask
if(!motor) // 0 means all
{
BLConfig_WriteMask = 0xFF; // all motors at once with the same configuration
}
else //only one specific motor
{
BLConfig_WriteMask = 0x0001<<(motor-1);
}
 
for(i = 0; i < MAX_MOTORS; i++)
{
Motor[i].SetPoint = 0;
Motor[i].SetPointLowerBits = 0;
if((0x0001<<i) & BLConfig_WriteMask)
{
Motor[i].SetPoint = 0;
Motor[i].SetPointLowerBits = 0;
}
}
motor_read = 0;
BLFlags |= BLFLAG_SET_CONFIG; // enable sending of BL config
motor_write = 0;
// needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms)
do
{
I2C_Start(TWI_STATE_MOTOR_TX); // start an i2c transmission
while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
}while(BLFlags & BLFLAG_SET_CONFIG); // repeat until the BL config has been send to all motors
}while(BLConfig_WriteMask); // repeat until the BL config has been sent
return(1);
}
 
uint8_t I2C_GetBLConfig(void)
uint8_t I2C_ReadBLConfig(uint8_t motor)
{
uint8_t i;
 
if(FCFlags & FCFLAG_MOTOR_RUN) return(0); // not when motors are running!
while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
if(FCFlags & FCFLAG_MOTOR_RUN) return(0); // not when motors are running!
if((motor == 0) || (motor > MAX_MOTORS)) return (0); // motor does not exist!
if(!(Motor[motor].State & MOTOR_STATE_PRESENT_MASK)) return(0); // motor does not exist!
if(!(Motor[motor].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) return(0); // not a new BL!
while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
// prepare the bitmask
BLConfig_ReadMask = 0x0001<<(motor-1);
for(i = 0; i < MAX_MOTORS; i++)
{
Motor[i].SetPoint = 0;
Motor[i].SetPointLowerBits = 0;
if((0x0001<<i) & BLConfig_ReadMask)
{
Motor[i].SetPoint = 0;
Motor[i].SetPointLowerBits = 0;
}
}
motor_read = 0;
BLFlags |= BLFLAG_GET_CONFIG; // enable request of BL config
// needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms)
do
{
I2C_Start(TWI_STATE_MOTOR_TX); // start an i2c transmission
while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
}while(BLFlags & BLFLAG_GET_CONFIG); // repeat until the BL config has been received from all motors
}while(BLConfig_ReadMask); // repeat until the BL config has been received from all motors
// validate result
if(BLConfig.Revision != BLCONFIG_REVISION) return (0); // bad revison
i = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1);
if(i != BLConfig.crc) return(0); // bad checksum
return(1);
}
/beta/Code Redesign killagreg/twimaster.h
23,8 → 23,6
 
#define BLFLAG_TX_COMPLETE 0x01
#define BLFLAG_READ_VERSION 0x02
#define BLFLAG_SET_CONFIG 0x04
#define BLFLAG_GET_CONFIG 0x08
 
extern volatile uint8_t BLFlags;
 
74,7 → 72,7
uint8_t crc; // checksum
} __attribute__((packed)) BLConfig_t;
 
extern BLConfig_t BLConfig[MAX_MOTORS];
extern BLConfig_t BLConfig;
 
extern volatile uint16_t I2CTimeout;
 
82,7 → 80,7
#define I2C_Start(start_state) {twi_state = start_state; BLFlags &= ~BLFLAG_TX_COMPLETE; TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);}
#define I2C_Stop(start_state) {twi_state = start_state; TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);}
void I2C_Reset(void); // Reset I2C
uint8_t I2C_SetBLConfig(void);
uint8_t I2C_GetBLConfig(void);
uint8_t I2C_WriteBLConfig(uint8_t motor);
uint8_t I2C_ReadBLConfig(uint8_t motor);
 
#endif
/beta/Code Redesign killagreg/uart0.c
498,7 → 498,7
#endif
 
case 't':// motor test
if(RxDataLen > 10) //
if(RxDataLen >= sizeof(MotorTest)) //
{
memcpy(&MotorTest[0], (uint8_t*)pRxData, sizeof(MotorTest));
}
607,51 → 607,29
case 'u': // request BL parameter
tempchar1 = pRxData[0];
Debug("Reading BL %d", tempchar1);
if( (tempchar1 > 0) && (tempchar1 <= MAX_MOTORS) )
{ // valid motor address
BLConfig[tempchar1-1].Revision = 0; // bad revision
BLConfig[tempchar1-1].crc = 0; // bad checksum
if(I2C_GetBLConfig())
{ // try to get the BL configs
if(BLConfig[tempchar1-1].crc == RAM_Checksum((uint8_t*)(&BLConfig[tempchar1-1]), sizeof(BLConfig_t)-1) )
{ // when crc is fine, send out
while(!txd_complete); // wait for previous frame to be sent
SendOutData('U', FC_ADDRESS, 2, tempchar1, 1, &BLConfig[tempchar1-1], sizeof(BLConfig_t));
}
}
BLConfig.Revision = 0; // bad revision
BLConfig.crc = 0; // bad checksum
// try to read BL configuration
tempchar1 = I2C_ReadBLConfig(pRxData[0]);
if(tempchar1)
{
while(!txd_complete); // wait for previous frame to be sent
SendOutData('U', FC_ADDRESS, 2, tempchar1, 1, &BLConfig, sizeof(BLConfig_t));
}
break;
 
case 'w': // write BL parameter
tempchar1 = 0;
Debug("Writing BL %d", pRxData[0]);
if(!(FCFlags & FCFLAG_MOTOR_RUN) && (pRxData[0] <= MAX_MOTORS)) // save parameter only if motors are off and a valid motor address was sent
if(RxDataLen >= 1+sizeof(BLConfig_t))
{
BLConfig_t *pBLConfig = (BLConfig_t*)(&pRxData[1]);
// version check
if(pBLConfig->Revision == BLCONFIG_REVISION)
{
uint8_t i, start, end;
if(pRxData[0] == 0) // all BLs!
{
start = 0;
end = MAX_MOTORS - 1;
}
else
{ // only one specific
start = pRxData[0] - 1;
end = start;
}
pBLConfig->crc = RAM_Checksum((uint8_t*)pBLConfig, sizeof(BLConfig_t) - 1); // update crc
for(i = start; i <= end; i++)
{
memcpy(&BLConfig[i], pBLConfig, sizeof(BLConfig_t));
}
tempchar1 = I2C_SetBLConfig();
}
memcpy(&BLConfig, (uint8_t*)(&pRxData[1]), sizeof(BLConfig_t));
// tbd. in MK-Tool
BLConfig.crc = RAM_Checksum((uint8_t*)&BLConfig, sizeof(BLConfig_t) - 1); // update crc
tempchar1 = I2C_WriteBLConfig(pRxData[0]);
while(!txd_complete); // wait for previous frame to be sent
SendOutData('W', FC_ADDRESS,1, &tempchar1, sizeof(tempchar1));
}
while(!txd_complete); // wait for previous frame to be sent
SendOutData('W', FC_ADDRESS,1, &tempchar1, sizeof(tempchar1));
 
break;
 
default:
/beta/Code Redesign killagreg/version.txt
381,11 → 381,18
- Einführung eines Vario-Zeichens (+/-/ ) auf der Jetibox
- BL-Timeout beim Start erhöht
 
0.79a G.Stobrawa 30.4.2010
0.79b H. Buss + G.Stobrawa 20.4.2010
- Motoren Starten nicht ohne Kalibrierung
- Checksummen gesicherte Dataenablage im EEProm
- Checksummen gesicherte Datenablage im EEProm
- statt 8 nun 11 Bit Auflösung der Lageregekung
- MotorSmooth()-Routine entfernt
- Unterstützung der BL2.0-Regler
 
0.79c H. Buss + G.Stobrawa 20.4.2010
- Unterstützung der BL-Ctrl 2.0 Konfiguration via MK-Tool
- Erste Version von "Head-Free"
 
 
Anpassungen bzgl. V0.78f
G.Stobrawa 23.3.2010