Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1198 → Rev 1199

/branches/V0.72p Code Redesign killagreg/fifo.c
File deleted
/branches/V0.72p Code Redesign killagreg/fifo.h
File deleted
/branches/V0.72p Code Redesign killagreg/Hex-Files/Readme.txt
143,16 → 143,7
 
Das Vorgehen erfolgt beim MM3 und MK3MAG exakt gleich wie hier beschrieben.
http://www.mikrokopter.de/ucwiki/MK3Mag?highlight=%28mk3mag%29
 
Zur Bestimmung des User Parameters 3 (Umrechnungsfaktor zwischen dem Gyrointegral und dem zugehörigen Neigungswinkel)
hat sich folgendes Vorgehen bewehrt.
- Man bestimmt den Wert des Roll- und Nick-Integrals für einen Neigungswinkel von 45°
über die Ausgaben des Koptertools.
- Aus diesem Wert kann man den benötigten UserParam3 berechnen.
UserParam3 = (Nick-Integral(45°)+Roll-Integral(45°))/2*GyroACCFaktor/45°/8. (typisch 170)
- Nachdem dieser Wert über die Settings des Koptertools im MK abgepeichert ist, sollte sich der CompassValue bei
Verkippungen nur unwesentlich verändern.
 
/branches/V0.72p Code Redesign killagreg/dsl.c
0,0 → 1,156
#include "dsl.h"
 
/*
1. GND (at channel 4 side)
2. TxD
3. RxD
4. 5V
*/
 
// dsl protocol parser state machine
#define DSLSTATE_IDLE 0
#define DLSSTATE_SYNC 1
#define DSLSTATE_HDR 2
#define DSLSTATE_DATA 3
 
#define DSL_SYNC1_CHAR 0xFF
#define DSL_SYNC2_CHAR 0xFF
 
 
volatile uint8_t rcdsl_RSSI = 0;
uint8_t dsl_battery = 0;
uint8_t dsl_allocation = 0;
uint8_t data[6];
 
typedef union {
uint16_t pos[2];
uint8_t dat[4];
} servos_t;
 
 
// This function is called, when a new servo signal is properly received.
// Parameters: servo - servo number (0-9)
// signal - servo signal between 7373 (1ms) and 14745 (2ms)
void dsl_new_signal(uint8_t channel, int16_t signal)
{
volatile signed int tmp;
uint8_t index = channel+1; // Der MK fängt bei 1 an zu zählen
 
 
// Signal vom ACTDSL-Empfänger liegt zwischen
// 7373 (1ms) und 14745 (2ms).
signal-= 11059; // Neutralstellung zur Null verschieben
signal/= 24; // Auf MK Skalierung umrechnen
 
// Stabiles Signal
//if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10;}
 
tmp = (3 * (PPM_in[index]) + signal) / 4;
if(tmp > signal+1) tmp--; else
if(tmp < signal-1) tmp++;
if(SenderOkay >= 105) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;
else PPM_diff[index] = 0;
PPM_in[index] = tmp;
//NewPpmData = 0;
//NewActData = 1;
if(index == 4) {
//NewActData = 1;
NewPpmData = 0;
//PORTC = (PORTC&(~(1<<PC4))) | ((~PORTC)&(1<<PC4));
//PINC = (1<<PC4);
}
//PPM_in[index] = signal;
 
}
 
// This function is called within dsl_parser(), when a complete
// data packet with valid checksum has been received.
void dsl_incoming_paket(void)
{
uint8_t i;
static servos_t servos;
// Look for status headers
if ((data[0])==0x1F) {
// Get frequency allocation
dsl_allocation = data[0+1];
// Get signal quality
dsl_RSSI = data[2+1];
// Get voltage of battery supply
dsl_battery = data[3+1];
}
 
// Look for signal headers
if ((data[0]&0xF0)==0x10) {
 
i = data[0]&0x0F; // Last 4 bits of the header indicates servo pair
 
if (i<10) {
// Convert byte array to two uint16
servos.dat[1] = data[1];
servos.dat[0] = data[2];
servos.dat[3] = data[3];
servos.dat[2] = data[4];
 
rcdsl_new_signal(i , (int16_t)servos.pos[0]);
rcdsl_new_signal(i+1, (int16_t)servos.pos[1]);
}
}
}
 
 
// Status Frame: |0xFF|0xFF|0x1F|FREQALLOC|??|RSSI|VBAT|CRC
// FREQ ALLOC = 35, 40, 72
// RSSI = 0.. 255 // Received signal strength indicator
// VBAT = 0...255 // supply voltage (0.0V.. 8.2V)
//
//|0xFF|0xFF|CMD|D0|D1|D2|D3
 
