Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 693 → Rev 694

/branches/V0.68d Code Redesign killagreg/eeprom.h
11,6 → 11,14
#define PID_ACC_ROLL 6 // word
#define PID_ACC_Z 8 // word
 
#define PID_MM3_X_OFF 9
#define PID_MM3_Y_OFF 10
#define PID_MM3_Z_OFF 11
#define PID_MM3_X_RANGE 12
#define PID_MM3_Y_RANGE 14
#define PID_MM3_Z_RANGE 16
 
 
#define EEPROM_ADR_PARAMSET_BEGIN 100
 
// bit mask for mk_param_struct.GlobalConfig
43,7 → 51,7
 
typedef struct
{
uint8_t ChannelAssignment[8]; // see upper defines for details
uint8_t ChannelAssignment[8]; // see upper defines for details
uint8_t GlobalConfig; // see upper defines for bitcoding
uint8_t Hoehe_MinGas; // Wert : 0-100
uint8_t Luftdruck_D; // Wert : 0-250
/branches/V0.68d Code Redesign killagreg/fc.c
64,6 → 64,7
#include "uart.h"
#include "rc.h"
#include "twimaster.h"
#include "mm3.h"
 
unsigned char h,m,s;
volatile unsigned int I2CTimeout = 100;
931,26 → 932,32
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Kompass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(KompassValue && (ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE))
{
int w,v;
static int SignalSchlecht = 0;
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
v = abs(IntegralRoll /512);
if(v > w) w = v; // grösste Neigung ermitteln
if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)
if(ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE)
{
int w,v;
static uint8_t updKompass = 0;
 
if (!updKompass--) // Aufruf mit ~10 Hz
{
KompassValue = MM3_heading();
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
updKompass = 50;
}
 
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
v = abs(IntegralRoll /512);
if(v > w) w = v; // grösste Neigung ermitteln
if(w < 35 && NeueKompassRichtungMerken)
{
KompassStartwert = KompassValue;
NeueKompassRichtungMerken = 0;
}
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
if(w > 0)
{
KompassStartwert = KompassValue;
NeueKompassRichtungMerken = 0;
}
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
if(w > 0)
{
if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten
if(SignalSchlecht) SignalSchlecht--;
}
else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten
}
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
/branches/V0.68d Code Redesign killagreg/main.c
64,7 → 64,8
#include "rc.h"
#include "analog.h"
#include "printf_P.h"
#include "spi.h"
//#include "spi.h"
#include "mm3.h"
#include "twimaster.h"
#include "eeprom.h"
#include "_Settings.h"
122,6 → 123,7
rc_sum_init();
ADC_Init();
i2c_init();
MM3_init();
//SPI_MasterInit();
 
// enable interrupts global
143,7 → 145,18
printf("\n\rACC nicht abgeglichen!");
}
 
//kurze Wartezeit (sonst reagiert die "Kompass kalibrieren?"-Abfrage nicht
timer = SetDelay(500);
while(!CheckDelay(timer));
 
//Compass calibration?
if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 100 && PPM_in[ParamSet.ChannelAssignment[CH_GIER]] > 100)
{
printf("\n\rCalibrating Compass");
MM3_calibrate();
}
 
 
if(ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)
{
printf("\n\rAbgleich Luftdrucksensor..");
173,7 → 186,7
{
if(UpdateMotor) // ReglerIntervall
{
SPI_TransmitByte(); //#
//SPI_TransmitByte(); //#
UpdateMotor=0;
//PORTD |= 0x08;
MotorRegler();
223,7 → 236,7
BeepModulation = 0x0300;
}
}
SPI_StartTransmitPacket();//#
//SPI_StartTransmitPacket();//#
timer = SetDelay(100);
}
//if(UpdateMotor) DebugOut.Analog[26]++;
/branches/V0.68d Code Redesign killagreg/makefile
75,7 → 75,7
##########################################################################################################
# List C source files here. (C dependencies are automatically generated.)
SRC = main.c uart.c printf_P.c timer0.c timer2.c analog.c menu.c
SRC += twimaster.c rc.c fc.c GPS.c spi.c eeprom.c
SRC += twimaster.c rc.c fc.c GPS.c spi.c eeprom.c mm3.c
SRC += mymath.c ubx.c fifo.c uart1.c
 
