Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1640 → Rev 1641

/beta/Code Redesign killagreg/analog.c
219,7 → 219,7
static int32_t filtergyronick, filtergyroroll;
static int32_t tmpAirPressure = 0;
static uint8_t AirPressCount = 0;
static int8_t Sub_AdBiasAccTop = 0;
static int16_t Sub_AdBiasAccTop = 0;
 
// state machine
switch(state++)
265,10 → 265,10
if(AdBiasAccTop < 750)
{
Sub_AdBiasAccTop += 2;
if(ModelIsFlying < 500) Sub_AdBiasAccTop += 10;
if(Sub_AdBiasAccTop > 100)
if(ModelIsFlying < 500) Sub_AdBiasAccTop += 100;
if(Sub_AdBiasAccTop > 1000)
{
Sub_AdBiasAccTop -= 100;
Sub_AdBiasAccTop -= 1000;
AdBiasAccTop++;
}
}
278,10 → 278,10
if(AdBiasAccTop > 550)
{
Sub_AdBiasAccTop -= 2;
if(ModelIsFlying < 500) Sub_AdBiasAccTop -= 10;
if(Sub_AdBiasAccTop < 100)
if(ModelIsFlying < 500) Sub_AdBiasAccTop -= 100;
if(Sub_AdBiasAccTop < 1000)
{
Sub_AdBiasAccTop += 100;
Sub_AdBiasAccTop += 1000;
AdBiasAccTop--;
}
 
/beta/Code Redesign killagreg/analog.h
18,8 → 18,16
extern volatile int32_t ReadingHeight; // height according to air pressure (altimeter in steps of 1cm)
extern volatile int16_t ReadingVario;
extern volatile int16_t AdAirPressure; // ADC value of air pressure measurement
 
 
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
#define EXPANDBARO_OPA_OFFSET_STEP 15
#else
#define EXPANDBARO_OPA_OFFSET_STEP 10
#endif
 
#define EXPANDBARO_ADC_SHIFT (-523) // adc shift per EXPANDBARO_OPA_OFFSET_STEP
 
extern uint8_t PressureSensorOffset;
extern int8_t ExpandBaro;
 
/beta/Code Redesign killagreg/fc.c
190,8 → 190,8
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = RC_Quality;
DebugOut.Analog[11] = YawGyroHeading / GYRO_DEG_FACTOR;
DebugOut.Analog[18] = ReadingVario;
DebugOut.Analog[19] = CompassCalState;
//DebugOut.Analog[18] = ReadingVario;
//DebugOut.Analog[19] = CompassCalState;
DebugOut.Analog[20] = ServoNickValue;
DebugOut.Analog[22] = Capacity.ActualCurrent;
DebugOut.Analog[23] = Capacity.UsedCapacity;
250,6 → 250,9
 
ExpandBaro = 0;
 
TransmitBlConfig = 1;
motor_read = 0;
 
// sample values with bias set to zero
Delay_ms_Mess(100);
 
306,7 → 309,7
AdBiasAccRoll = (int16_t)GetParamWord(PID_ACC_ROLL);
AdBiasAccTop = (int16_t)GetParamWord(PID_ACC_TOP);
 
if((AdBiasAccNick > 2048) || (AdBiasAccRoll > 2048) || (AdBiasAccTop > 1024))
if(((uint16_t)AdBiasAccNick > 2048) || ((uint16_t)AdBiasAccRoll > 2048) || ((uint16_t)AdBiasAccTop > 1024))
{
printf("\n\rACC not calibrated!\r\n");
AdBiasAccNick = 1024;
586,8 → 589,20
FCFlags &= ~(FCFLAG_FLY|FCFLAG_START); // clear flag FLY and START if motors are off
for(i = 0; i < MAX_MOTORS; i++)
{
if(!MotorTest_Active) Motor[i].SetPoint = 0;
else Motor[i].SetPoint = MotorTest[i];
if(!MotorTest_Active)
{
Motor[i].SetPoint = 0;
Motor[i].SetPointLowerBits = 0;
}
else
{
Motor[i].SetPoint = MotorTest[i];
Motor[i].SetPointLowerBits = 0;
/*
Motor[i].SetPoint = MotorTest[i] / 4;
Motor[i].SetPointLowerBits = MotorTest[i] % 4;
*/
}
}
if(MotorTest_Active) MotorTest_Active--;
}
1521,7 → 1536,6
// get the current hoverpoint
DebugOut.Analog[21] = HoverGas;
 
 
// --------- barometer range expansion ------------------
if(BaroExpandActive) // delay, because of expanding the Baro-Range
{
1894,11 → 1908,16
tmp += ((int32_t)RollMixFraction * Mixer.Motor[i][MIX_ROLL]) / 64L;
tmp += ((int32_t)YawMixFraction * Mixer.Motor[i][MIX_YAW] ) / 64L;
MotorValue[i] = MotorSmoothing(tmp, MotorValue[i]); // Spike Filter
tmp = MotorValue[i] / STICK_GAIN;
tmp = MotorValue[i] / 4;
LIMIT_MIN_MAX(tmp, ParamSet.GasMin, ParamSet.GasMax);
Motor[i].SetPoint = tmp;
Motor[i].SetPointLowerBits = MotorValue[i] % 4; // remaining bits
}
else Motor[i].SetPoint = 0;
else
{
Motor[i].SetPoint = 0;
Motor[i].SetPointLowerBits = 0;
}
}
}
 