// this function should be called within the UART RX ISR
void dsl_parser(uint8_t c)
{
static uint8_t crc = 0;
static uint8_t packet_len = 0;
static uint8_t data_counter = 0;
 
static uint8_t dslstate = DSLSTATE_IDLE;
 
switch(dslstate)
{
case DSLSTATE_IDLE: // check 1st sync byte
if (c == DSL_SYNC1_CHAR) dslstate = DSLSTATE_SYNC;
else dslstate = UBXSTATE_IDLE; // out of synchronization
break;
 
case DSLSTATE_SYNC: // check 2nd sync byte
if (c == DSL_SYNC2_CHAR) dslstate = DSLSTATE_CMD; // snchrinization complete trigger to cmd
else dslstate = DSLSTATE_IDLE; // out of synchronization
break;
 
case DSLSTATE_HDR: // check cmd byte
if (c == 0x1F) packet_len = 5;
else packet_len = 4;
dcnt = 1;
crc = 0;
dslstate = DSLSTATE_DATA;
break;
 
case DSLSTATE_DATA:
data[dcnt++] = c;
crc += c;
if(dcnt > packetlen) dslstate = DSLSTATE_CRC;
break;
 
case DSLSTATE_CRC:
// Calculate checksum
crc = ~crc;
if (crc == 0xFF) crc = 0xFE;
// If it match the received one, then apply data
if (c == crc) rcdsl_incoming_paket();
dslstate = DSLSTATE_IDLE;
break;
 
default: // unknown ubx state
dslstate = DSLSTATE_IDLE;
break;
}
}
/branches/V0.72p Code Redesign killagreg/dsl.h
0,0 → 1,19
#ifndef _DSL_H
#define _DSL_H
 
#include <inttypes.h>
 
// Received signal strength indicator
extern volatile uint8_t dsl_RSSI;
 
// Battery voltage (0-255 [0V - 8.2V])
extern uint8_t dsl_battery;
 
// Frequency allocation (35,40,72)
extern uint8_t dsl_allocation;
 
// this function should be called within the UART RX ISR
extern void dsl_parser(uint8_t c);
 
#endif //_DSL_H
 
/branches/V0.72p Code Redesign killagreg/main.c
206,7 → 206,13
 
#ifdef USE_NAVICTRL
printf("\n\rSupport for NaviCtrl");
#ifdef USE_RC_DSL
printf("\r\nSupport for DSL RC at 2nd UART");
#endif
#ifdef USE_RC_SPECTRUM
printf("\r\nSupport for SPECTRUM RC at 2nd UART");
#endif
#endif
 
#ifdef USE_KILLAGREG
printf("\n\rSupport for MicroMag3 Compass");
/branches/V0.72p Code Redesign killagreg/makefile
15,10 → 15,14
#OPTIONS
 
# Use one of the extensions for a gps solution
#EXT = KILLAGREG
EXT = NAVICTRL
EXT = KILLAGREG
#EXT = NAVICTRL
#EXT = MK3MAG
 
# Use optional one the RCs if EXT = NAVICTRL has been used
#RC = DSL
#RC = SPECTRUM
 
# Use one of the motor setups
 
# Standard
158,7 → 162,7
##########################################################################################################
# List C source files here. (C dependencies are automatically generated.)
SRC = main.c uart0.c printf_P.c timer0.c timer2.c analog.c menu.c led.c
SRC += twimaster.c rc.c fc.c eeprom.c fifo.c uart1.c
SRC += twimaster.c rc.c fc.c eeprom.c uart1.c
 
ifeq ($(EXT), KILLAGREG)
SRC += mm3.c mymath.c gps.c ubx.c
168,7 → 172,13
endif
ifeq ($(EXT), NAVICTRL)
SRC += spi.c
ifeq ($(RC), DSL)
SRC += dsl.c
endif
ifeq ($(RC), SPECTRUM)
SRC += spectrum.c
endif
endif
##########################################################################################################
 
 
220,7 → 230,13
endif
ifeq ($(EXT), NAVICTRL)
CFLAGS += -DUSE_NAVICTRL
ifeq ($(RC), DSL)
CFLAGS += -DUSE_RC_DSL
endif
ifeq ($(RC), SPECTRUM)
CFLAGS += -DUSE_RC_SPECTRUM
endif
endif
 
ifeq ($(SETUP), QUADRO)
CFLAGS += -DUSE_QUADRO
/branches/V0.72p Code Redesign killagreg/mm3.c
27,6 → 27,55
#include "eeprom.h"
#include "printf_P.h"
 
 
// for compatibility reasons gcc3.x <-> gcc4.x
#ifndef SPCR
#define SPCR SPCR0
#endif
#ifndef SPIE
#define SPIE SPIE0
#endif
#ifndef SPE
#define SPE SPE0
#endif
#ifndef DORD
#define DORD DORD0
#endif
#ifndef MSTR
#define MSTR MSTR0
#endif
#ifndef CPOL
#define CPOL CPOL0
#endif
#ifndef CPHA
#define CPHA CPHA0
#endif
#ifndef SPR1
#define SPR1 SPR01
#endif
#ifndef SPR0
#define SPR0 SPR00
#endif
 
#ifndef SPDR
#define SPDR SPDR0
#endif
 
#ifndef SPSR
#define SPSR SPSR0
#endif
#ifndef SPIF
#define SPIF SPIF0
#endif
#ifndef WCOL
#define WCOL WCOL0
#endif
#ifndef SPI2X
#define SPI2X SPI2X0
#endif
// -------------------------
 
 
#define MAX_AXIS_VALUE 500
 
 
86,7 → 135,7
#define MM3_RESET_DDR DDRC
#define MM3_RESET_PIN PC5
#endif
 