##########################################################################################################
/branches/V0.68d Code Redesign killagreg/menu.c
15,9 → 15,9
#include "uart.h"
#include "printf_P.h"
#include "analog.h"
#include "mm3.h"
#include "_Settings.h"
 
uint16_t TestInt = 0;
#define ARRAYSIZE 10
uint8_t Array[ARRAYSIZE] = {1,2,3,4,5,6,7,8,9,10};
#define DISPLAYBUFFSIZE 80
47,7 → 47,7
// Display with 20 characters in 4 lines
void LCD_PrintMenu(void)
{
static uint8_t MaxMenuItem = 11, MenuItem=0;
static uint8_t MaxMenuItem = 13, MenuItem=0;
 
// if KEY1 is activated goto previous menu item
if(RemoteButtons & KEY1)
90,29 → 90,29
case 1:// Hight Control Menu Item
if(ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)
{
LCD_printfxy(0,0,"Hoehe: %5i",HoehenWert);
LCD_printfxy(0,1,"SollHoehe: %5i",SollHoehe);
LCD_printfxy(0,2,"Luftdruck: %5i",MessLuftdruck);
LCD_printfxy(0,3,"Off : %5i",DruckOffsetSetting);
LCD_printfxy(0,0,"Hight: %5i",HoehenWert);
LCD_printfxy(0,1,"Set Point: %5i",SollHoehe);
LCD_printfxy(0,2,"Air Press.: %5i",MessLuftdruck);
LCD_printfxy(0,3,"Offset : %5i",DruckOffsetSetting);
}
else
{
LCD_printfxy(0,1,"Keine ");
LCD_printfxy(0,2,"Höhenregelung");
LCD_printfxy(0,1,"No ");
LCD_printfxy(0,2,"Hight Control");
}
 
break;
case 2:// Attitude Menu Item
LCD_printfxy(0,0,"akt. Lage");
LCD_printfxy(0,0,"Attitude");
LCD_printfxy(0,1,"Nick: %5i",IntegralNick/1024);
LCD_printfxy(0,2,"Roll: %5i",IntegralRoll/1024);
LCD_printfxy(0,3,"Kompass: %5i",KompassValue);
LCD_printfxy(0,3,"Compass: %5i",KompassValue);
break;
case 3:// Remote Control Channel Menu Item
LCD_printfxy(0,0,"K1:%4i K2:%4i ",PPM_in[1],PPM_in[2]);
LCD_printfxy(0,1,"K3:%4i K4:%4i ",PPM_in[3],PPM_in[4]);
LCD_printfxy(0,2,"K5:%4i K6:%4i ",PPM_in[5],PPM_in[6]);
LCD_printfxy(0,3,"K7:%4i K8:%4i ",PPM_in[7],PPM_in[8]);
LCD_printfxy(0,0,"C1:%4i C2:%4i ",PPM_in[1],PPM_in[2]);
LCD_printfxy(0,1,"C3:%4i C4:%4i ",PPM_in[3],PPM_in[4]);
LCD_printfxy(0,2,"C5:%4i C6:%4i ",PPM_in[5],PPM_in[6]);
LCD_printfxy(0,3,"C7:%4i C8:%4i ",PPM_in[7],PPM_in[8]);
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]]);
126,29 → 126,29
{
LCD_printfxy(0,1,"Nick %4i (%3i)",AdValueGyrNick - AdNeutralNick, AdNeutralNick);
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueGyrRoll - AdNeutralRoll, AdNeutralRoll);
LCD_printfxy(0,3,"Gier %4i (%3i)",MesswertGier, AdNeutralGier);
LCD_printfxy(0,3,"Yaw %4i (%3i)",MesswertGier, AdNeutralGier);
}
else
{
LCD_printfxy(0,1,"Nick %4i (%3i)",AdValueGyrNick - AdNeutralNick, AdNeutralNick/2);
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueGyrRoll - AdNeutralRoll, AdNeutralRoll/2);
LCD_printfxy(0,3,"Gier %4i (%3i)",MesswertGier, AdNeutralGier/2);
LCD_printfxy(0,3,"Yaw %4i (%3i)",MesswertGier, AdNeutralGier/2);
}
break;
case 6:// Acceleration Sensor Menu Item
LCD_printfxy(0,0,"ACC - Sensor");
LCD_printfxy(0,1,"Nick %4i (%3i)",AdValueAccNick,NeutralAccX);
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueAccRoll,NeutralAccY);
LCD_printfxy(0,3,"Hoch %4i (%3i)",Mittelwert_AccHoch/*accumulate_AccHoch / messanzahl_AccHoch*/,(int)NeutralAccZ);
LCD_printfxy(0,1,"Nick %4i (%3i)",AdValueAccNick,NeutralAccX);
LCD_printfxy(0,2,"Roll %4i (%3i)",AdValueAccRoll,NeutralAccY);
LCD_printfxy(0,3,"Hight %4i (%3i)",Mittelwert_AccHoch/*accumulate_AccHoch / messanzahl_AccHoch*/,(int)NeutralAccZ);
break;
case 7:// Accumulator Voltage / Remote Control Level
LCD_printfxy(0,1,"Spannung: %5i",UBat);
LCD_printfxy(0,2,"Empf.Pegel:%5i",SenderOkay);
LCD_printfxy(0,1,"Voltage: %5i",UBat);
LCD_printfxy(0,2,"RC-Level: %5i",SenderOkay);
break;
case 8:// Compass Menu Item
LCD_printfxy(0,0,"Kompass ");
LCD_printfxy(0,1,"Richtung: %5i",KompassRichtung);
LCD_printfxy(0,2,"Messwert: %5i",KompassValue);
LCD_printfxy(0,0,"Compass ");
LCD_printfxy(0,1,"Direction: %5i",KompassRichtung);
LCD_printfxy(0,2,"Reading: %5i",KompassValue);
LCD_printfxy(0,3,"Start: %5i",KompassStartwert);
break;
case 9:// Poti Menu Item
160,7 → 160,7
case 10:// Servo Menu Item
LCD_printfxy(0,0,"Servo " );
LCD_printfxy(0,1,"Setpoint %3i",Parameter_ServoNickControl);
LCD_printfxy(0,2,"Stellung: %3i",ServoValue);
LCD_printfxy(0,2,"Position: %3i",ServoValue);
LCD_printfxy(0,3,"Range:%3i-%3i",ParamSet.ServoNickMin,ParamSet.ServoNickMax);
break;
case 11://Extern Control
169,6 → 169,18
LCD_printfxy(0,2,"Gs:%4i Gi:%4i ",ExternControl.Gas,ExternControl.Gier);
LCD_printfxy(0,3,"Hi:%4i Cf:%4i ",ExternControl.Hight,ExternControl.Config);
break;
case 12:// MM3 Kompass
LCD_printfxy(0,0,"MM3 Offset");
LCD_printfxy(0,1,"X_Offset: %3i",MM3_calib.X_off);
LCD_printfxy(0,2,"Y_Offset: %3i",MM3_calib.Y_off);
LCD_printfxy(0,3,"Z_Offset: %3i",MM3_calib.Z_off);
break;
case 13://MM3 Range
LCD_printfxy(0,0,"MM3 Range");
LCD_printfxy(0,1,"X_Range: %4i",MM3_calib.X_range);
LCD_printfxy(0,2,"Y_Range: %4i",MM3_calib.Y_range);
LCD_printfxy(0,3,"Z_Range: %4i",MM3_calib.Z_range);
break;
default: MaxMenuItem = MenuItem - 1;
MenuItem = 0;
break;
/branches/V0.68d Code Redesign killagreg/mymath.c
1,3 → 1,4
#include <stdlib.h>
#include <avr/pgmspace.h>
#include "mymath.h"
 