/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
226,7 → 226,12
while(!UpdateMotor); // wait 2 ms
}
}
if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) printf("%d ",i+1);
if(Motor[i].State & MOTOR_STATE_PRESENT_MASK)
{
printf("%d ",i+1);
if(Motor[i].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) printf("(new) ");
else printf(" ");
}
}
for(i = 0; i < MAX_MOTORS; i++)
{
239,6 → 244,8
}
printf("\n\r===================================");
SendMotorData();
TransmitBlConfig = 1; // enable sending of BL config
motor_read = 0;
 
//wait for a short time (otherwise the RC channel check won't work below)
timer = SetDelay(500);
348,95 → 355,88
if(!BeepTime) FCFlags &= ~FCFLAG_I2C_ERR;
}
 
// allow Serial Data Transmit if motors must not updated or motors are not running
if( !UpdateMotor || !(FCFlags & FCFLAG_MOTOR_RUN) )
// allow Serial Data Transmit if motors must not updated at this time
if( !UpdateMotor )
{
USART0_TransmitTxData();
}
USART0_ProcessRxData();
USART0_ProcessRxData();
 
if(CheckDelay(timer))
{
static uint8_t counter_second = 0;
static uint16_t counter_minute = 0;
static uint8_t counter_debug = 0;
if(CheckDelay(timer))
{
static uint8_t counter_second = 0;
static uint16_t counter_minute = 0;
 
timer += 20; // every 20 ms
if(PcAccess) PcAccess--;
else
{
ExternControl.Config = 0;
ExternStickNick= 0;
ExternStickRoll = 0;
ExternStickYaw = 0;
if((BeepModulation == 0xFFFF) && (RC_Quality == 0))
timer += 20; // every 20 ms
if(PcAccess) PcAccess--;
else
{
BeepTime = 15000; // 1.5 seconds
BeepModulation = 0x0C00;
ExternControl.Config = 0;
ExternStickNick= 0;
ExternStickRoll = 0;
ExternStickYaw = 0;
if((BeepModulation == 0xFFFF) && (RC_Quality == 0))
{
BeepTime = 15000; // 1.5 seconds
BeepModulation = 0x0C00;
}
}
}
#ifdef USE_NAVICTRL
if(NCDataOkay)
{
NCDataOkay--;
if(!BeepTime) FCFlags &= ~FCFLAG_SPI_RX_ERR;
}
else // no data from NC
{ // set gps control sticks neutral
GPSStickNick = 0;
GPSStickNick = 0;
FCFlags |= FCFLAG_SPI_RX_ERR;
}
#endif
if(UBat < LowVoltageWarning)
{
FCFlags |= FCFLAG_LOWBAT;
BeepModulation = 0x0300;
if(!BeepTime )
#ifdef USE_NAVICTRL
if(NCDataOkay)
{
BeepTime = 6000; // 0.6 seconds
NCDataOkay--;
if(!BeepTime) FCFlags &= ~FCFLAG_SPI_RX_ERR;
}
}
else
{
if(!BeepTime) FCFlags &= ~FCFLAG_LOWBAT;
}
#ifdef USE_NAVICTRL
SPI_StartTransmitPacket();
SendSPI = 4;
#endif
else // no data from NC
{ // set gps control sticks neutral
GPSStickNick = 0;
GPSStickNick = 0;
FCFlags |= FCFLAG_SPI_RX_ERR;
}
#endif
if(UBat < LowVoltageWarning)
{
FCFlags |= FCFLAG_LOWBAT;
BeepModulation = 0x0300;
if(!BeepTime )
{
BeepTime = 6000; // 0.6 seconds
}
}
else
{
if(!BeepTime) FCFlags &= ~FCFLAG_LOWBAT;
}
#ifdef USE_NAVICTRL
SPI_StartTransmitPacket();
SendSPI = 4;
#endif
 
if(!(FCFlags & FCFLAG_MOTOR_RUN)) counter_minute = 1450; // 0.5 minutes round up
else
{
if(++counter_second == 49) // one second
if(!(FCFlags & FCFLAG_MOTOR_RUN)) counter_minute = 1450; // 0.5 minutes round up
else
{
counter_second = 0;
FlightSeconds++;
if(++counter_second == 49) // one second
{
counter_second = 0;
FlightSeconds++;
}
}
}
if(++counter_minute == 2930) // one minute
{
counter_minute = 0;
FlightMinutesTotal++;
FlightMinutes++;
SetParamWord(PID_FLIGHT_MINUTES_TOTAL, FlightMinutesTotal);
SetParamWord(PID_FLIGHT_MINUTES, FlightMinutes);
timer = SetDelay(20); // in case "timer += 20;" will not work
}
if(++counter_minute == 2930) // one minute
{
counter_minute = 0;
FlightMinutesTotal++;
FlightMinutes++;
SetParamWord(PID_FLIGHT_MINUTES_TOTAL, FlightMinutesTotal);
SetParamWord(PID_FLIGHT_MINUTES, FlightMinutes);
timer = SetDelay(20); // in case "timer += 20;" will not work
}
 
}// EOF CheckDelay(timer)
 
if((++counter_debug > 2) && (!UpdateMotor)) // 60 ms
{
counter_debug = 0;
UpdateDebugValues();
}
}// EOF CheckDelay(timer)
LED_Update();
Capacity_Update();
} // EOF !UpdateMotor
} // EOF Update Motor && ADReady
 