#define MM3_SS_ON MM3_SS_PORT &= ~(1<<MM3_SS_PIN);
#define MM3_SS_OFF MM3_SS_PORT |= (1<<MM3_SS_PIN);
#define MM3_RESET_ON MM3_RESET_PORT |= (1<<MM3_RESET_PIN);
416,7 → 465,6
int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw;
int32_t Hx, Hy, Hz, Hx_corr, Hy_corr;
int16_t angle;
uint16_t div_factor;
int16_t heading;
 
if (MM3_Timeout)
449,15 → 497,12
 
// tilt compensation
 
// calibration factor for transforming Gyro Integrals to angular degrees
div_factor = (uint16_t)ParamSet.UserParam3 * 8;
 
// calculate sinus cosinus of nick and tilt angle
angle = (IntegralNick/div_factor);
angle = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR);
sin_nick = (int32_t)(c_sin_8192(angle));
cos_nick = (int32_t)(c_cos_8192(angle));
 
angle = (IntegralRoll/div_factor);
angle = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR);
sin_roll = (int32_t)(c_sin_8192(angle));
cos_roll = (int32_t)(c_cos_8192(angle));
 
/branches/V0.72p Code Redesign killagreg/uart1.c
52,23 → 52,24
#include <avr/interrupt.h>
#include "main.h"
#include "uart1.h"
#include "fifo.h"
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
#include "ubx.h"
#define USART1_BAUD 57600
#else
#ifdef USE_RC_DSL
#include "dsl.h"
#define USART1_BAUD 38400
#endif
#ifdef USE_RC_SPECTRUM
#include "spectrum.h"
#define USART1_BAUD 115200
#endif
#endif
 
#ifndef USART1_BAUD
#define USART1_BAUD 57600
#endif
 
 
// FIFO-objects and buffers for input and output
 
//#define BUFSIZE_IN 0x96
//volatile uint8_t inbuf[BUFSIZE_IN];
//fifo_t infifo;
 
#define BUFSIZE_OUT 0x96
volatile uint8_t outbuf[BUFSIZE_OUT];
fifo_t outfifo;
 
/****************************************************************/
/* Initialization of the USART1 */
/****************************************************************/
126,7 → 127,7
// enable RX-Interrupt
UCSR1B |= (1 << RXCIE1);
// enable TX-Interrupt
UCSR1B |= (1 << TXCIE1);
//UCSR1B |= (1 << TXCIE1);
// enable DRE interrupt
//UCSR1B |= (1 << UDRIE1);
 
134,44 → 135,16
// restore global interrupt flags
SREG = sreg;
 
// inint FIFO buffer
//fifo_init (&infifo, inbuf, BUFSIZE_IN);
//fifo_init (&outfifo, outbuf, BUFSIZE_OUT);
}
 
/*int16_t USART1_putc (const uint8_t c)
{
int16_t ret = fifo_put (&outfifo, c);
// create an data register empty interrupt
UCSR1B |= (1 << UDRIE1);
 
return ret;
}
*/
/*int16_t USART1_getc_nowait ()
{
return fifo_get_nowait (&infifo);
}
 
 
uint8_t USART1_getc_wait ()
{
return fifo_get_wait (&infifo);
}
*/
 
/****************************************************************/
/* USART1 data register empty ISR */
/****************************************************************/
/*ISR(USART1_UDRE_vect)
{
// Move a character from the output buffer to the data register.
// When the character was processed the next interrupt is generated.
// If the output buffer is empty the DRE-interrupt is disabled.
if (outfifo.count > 0)
UDR1 = _inline_fifo_get (&outfifo);
else
UCSR1B &= ~(1 << UDRIE1);
 
}
*/
 
191,6 → 164,13
uint8_t c;
c = UDR1; // get data byte
#if (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
ubx_parser(c); // and put it into the ubx protocol parser
ubx_parser(c); // and put it into the ubx protocol parser
#else
#ifdef USE_RC_DSL
dsl_parser(c); // parse dsl data stream
#endif
#ifdef USE_RC_SPECTRUM
spectrum_parser(c); // parse spectrum data stream
#endif
#endif
}
/branches/V0.72p Code Redesign killagreg/uart1.h
1,25 → 1,8
#ifndef _UART1_H
#define _UART1_H
 
#define USART1_BAUD 57600
 
/*
Initialize the USART und activate the receiver and transmitter
as well as the receive-interrupt. The IO-FIFOs are initialized.
The global interrupt-enable-flag (I-Bit in SREG) is not changed
*/
extern void USART1_Init (void);
 
/*
The character c is stored in the output buffer. If the character was pushed sucessfully to
the output buffer then the return value is 1. In case of an output buffer overflow the return value is 0.
The isr is activated, which will send the data from the outbut buffer to the UART.
*/
extern int USART1_putc (const uint8_t c);
 
/*
extern uint8_t USART1_getc_wait(void);
extern int16_t USART1_getc_nowait(void);
*/
 
#endif //_UART1_H