15,14 → 16,14
if (angle < 0)
{
m = -1;
angle = -angle;
angle = abs(angle);
}
else m = 1;
else m = +1;
 
// fold angle to intervall 0 to 359
angle %= 360;
 
// queck quadrant
// check quadrant
if (angle <= 90) n=1; // first quadrant
else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant
else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant
/branches/V0.68d Code Redesign killagreg/timer0.c
5,6 → 5,7
#include "analog.h"
#include "main.h"
#include "fc.h"
#include "mm3.h"
 
volatile uint16_t CountMilliseconds = 0;
volatile uint8_t UpdateMotor = 0;
30,6 → 31,18
DDRB |= (1<<DDB4)|(1<<DDB3);
PORTB &= ~((1<<PORTB4)|(1<<PORTB3));
 
if(BoardRelease == 10)
{
DDRD |= (1<<DDD2);
PORTD &= ~(1<<PORTD2);
 
}
else
{
DDRC |= (1<<DDC7);
PORTC &= ~(1<<PORTC7);
}
 
// Timer/Counter 0 Control Register A
 
// Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1)
106,26 → 119,14
else // beeper is off
{
// set speaker port to low
if(BoardRelease == 10) PORTD &= ~(1<<2);// Speaker at PD2
else PORTC &= ~(1<<7);// Speaker at PC7
if(BoardRelease == 10) PORTD &= ~(1<<PORTD2);// Speaker at PD2
else PORTC &= ~(1<<PORTC7);// Speaker at PC7
}
 