LED_Update();
Capacity_Update();
}
 
#ifdef USE_NAVICTRL
if(!SendSPI)
{ // SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
/beta/Code Redesign killagreg/main.h
3,6 → 3,7
 
#include <avr/io.h>
 
//#define ACT_S3D_SUMMENSIGNAL
 
// neue Hardware
#define RED_OFF {if((BoardRelease == 10)||(BoardRelease == 20)) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);}
/beta/Code Redesign killagreg/menu.c
63,7 → 63,7
#include "capacity.h"
#include "menu.h"
 
uint8_t MaxMenuItem = 16;
uint8_t MaxMenuItem = 17;
uint8_t MenuItem = 0;
 
 
245,7 → 245,7
for(i = 0; i < 2; i++)
{
j=i<<2;
LCD_printfxy(0,i+1," %c %c %c %c",'-' + 4 * (Motor[j].State>>7),'-' + 5 * (Motor[1+j].State>>7),'-' + 6 * (Motor[2+j].State>>7),'-' + 7 * (Motor[3+j].State>>7));
LCD_printfxy(0,i+1," %c %c %c %c",'-' + 4 * (Motor[j].State>>7),'-' + 5 * (Motor[j+1].State>>7),'-' + 6 * (Motor[j+2].State>>7),'-' + 7 * (Motor[j+3].State>>7));
}
LCD_printfxy(0,3," %c - - -",'-' + 12 * (Motor[8].State>>7));
if(Motor[9].State>>7) LCD_printfxy(4,3,"10");
253,7 → 253,16
if(Motor[11].State>>7) LCD_printfxy(12,3,"12");
break;
 
case 16:// flight time counter
case 16:// new BL infos
LCD_printfxy(0,0,"BL Temperature" );
for(i = 0; i < 3; i++)
{
j=i<<2;
LCD_printfxy(0,i+1,"%3i %3i %3i %3i ",Motor[j].Temperature, Motor[j+1].Temperature, Motor[j+2].Temperature, Motor[j+3].Temperature);
}
break;
 
case 17:// flight time counter
LCD_printfxy(0,0,"Flight-Time");
LCD_printfxy(0,1,"Total:%5u min",FlightMinutesTotal);
LCD_printfxy(0,2,"Trip: %5u min",FlightMinutes);
/beta/Code Redesign killagreg/rc.c
58,7 → 58,7
#include "fc.h"
 
 
//#define ACT_S3D_SUMSIGNAL
//#define ACT_S3D_SUMSIGNAL is set in main.h
 
volatile uint8_t RC_Channels; // number of received remote control channels
volatile int16_t PPM_in[MAX_CHANNELS];
251,10 → 251,13
{
RED_ON;
}
// demux sum signal for channels 5 to 7 to J3, J4, J5
if(index == 5) J3HIGH; else J3LOW;
if(index == 6) J4HIGH; else J4LOW;
//if(index == 7) J5HIGH; else J5LOW; //used as TXD1
if(BoardRelease < 20)
{
// demux sum signal for channels 5 to 7 to J3, J4, J5
if(index == 5) J3HIGH; else J3LOW;
if(index == 6) J4HIGH; else J4LOW;
//if(index == 7) J5HIGH; else J5LOW; //used as TXD1
}
index++; // next channel
}
else // (index >= MAX_RC_CHANNELS)
313,11 → 316,14
else PPM_diff[index] = 0;
PPM_in[index] = tmp; // update channel value
}
if(BoardRelease < 20)
{
// demux sum signal for channels 5 to 7 to J3, J4, J5
if(index == 5) J3HIGH; else J3LOW;
if(index == 6) J4HIGH; else J4LOW;
//if(index == 7) J5HIGH; else J5LOW; //used as TXD1
}
index++; // next channel
// demux sum signal for channels 5 to 7 to J3, J4, J5
if(index == 5) J3HIGH; else J3LOW;
if(index == 6) J4HIGH; else J4LOW;
//if(index == 7) J5HIGH; else J5LOW; //used as TXD1
} // eof (index < MAX_CHANNELS)
} // eof within the PPM frame
} // eof old more tolerant version
/beta/Code Redesign killagreg/timer0.c
124,7 → 124,7
 
// initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4
OCR0A = 0; // for PB3
OCR0B = 120; // for PB4
OCR0B = 180; // for PB4
 
// init Timer/Counter 0 Register
TCNT0 = 0;
/beta/Code Redesign killagreg/twimaster.c
62,6 → 62,7
volatile uint8_t dac_channel = 0;
volatile uint8_t motor_write = 0;
volatile uint8_t motor_read = 0;
volatile uint8_t TransmitBlConfig = 0;
 
volatile uint16_t I2CTimeout = 100;
 
201,6 → 202,7
/****************************************/
/* I2C ISR */
/****************************************/
/*
ISR (TWI_vect)
{
static uint8_t missing_motor = 0;
311,4 → 313,163
motor_write = 0;
motor_read = 0;
}
}*/
 
 
/****************************************/
/* I2C ISR */
/****************************************/
ISR (TWI_vect)
{
static uint8_t missing_motor = 0, send = 0, crc = 0;
uint8_t test[] = {0,0,'#',0x1F,255,30,100,64,0x00,7,8,9,10};
 
switch (twi_state++) // First i2c_start from SendMotorData()
{
// Master Transmit
case 0: // TWI_STATE_MOTOR_TX
// skip motor if not used in mixer
while((Mixer.Motor[motor_write][MIX_GAS] <= 0) && (motor_write < MAX_MOTORS)) motor_write++;
if(motor_write >= MAX_MOTORS) // writing finished, read now
{
motor_write = 0; // reset motor write counter for next cycle
twi_state = TWI_STATE_MOTOR_RX;
I2C_WriteByte(0x53 + (motor_read * 2) ); // select slave adress in rx mode
}
else I2C_WriteByte(0x52 + (motor_write * 2) ); // select slave adress in tx mode
send = 0; // reset send byte counter
break;
case 1: // Send Data to Slave
I2C_WriteByte(Motor[motor_write].SetPoint); // transmit rotation rate setpoint
// jump over sending more data if old version has been detected
if(!(Motor[motor_write].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) twi_state = 3;
break;
case 2:
if(!send++)
{ // init
I2C_WriteByte((Motor[motor_write].SetPointLowerBits << 1)); // send the lower bits of pwm
crc = 0xAA; // init calculation of crc
}
else if(send == 9)
{ // send crc byte at the end
I2C_WriteByte(crc);
}
else
{ // transmit data to BLs
crc += test[send]; // update crc
I2C_WriteByte(test[send]); // submit next byte
}
// check for end of transmission condition
if(TransmitBlConfig && !(FCFlags&FCFLAG_MOTOR_RUN) && (motor_write == motor_read) && (Motor[motor_read].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) )
{
if(send <= 10) twi_state = 2; // repeat sending data to this motor
}
break;
case 3: // repeat case 0+1+2+3 for all motors
if(TWSR == TW_MT_DATA_NACK) // Data transmitted, NACK received
{
if(!missing_motor) missing_motor = motor_write + 1;
if((Motor[motor_write].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor_write].State++; // increment error counter and handle overflow
}
I2C_Stop(TWI_STATE_MOTOR_TX);
I2CTimeout = 10;
motor_write++; // next motor
I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive
break;
// Master Receive Data
case 4: // TWI_STATE_MOTOR_RX
if(TWSR != TW_MR_SLA_ACK) // SLA+R transmitted, if not ACK received
{ // no response from the addressed slave received
Motor[motor_read].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
motor_read++; // read next motor
if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one
I2C_Stop(TWI_STATE_MOTOR_TX);
}
else
{
Motor[motor_read].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
I2C_ReceiveByte(); //Transmit 1st byte
}
MissingMotor = missing_motor;
missing_motor = 0;
break;
case 5: //Read 1st byte and transmit 2nd Byte
Motor[motor_read].Current = TWDR;
I2C_ReceiveByte(); // ack
break;
case 6:
//Read 2nd byte and transmit 3rd Byte
Motor[motor_read].MaxPWM = TWDR;
if(TWDR == 250)
{ // a new BL found
Motor[motor_read].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK;
}
else if(TransmitBlConfig)
{ // its not a new BL
Motor[motor_read].Version = 0; // jump over send config block in tx cycle
}
I2C_ReceiveLastByte(); // nack
break;
 
case 7:
// read 3rd byte
Motor[motor_read].Temperature = TWDR;
motor_read++; // next motor
if(motor_read >= MAX_MOTORS)
{
TransmitBlConfig = 0; // send config only once
motor_read = 0; // restart reading of first motor if we have reached the last one
}
I2C_Stop(TWI_STATE_MOTOR_TX);
break;
 
// writing Gyro-Offsets
case 18:
I2C_WriteByte(0x98); // Address the DAC
break;
 
case 19:
I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C)
break;
 
case 20:
switch(dac_channel)
{
case 0:
I2C_WriteByte(DacOffsetGyroNick); // 1st byte for Channel A
break;
case 1:
I2C_WriteByte(DacOffsetGyroRoll); // 1st byte for Channel B
break;
case 2:
I2C_WriteByte(DacOffsetGyroYaw ); // 1st byte for Channel C
break;
}
break;
 
case 21:
I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80
break;
 
case 22:
I2C_Stop(TWI_STATE_MOTOR_TX);
I2CTimeout = 10;
// repeat case 7...10 until all DAC Channels are updated
if(dac_channel < 2)
{
dac_channel ++; // jump to next channel
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel
}
else
{ // data to last motor send
dac_channel = 0; // reset dac channel counter
}
break;
 
default:
I2C_Stop(TWI_STATE_MOTOR_TX);
I2CTimeout = 10;
motor_write = 0;
motor_read = 0;
}
}
/beta/Code Redesign killagreg/twimaster.h
5,25 → 5,31
#include <inttypes.h>
 
#define TWI_STATE_MOTOR_TX 0
#define TWI_STATE_MOTOR_RX 3
#define TWI_STATE_GYRO_OFFSET_TX 7
#define TWI_STATE_MOTOR_RX 4
#define TWI_STATE_GYRO_OFFSET_TX 18
 
extern volatile uint8_t twi_state;
extern volatile uint8_t motor_write;
extern volatile uint8_t motor_read;
extern volatile uint8_t TransmitBlConfig;
 
extern uint8_t MissingMotor;
 
#define MAX_MOTORS 12
#define MOTOR_STATE_PRESENT_MASK 0x80
#define MOTOR_STATE_ERROR_MASK 0x7F
#define MOTOR_STATE_PRESENT_MASK 0x80
#define MOTOR_STATE_ERROR_MASK 0x7F
 
#define MOTOR_STATE_NEW_PROTOCOL_MASK 0x01
 
typedef struct
{
uint8_t SetPoint; // written by attitude controller
uint8_t State; // 7 bit for I2C error counter, highest bit indicates if motor is present
uint8_t Current; // in 0.1 A steps, read back from BL
uint8_t MaxPWM; // read back from BL is less than 255 if BL is in current limit
uint8_t SetPoint; // written by attitude controller
uint8_t State; // 7 bit for I2C error counter, highest bit indicates if motor is present
uint8_t Current; // in 0.1 A steps, read back from BL
uint8_t MaxPWM; // read back from BL is less than 255 if BL is in current limit
int8_t Temperature; // old BL-Ctrl will return a 255 here
uint8_t SetPointLowerBits; // for higher Resolution
uint8_t Version;
} __attribute__((packed)) MotorData_t;
 
extern MotorData_t Motor[MAX_MOTORS];
/beta/Code Redesign killagreg/uart0.c
154,8 → 154,8
"Motor 4 ", //15
" ",
" ",
"VarioMeter ",
"MK3MAG CalState ",
" ",
" ",
"NickServo ", //20
"Hovergas ",
"Current [0.1A] ",
509,6 → 509,7
//Request_MotorTest = TRUE;
MotorTest_Active = 255;
PcAccess = 255;
AboTimeOut = SetDelay(ABO_TIMEOUT);
break;
 
case 'n':// "Get Mixer Table
743,6 → 744,7
}
else if( (((DebugData_Interval > 0) && CheckDelay(DebugData_Timer)) || Request_DebugData) && txd_complete)
{
UpdateDebugValues();
SendOutData('D', FC_ADDRESS, 1,(uint8_t *) &DebugOut, sizeof(DebugOut));
DebugData_Timer = SetDelay(DebugData_Interval);
Request_DebugData = FALSE;