// update compass value if this option is enabled in the settings
if(ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE)
{
if(PINC & 0x10)
{
cntKompass++;
}
else
{
if((cntKompass) && (cntKompass < 4000))
{
KompassValue = cntKompass;
}
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
cntKompass = 0;
}
MM3_timer0();
}
}
 
134,17 → 135,13
// -----------------------------------------------------------------------
uint16_t SetDelay (uint16_t t)
{
// TIMSK0 &= ~(1<<TOIE0);
return(CountMilliseconds + t + 1);
// TIMSK0 |= (1<<TOIE0);
}
 
// -----------------------------------------------------------------------
int8_t CheckDelay(uint16_t t)
{
// TIMSK0 &= ~(1<<TOIE0);
return(((t - CountMilliseconds) & 0x8000) >> 9);
// TIMSK0 |= (1<<TOIE0);
return(((t - CountMilliseconds) & 0x8000) >> 9); // check sign bit
}
 
// -----------------------------------------------------------------------
/branches/V0.68d Code Redesign killagreg/twimaster.c
8,79 → 8,134
#include "twimaster.h"
#include "fc.h"
 
unsigned char twi_state = 0;
unsigned char motor = 0;
unsigned char motorread = 0;
unsigned char motor_rx[8];
volatile uint8_t twi_state = 0;
volatile uint8_t motor = 0;
volatile uint8_t motor_rx[8];
 
//############################################################################
//Initzialisieren der I2C (TWI) Schnittstelle
/**************************************************/
/* Initialize I2C (TWI) */
/**************************************************/
void i2c_init(void)
//############################################################################
{
TWSR = 0;
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
uint8_t sreg = SREG;
cli();
 
// SDA is INPUT
DDRC &= ~(1<<DDC1);
// SCL is output
DDRC |= (1<<DDC0);
// pull up SDA
PORTC |= (1<<PORTC0)|(1<<PORTC1);
 
// TWI Status Register
// prescaler 1 (TWPS1 = 0, TWPS0 = 0)
TWSR &= ~((1<<TWPS1)|(1<<TWPS0));
 
// set TWI Bit Rate Register
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
 
SREG = sreg;
}
 
//############################################################################
//Start I2C
char i2c_start(void)
//############################################################################
/****************************************/
/* Start I2C */
/****************************************/
void i2c_start(void)
{
TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
return(0);
// TWI Control Register
// clear TWI interrupt flag (TWINT=1)
// disable TWI Acknowledge Bit (TWEA = 0)
// enable TWI START Condition Bit (TWSTA = 1), MASTER
// disable TWI STOP Condition Bit (TWSTO = 0)
// disable TWI Write Collision Flag (TWWC = 0)
// enable i2c (TWIE = 1)
// enable TWI Interrupt (TWIE = 1)
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE);
}
 
//############################################################################
//Start I2C
/****************************************/
/* Stop I2C */
/****************************************/
void i2c_stop(void)
//############################################################################
{
TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);
// TWI Control Register
// clear TWI interrupt flag (TWINT=1)
// disable TWI Acknowledge Bit (TWEA = 0)
// diable TWI START Condition Bit (TWSTA = 1), no MASTER
// enable TWI STOP Condition Bit (TWSTO = 1)
// disable TWI Write Collision Flag (TWWC = 0)
// enable i2c (TWIE = 1)
// disable TWI Interrupt (TWIE = 0)
TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN);
}
 
/****************************************/
/* Reset I2C */
/****************************************/
void i2c_reset(void)
//############################################################################
{
i2c_stop();
twi_state = 0;
motor = TWDR;
motor = 0;
TWCR = 0x80;
TWAMR = 0;
TWAR = 0;
TWDR = 0;
TWSR = 0;
TWBR = 0;
i2c_init();
i2c_start();
i2c_write_byte(0);
// stop i2c bus
i2c_stop();
twi_state = 0;
motor = TWDR; // ??
motor = 0;
TWCR = (1<<TWINT); // reset to original state incl. interrupt flag reset
TWAMR = 0;
TWAR = 0;
TWDR = 0;
TWSR = 0;
TWBR = 0;
i2c_init();
i2c_start();
i2c_write_byte(0);
}
 
//############################################################################
//Start I2C
int8_t i2c_write_byte(int8_t byte)
//############################################################################
/****************************************/
/* Write to I2C */
/****************************************/
void i2c_write_byte(int8_t byte)
{
TWSR = 0x00;
// move byte to send into TWI Data Register
TWDR = byte;
// clear interrupt flag (TWINT = 1)
// enable i2c bus (TWEN = 1)
// enable intterupt (TWIW = 1)
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
}
 
return(0);
 
/****************************************/
/* Receive byte and send ACK */
/****************************************/
void i2c_receive_byte(void)
{
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);
}
 
//############################################################################
//Start I2C
SIGNAL (TWI_vect)
//############################################################################
/****************************************/
/* I2C receive last byte and send no ACK*/
/****************************************/
void i2c_receive_last_byte(void)
{
switch (twi_state++)
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
}
 
 
/****************************************/
/* I2C ISR */
/****************************************/
ISR (TWI_vect)
 
{
static uint8_t motorread = 0;
 
switch (twi_state++) // First i2s_start from SendMotorData()
{
case 0:
// Master Transmit
case 0: // Send SLA-W
i2c_write_byte(0x52+(motor*2));
break;
case 1:
case 1: // Send Data to Salve
switch(motor++)
{
case 0:
97,61 → 152,33
break;
}
break;
case 2:
i2c_stop();
case 2: // repeat case 0+1 for all Slaves
if (motor<4) twi_state = 0;
else motor = 0;
i2c_start();
i2c_start(); // Repeated start -> switch salve or switch Master Transmit -> Master Receive
break;
 
//Liest Daten von Motor
case 3:
// Master Receive
case 3: // Send SLA-R
i2c_write_byte(0x53+(motorread*2));
break;
case 4:
switch(motorread)
{
case 0:
i2c_write_byte(Motor_Vorne);
break;
case 1:
i2c_write_byte(Motor_Hinten);
break;
case 2:
i2c_write_byte(Motor_Rechts);
break;
case 3:
i2c_write_byte(Motor_Links);
break;
}
//Transmit 1st byte
i2c_receive_byte();
break;
case 5: //1 Byte vom Motor lesen
case 5: //Read 1st byte and transmit 2nd Byte
motor_rx[motorread] = TWDR;
i2c_receive_last_byte();
break;
case 6:
//Read 2nd byte
motor_rx[motorread+4] = TWDR;
motorread++;
if (motorread > 3) motorread=0;
 
case 6:
switch(motorread)
{
case 0:
i2c_write_byte(Motor_Vorne);
break;
case 1:
i2c_write_byte(Motor_Hinten);
break;
case 2:
i2c_write_byte(Motor_Rechts);
break;
case 3:
i2c_write_byte(Motor_Links);
break;
}
break;
case 7: //2 Byte vom Motor lesen
motor_rx[motorread+4] = TWDR;
motorread++;
if (motorread>3) motorread=0;
default:
i2c_stop();
twi_state = 0;
I2CTimeout = 10;
twi_state = 0;
motor = 0;
}
TWCR |= 0x80;
}
/branches/V0.68d Code Redesign killagreg/twimaster.h
19,16 → 19,16
 
//############################################################################
 
extern uint8_t twi_state;
extern uint8_t motor;
extern uint8_t motorread;
extern uint8_t motor_rx[8];
extern volatile uint8_t twi_state;
extern volatile uint8_t motor;
extern volatile uint8_t motorread;
extern volatile uint8_t motor_rx[8];
 
void i2c_reset(void);
extern void i2c_init (void); // I2C initialisieren
extern char i2c_start (void); // Start I2C
extern void i2c_start (void); // Start I2C
extern void i2c_stop (void); // Stop I2C
extern int8_t i2c_write_byte (int8_t byte); // 1 Byte schreiben
extern void i2c_write_byte (int8_t byte); // 1 Byte schreiben
extern void i2c_reset(void);
 
#endif