Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 4 → Rev 5

/branches/V0.1 killagreg/GPS.c
55,26 → 55,39
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
#include "GPS.h"
 
u8 OsdBar; // Direction home for OSD
s16 OsdDistance; // Distance home
s16 GPS_Nick;
s16 GPS_Pitch;
s16 GPS_Roll;
s16 GPS_Yaw;
 
struct str_GPSParameter GPSParameter;
GPSParameter_t GPSParameter;
 
#define GPS_MODE_FREE 0
#define GPS_MODE_AID 1
#define GPS_MODE_HOLD 2
#define GPS_MODE_HOME 3
u32 GpsFlightMode = GPS_MODE_FREE;
typedef enum
{
GPS_MODE_FREE,
GPS_MODE_AID,
GPS_MODE_HOLD,
GPS_MODE_HOME,
} GpsFlightMode_t;
 
GpsFlightMode_t GpsFlightMode = GPS_MODE_FREE;
 
GPS_Pos_t GPSTargetPosition = {0,0,0, INVALID};
GPS_Pos_t GPSTHomePosition = {0,0,0, INVALID};
GPS_Pos_t PCGPSTargetPosition = {0,0,0, INVALID};
 
 
//------------------------------------------------------------
// Init variables or send configuration to GPS module
void GPS_Init(void)
{
GPS_Nick = 0;
GPS_Pitch = 0;
GPS_Roll = 0;
GPS_Yaw = 0;
 
OsdDistance = 0;
OsdBar = 0;
 
81,39 → 94,39
GPSParameter.P = 100;
GPSParameter.I = 100;
GPSParameter.D = 100;
GPSParameter.A = 100;
GPSParameter.ACC = 100;
GPSParameter.ModeSchalter = 100;
GPSParameter.Amplification = 100;;
GPSParameter.ModeSwitch = 100;
GPSParameter.Amplification = 100;
 
GpsFlightMode = GPS_MODE_FREE;
}
//------------------------------------------------------------
 
u8 Navigation(void)
{
static char GpsFix = 0, NewGpsMode = 0;
static s8 GpsFix = 0, NewGpsMode = 0;
static GpsFlightMode_t oldGpsFlightMode = GPS_MODE_FREE;
static u32 beep_rythm;
u32 tmp_long;
if(NewGPSDataAvail) // there are new data from gps module
if(GPS_Data.Status == NEWDATA) // there are new data from gps module
{
NewGPSDataAvail = 0;
GPS_Data.Status = PROCESSED;
beep_rythm++;
 
GPSParameter.ModeSchalter = Parameter_UserParam1;
GPSParameter.Amplification = (float) Parameter_UserParam2 / (100.0);
GPSParameter.P = (float) Parameter_UserParam3;
GPSParameter.I = (float) Parameter_UserParam4;
GPSParameter.D = (float) Parameter_UserParam5;
GPSParameter.A = (float) Parameter_UserParam6;
GPSParameter.ACC = (float) Parameter_UserParam7;
GPSParameter.ModeSwitch = Parameter.User1;
GPSParameter.Amplification = (float) Parameter.User2 / (100.0);
GPSParameter.P = (float) Parameter.User3;
GPSParameter.I = (float) Parameter.User4;
GPSParameter.D = (float) Parameter.User5;
GPSParameter.ACC = (float) Parameter.User6;
if(SenderOkay < 100) // Empfangsausfall
if(RC_Quality < 100) // RC-Signal lost
{
GPSParameter.ModeSchalter = 0;
GPSParameter.ModeSwitch = 0;
GPSParameter.Amplification = 100;
GPSParameter.P = (float) 90;
GPSParameter.I = (float) 90;
GPSParameter.D = (float) 90;
GPSParameter.A = (float) 90;
GPSParameter.ACC = (float) 90;
}
 
120,12 → 133,12
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ GPS-Mode
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
tmp_long = GpsFlightMode;
if(GPSParameter.ModeSchalter < 20) GpsFlightMode = GPS_MODE_AID;
else
if(GPSParameter.ModeSchalter < 200) GpsFlightMode = GPS_MODE_FREE;
else GpsFlightMode = GPS_MODE_HOME;
if(GpsFlightMode != tmp_long) // Mode changed
oldGpsFlightMode = GpsFlightMode;
if(GPSParameter.ModeSwitch < 50) GpsFlightMode = GPS_MODE_AID;
else if(GPSParameter.ModeSwitch < 180) GpsFlightMode = GPS_MODE_FREE;
else GpsFlightMode = GPS_MODE_HOME;
 
if(GpsFlightMode != oldGpsFlightMode) // Mode changed
{
BeepTime = 100;
NewGpsMode = 1;
136,11 → 149,11
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Fix okay
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((GPS_Data.Flags & GPS_FIX) && ((GPS_Data.Used_Sat >= GPS_SAT_MIN) || GpsFix))
if((GPS_Data.Flags & FLAG_GPSfixOK) && ((GPS_Data.NumOfSats >= GPS_SAT_MIN) || GpsFix))
{
GpsFix = 1; // hysteresis
// here is a good place to put your GPS code...
GPS_Nick = 0; // do nothing
GPS_Pitch = 0; // do nothing
GPS_Roll = 0; // do nothing
}
else
148,10 → 161,10
//+ No Fix
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
{
GPS_Nick = 0; // reset value
GPS_Pitch = 0; // reset value
GPS_Roll = 0; // reset value
if(!(GPS_Data.Flags & 0x01) && !(beep_rythm % 5)) BeepTime = 100;
else if (GPS_Data.Used_Sat < GPS_SAT_MIN && !(beep_rythm % 5)) BeepTime = 10;
else if (GPS_Data.NumOfSats < GPS_SAT_MIN && !(beep_rythm % 5)) BeepTime = 10;
}
}
return (0);
/branches/V0.1 killagreg/GPS.h
1,29 → 1,42
#ifndef __GPS_H
#define __GPS_H
 
#include "91x_lib.h"
#include "GPSUart.h"
 
#define GPS_SAT_MIN 6
#define GPS_FIX 0x01
 
typedef struct
{
s32 Longitude;
s32 Latitude;
s32 Altitude;
Status_t Status;
} GPS_Pos_t;
 
struct str_GPSParameter
extern GPS_Pos_t GPSTargetPosition;
extern GPS_Pos_t GPSTHomePosition;
extern GPS_Pos_t PCGPSTargetPosition;
 
 
typedef struct
{
float Amplification;
float P;
float I;
float D;
float V;
float A;
float ACC;
s32 ModeSchalter;
} __attribute__((packed));
s32 ModeSwitch;
} __attribute__((packed)) GPSParameter_t;
 
extern struct str_GPSParameter GPSParameter;
extern GPSParameter_t GPSParameter;
 
extern u8 OsdBar;
extern s16 OsdDistance;
extern s16 GPS_Nick;
 
extern s16 GPS_Pitch;
extern s16 GPS_Roll;
extern s16 GPS_Yaw;
 
extern u8 Navigation(void);
extern void GPS_Init(void);
/branches/V0.1 killagreg/GPSUart.c
54,27 → 54,130
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
 
u8 NewGPSDataAvail = 0;
u8 NewPCTargetGPSPosition = 0;
u8 GPS_RxBuffer[GPS_RX_SIZE];
#include "91x_lib.h"
#include "uart.h"
#include "GPSUart.h"
#include "led.h"
 
struct str_ubx_nav_sol *GPS_DataSol;
struct str_ubx_nav_posllh *GPS_DataPosllh;
struct str_ubx_nav_velned *GPS_DataVelned;
struct str_ubx_nav_posutm *GPS_DataPosutm;
struct str_gps_nav_data GPS_Data;
// ------------------------------------------------------------------------------------------------
// defines
 
 
// ----------------------------- GPS - Receive ------------------------------------
// --- Connect RXD & TXD to GPS ---
// message sync bytes
#define UBX_SYNC1_CHAR 0xB5
#define UBX_SYNC2_CHAR 0x62
// protocoll identifier
#define UBX_CLASS_NAV 0x01
// message id
#define UBX_ID_POSLLH 0x02
#define UBX_ID_SOL 0x06
#define UBX_ID_POSUTM 0x08
#define UBX_ID_VELNED 0x12
 
// ------------------------------------------------------------------------------------------------
// typedefs
 
// ubx parser state
typedef enum
{
UBXSTATE_IDLE,
UBXSTATE_SYNC1,
UBXSTATE_SYNC2,
UBXSTATE_CLASS,
UBXSTATE_LEN1,
UBXSTATE_LEN2,
UBXSTATE_DATA,
UBXSTATE_CKA,
UBXSTATE_CKB
} ubxState_t;
 
typedef struct
{
u32 ITOW; // ms GPS Millisecond Time of Week
s32 Frac; // ns remainder of rounded ms above
s16 week; // GPS week
u8 GPSfix; // GPSfix Type, range 0..6
u8 Flags; // Navigation Status Flags
s32 ECEF_X; // cm ECEF X coordinate
s32 ECEF_Y; // cm ECEF Y coordinate
s32 ECEF_Z; // cm ECEF Z coordinate
u32 PAcc; // cm 3D Position Accuracy Estimate
s32 ECEFVX; // cm/s ECEF X velocity
s32 ECEFVY; // cm/s ECEF Y velocity
s32 ECEFVZ; // cm/s ECEF Z velocity
u32 SAcc; // cm/s Speed Accuracy Estimate
u16 PDOP; // 0.01 Position DOP
u8 res1; // reserved
u8 numSV; // Number of SVs used in navigation solution
u32 res2; // reserved
Status_t Status; // invalid/newdata/processed
} __attribute__((packed)) ubx_nav_sol_t;
 
 
typedef struct
{
u32 ITOW; // ms GPS Millisecond Time of Week
s32 VEL_N; // cm/s NED north velocity
s32 VEL_E; // cm/s NED east velocity
s32 VEL_D; // cm/s NED down velocity
u32 Speed; // cm/s Speed (3-D)
u32 GSpeed; // cm/s Ground Speed (2-D)
s32 Heading; // 1e-05 deg Heading 2-D
u32 SAcc; // cm/s Speed Accuracy Estimate
u32 CAcc; // deg Course / Heading Accuracy Estimate
Status_t Status; // invalid/newdata/processed
} __attribute__((packed)) ubx_nav_velned_t;
 
typedef struct
{
u32 ITOW; // ms GPS Millisecond Time of Week
s32 LON; // 1e-07 deg Longitude
s32 LAT; // 1e-07 deg Latitude
s32 HEIGHT; // mm Height above Ellipsoid
s32 HMSL; // mm Height above mean sea level
u32 Hacc; // mm Horizontal Accuracy Estimate
u32 Vacc; // mm Vertical Accuracy Estimate
Status_t Status; // invalid/newdata/processed
} __attribute__((packed)) ubx_nav_posllh_t;
 
typedef struct
{
u32 ITOW; // ms GPS Millisecond Time of Week
s32 EAST; // cm easting
s32 NORTH; // cm northing
s32 ALT; // cm altitude
s8 ZONE; // UTM zone number
s8 HEM; // hemissphere indicator (0= North, 1 = South)
Status_t Status; // invalid/newdata/processed
} __attribute__((packed)) ubx_nav_posutm_t;
 
 
 
//------------------------------------------------------------------------------------
// global variables
 
// local buffers for the incomming ubx messages
ubx_nav_sol_t UbxSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
ubx_nav_posllh_t UbxPosLlh = {0,0,0,0,0,0,0, INVALID};
ubx_nav_velned_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
ubx_nav_posutm_t UbxPosUtm = {0,0,0,0,0,0, INVALID};
// shared buffer
gps_data_t GPS_Data = {0,0,0,0,0,0,0,0,0,0, INVALID};
 
//------------------------------------------------------------------------------------
// functions
 
/********************************************************/
/* Connect RXD & TXD to GPS */
/********************************************************/
void Connect_UART0_to_GPS(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
 
SCU_APBPeriphClockConfig(__GPIO6, ENABLE);
SCU_APBPeriphClockConfig(__GPIO6, ENABLE); // Enable the GPIO6 Clock
// unmap UART0 from Compass
// set port pin 5.1 (serial data from compass) to input and disconnect from IP
GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
81,13 → 184,14
GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Disable;
GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1 ;
GPIO_Init(GPIO5, &GPIO_InitStructure);
 
// set port pin 5.0 (serial data to compass) to input
GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1 ;
GPIO_Init(GPIO5, &GPIO_InitStructure);
 
// map UART0 to GPS
// set port pin 6.6 (serial data from gps) to input and connect to IP
GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
94,7 → 198,7
GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;
GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1 ;
GPIO_Init(GPIO6, &GPIO_InitStructure);
 
// set port pin 6.7 (serial data to gps) to output
GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
101,13 → 205,17
GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt3 ;
GPIO_Init(GPIO6, &GPIO_InitStructure);
}
//-----------------------------------------------
 
/********************************************************/
/* Connect RXD & TXD to Compass */
/********************************************************/
void Connect_UART0_to_Compass(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
 
SCU_APBPeriphClockConfig(__GPIO5, ENABLE);
// GPS off
// unmap UART0 from GPS
// set port pin 6.6 (serial data from gps) to input and disconnect from IP
GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
114,7 → 222,7
GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Disable;
GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1 ;
GPIO_Init(GPIO6, &GPIO_InitStructure);
 
// set port pin 6.7 (serial data to gps) to input
GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
122,6 → 230,7
GPIO_Init(GPIO6, &GPIO_InitStructure);
 
// map UART0 to Compass
// set port pin 5.1 (serial data from compass) to input and connect to IP
GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
128,7 → 237,7
GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;
GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1 ;
GPIO_Init(GPIO5, &GPIO_InitStructure);
 
// set port pin 5.0 (serial data to compass) to output
GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
136,8 → 245,9
GPIO_Init(GPIO5, &GPIO_InitStructure);
}
 
 
// --------------------------- UART0 Init ------------------------------------
/********************************************************/
/* Initialize UART0 */
/********************************************************/
void GPS_UART0_Init (void)
{
UART_InitTypeDef UART_InitStructure;
144,121 → 254,226
 
SerialPutString("GPS init...");
SCU_APBPeriphClockConfig(__UART0, ENABLE); // Enable the UART1 Clock
SCU_APBPeriphClockConfig(__UART0, ENABLE); // Enable the UART0 Clock
Connect_UART0_to_GPS();
Connect_UART0_to_GPS(); // initialize pins to GPS
 
/* UART1 configured as follow:
- Word Length = 8 Bits
- One Stop Bit
- No parity
- BaudRate = 57600 baud
- Hardware flow control Disabled
- Receive and transmit enabled
- Receive and transmit FIFOs are Disabled
*/
UART_InitStructure.UART_WordLength = UART_WordLength_8D;
UART_InitStructure.UART_StopBits = UART_StopBits_1;
UART_InitStructure.UART_Parity = UART_Parity_No ;
UART_InitStructure.UART_BaudRate = BAUD_RATE;
UART_InitStructure. UART_HardwareFlowControl = UART_HardwareFlowControl_None;
UART_InitStructure.UART_HardwareFlowControl = UART_HardwareFlowControl_None;
UART_InitStructure.UART_Mode = UART_Mode_Tx_Rx;
//UART_InitStructure.UART_FIFO = UART_FIFO_Disable;
UART_InitStructure.UART_FIFO = UART_FIFO_Enable;
UART_InitStructure.UART_TxFIFOLevel = UART_FIFOLevel_1_2;
UART_InitStructure.UART_RxFIFOLevel = UART_FIFOLevel_1_2;
UART_InitStructure.UART_TxFIFOLevel = UART_FIFOLevel_1_2;
UART_InitStructure.UART_RxFIFOLevel = UART_FIFOLevel_1_2;
 
UART_DeInit(UART0);
UART_Init(UART0, &UART_InitStructure);
UART_DeInit(UART0); // reset uart 0 to default
UART_Init(UART0, &UART_InitStructure); // initialize uart 0
 
VIC_Config(UART0_ITLine, VIC_IRQ, 10);
VIC_ITCmd(UART0_ITLine, ENABLE);
// mark msg buffers invalid
UbxSol.Status = INVALID;
UbxPosLlh.Status = INVALID;
UbxPosUtm.Status = INVALID;
UbxVelNed.Status = INVALID;
GPS_Data.Status = INVALID;
 
// enable uart 0 interrupts selective
UART_ITConfig(UART0, UART_IT_Receive | UART_IT_ReceiveTimeOut, ENABLE);
UART_Cmd(UART0, ENABLE);
UART_Cmd(UART0, ENABLE); // enable uart 0
// configure the uart 0 interupt line as an IRQ with priority 10 (0 is highest)
VIC_Config(UART0_ITLine, VIC_IRQ, 10);
// enable the uart 0 IRQ
VIC_ITCmd(UART0_ITLine, ENABLE);
 
GPS_Init();
 
SerialPutString("ok\n\r");
}
 
// ----------------------------- GPS - Receive ------------------------------------
/********************************************************/
/* UART0 Interrupt Handler */
/********************************************************/
void Update_GPS_Data (void)
{
static u32 lasttime;
 
LED_GRN_TOGGLE;
// update GPS data only if the taus is INVALID or PROCESSED
if(GPS_Data.Status != NEWDATA)
{ // wait for new data at all neccesary ubx messages
if ((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA) && (UbxPosUtm.Status == NEWDATA))
{
// NAV SOL
GPS_Data.Flags = UbxSol.Flags;
GPS_Data.NumOfSats = UbxSol.numSV;
GPS_Data.SatFix = UbxSol.GPSfix;
GPS_Data.Position_Accuracy = UbxSol.PAcc;
GPS_Data.Speed_Accuracy = UbxSol.SAcc;
UbxSol.Status = PROCESSED; // ready for new data
// NAV POSLLH
GPS_Data.UpdateTime = UbxPosLlh.ITOW - lasttime;
lasttime = UbxPosLlh.ITOW;
GPS_Data.Longitude = UbxPosLlh.LON;
GPS_Data.Latitude = UbxPosLlh.LAT;
GPS_Data.Altitude = UbxPosLlh.HEIGHT;
UbxPosLlh.Status = PROCESSED; // ready for new data
// NAV VELNED
GPS_Data.Speed_East = UbxVelNed.VEL_E;
GPS_Data.Speed_North = UbxVelNed.VEL_N;
GPS_Data.Speed_Top = -UbxVelNed.VEL_D;
GPS_Data.Speed_Ground = UbxVelNed.GSpeed;
UbxVelNed.Status = PROCESSED; // ready for new data
 
// NAV POSUTM (unused)
UbxPosUtm.Status = PROCESSED; // ready for new data
 
GPS_Data.Status = NEWDATA; // new data available
}
}
}
 
 
/********************************************************/
/* UART0 Interrupt Handler */
/********************************************************/
void UART0_IRQHandler(void)
{
u8 sioTmp = 0;
static u8 uartState = 0, gps_buf_ptr = 0;
u8 c = 0;
static ubxState_t ubxState = UBXSTATE_IDLE;
static u16 msglen;
static u8 cka, ckb;
static u8 *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered
// if receive irq or receive timeout irq has occured
if((UART_GetITStatus(UART0, UART_IT_Receive) != RESET) || (UART_GetITStatus(UART0, UART_IT_ReceiveTimeOut) != RESET) )
{
UART_ClearITPendingBit(UART0, UART_IT_Receive); // clear receive interrupt flag
UART_ClearITPendingBit(UART0, UART_IT_ReceiveTimeOut); // clear receive timeout interrupt flag
// repeat until no byte is in the RxFIFO
while (UART_GetFlagStatus(UART0, UART_FLAG_RxFIFOEmpty) != SET)
{
c = UART_ReceiveData(UART0); // read byte from RxFIFO
//state machine
switch (ubxState) // ubx message parser
{
case UBXSTATE_IDLE: // check 1st sync byte
if (c == UBX_SYNC1_CHAR) ubxState = UBXSTATE_SYNC1;
else ubxState = UBXSTATE_IDLE; // out of synchronization
break;
 
GPIO_ToggleBit(GPIO5, GPIO_Pin_6);
if (DebugUART == UART0)
{
while (UART_GetFlagStatus(UART0, UART_FLAG_RxFIFOEmpty) != SET)
UART_SendData(UART1, UART_ReceiveData(UART0));
case UBXSTATE_SYNC1: // check 2nd sync byte
if (c == UBX_SYNC2_CHAR) ubxState = UBXSTATE_SYNC2;
else ubxState = UBXSTATE_IDLE; // out of synchronization
break;
case UBXSTATE_SYNC2: // check msg class to be NAV
if (c == UBX_CLASS_NAV) ubxState = UBXSTATE_CLASS;
else ubxState = UBXSTATE_IDLE; // unsupported message class
break;
case UBXSTATE_CLASS: // check message identifier
switch(c)
{
case UBX_ID_POSLLH: // geodetic position
ubxP = (u8 *)&UbxPosLlh; // data start pointer
ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer
ubxSp = (u8 *)&UbxPosLlh.Status; // status pointer
break;
 
UART_ClearITPendingBit(UART0, UART_IT_Receive);
UART_ClearITPendingBit(UART0, UART_IT_ReceiveTimeOut);
case UBX_ID_POSUTM: // geodetic position
ubxP = (u8 *)&UbxPosUtm; // data start pointer
ubxEp = (u8 *)(&UbxPosUtm + 1); // data end pointer
ubxSp = (u8 *)&UbxPosUtm.Status; // status pointer
break;
 
}
else
if((UART_GetITStatus(UART0, UART_IT_Receive) != RESET)|| (UART_GetITStatus(UART0, UART_IT_ReceiveTimeOut) != RESET) )
{
UART_ClearITPendingBit(UART0, UART_IT_Receive);
UART_ClearITPendingBit(UART0, UART_IT_ReceiveTimeOut);
case UBX_ID_SOL: // navigation solution
ubxP = (u8 *)&UbxSol; // data start pointer
ubxEp = (u8 *)(&UbxSol + 1); // data end pointer
ubxSp = (u8 *)&UbxSol.Status; // status pointer
break;
 
while ((UART_GetFlagStatus(UART0, UART_FLAG_RxFIFOEmpty) != SET) && NewGPSDataAvail == 0)
{
case UBX_ID_VELNED: // velocity vector in tangent plane
ubxP = (u8 *)&UbxVelNed; // data start pointer
ubxEp = (u8 *)(&UbxVelNed + 1); // data end pointer
ubxSp = (u8 *)&UbxVelNed.Status; // status pointer
break;
 
sioTmp = UART_ReceiveData(UART0);
default: // unsupported identifier
ubxState = UBXSTATE_IDLE;
break;
}
if (ubxState != UBXSTATE_IDLE)
{
ubxState = UBXSTATE_LEN1;
cka = UBX_CLASS_NAV + c;
ckb = UBX_CLASS_NAV + cka;
}
break;
 
case UBXSTATE_LEN1: // 1st message length byte
msglen = c;
cka += c;
ckb += cka;
ubxState = UBXSTATE_LEN2;
break;
case UBXSTATE_LEN2: // 2nd message length byte
msglen += ((u16)c)<<8;
cka += c;
ckb += cka;
// if the old data are not processed so far then break parsing now
// to avoid writing new data in ISR during reading by another function
if ( *ubxSp == NEWDATA ) ubxState = UBXSTATE_IDLE;
else // data invalid or allready processd
{
*ubxSp = INVALID; // mark invalid during buffer filling
ubxState = UBXSTATE_DATA;
}
break;
case UBXSTATE_DATA: // collecting data
if (ubxP < ubxEp) *ubxP++ = c; // copy curent data byte if any space is left
cka += c;
ckb += cka;
if (--msglen == 0) ubxState = UBXSTATE_CKA; // switch to next state if all data was read
break;
case UBXSTATE_CKA:
if (c == cka) ubxState = UBXSTATE_CKB;
else
{
*ubxSp = INVALID;
ubxState = UBXSTATE_IDLE;
}
break;
case UBXSTATE_CKB:
if (c == ckb)
{
*ubxSp = NEWDATA; // new data are valid
LED_RED_TOGGLE;
Update_GPS_Data(); //update GPS info respectively
}
else
{ // if checksum not match then set data invalid
*ubxSp = INVALID;
}
ubxState = UBXSTATE_IDLE; // ready to parse new data
break;
default: // unknown ubx state
ubxState = UBXSTATE_IDLE;
break;
 
switch (uartState)
{
case 0: if(sioTmp == 0xB5) { GPS_RxBuffer[4]=0; uartState = 1; } // GPS-UBX Startzeichen
gps_buf_ptr = 0;
GPS_RxBuffer[gps_buf_ptr++] = sioTmp;
break;
case 1:
if(gps_buf_ptr == 1 && sioTmp != 0x62) { uartState = 0; break;}; // andere Meldung unterdrücken
if(gps_buf_ptr == 2 && sioTmp != 0x01) { uartState = 0; break;}; // andere Meldung unterdrücken
if(gps_buf_ptr == 3 && sioTmp != 0x02 && sioTmp != 0x06 && sioTmp != 0x12 && sioTmp != 0x08) { uartState = 0; break;}; // andere Meldung unterdrücken
GPS_RxBuffer[gps_buf_ptr] = sioTmp;
if(gps_buf_ptr < (GPS_RxBuffer[4]+6)) gps_buf_ptr++;
else
{
if(GPS_RxBuffer[3] == 0x06) // NAV SOL
{
GPS_DataSol = (struct str_ubx_nav_sol *) GPS_RxBuffer;
GPS_Data.Flags = GPS_DataSol->Flags;
GPS_Data.Used_Sat = GPS_DataSol->Number_SV;
GPS_Data.GPS_week = GPS_DataSol->GPS_week;
}
else if(GPS_RxBuffer[3] == 0x12) //NAV VELNED
{
GPS_DataVelned = (struct str_ubx_nav_velned *) GPS_RxBuffer;
GPS_Data.N_Speed = GPS_DataVelned->N_Speed;
GPS_Data.E_Speed = GPS_DataVelned->E_Speed;
GPS_Data.DownSpeed = GPS_DataVelned->DownSpeed;
 
GPS_Data.SpeedAccuracy = GPS_DataVelned->SpeedAccuracy;
NewGPSDataAvail = 1;
}
else if(GPS_RxBuffer[3] == 0x02) // NAV POSLLH
{
GPS_DataPosllh = (struct str_ubx_nav_posllh *) GPS_RxBuffer;
GPS_Data.Longitude = GPS_DataPosllh->Longitude;
GPS_Data.Latitude = GPS_DataPosllh->Latitude;
GPS_Data.Height = GPS_DataPosllh->Height;
GPS_Data.HeightSL = GPS_DataPosllh->HeightSL;
GPS_Data.GPS_time = GPS_DataPosllh->GPS_time;
GPS_Data.HorizontalAccuracy = GPS_DataPosllh->HorizontalAccuracy;
GPS_Data.VerticalAccuracy = GPS_DataPosllh->VerticalAccuracy;
}
else if(GPS_RxBuffer[3] == 0x08) // NAV POSUTM
{
GPS_DataPosutm = (struct str_ubx_nav_posutm *) GPS_RxBuffer;
GPS_Data.North = GPS_DataPosutm->North;
GPS_Data.East = GPS_DataPosutm->East;
GPS_Data.Altitude = GPS_DataPosutm->Altitude;
}
uartState = 0;
}
break;
}
}
}
}
}
}
}
/branches/V0.1 killagreg/GPSUart.h
1,130 → 1,50
#ifndef __GPSUART_H
#define __GPSUART_H
 
struct str_ubx_nav_sol
{
u16 Header; // 0xB5 0x62
u16 ID; // 0x01 0x06
u16 Length; // 58 Bytes (+ Header - ohne ChkSum) Total: 60 Bytes
// ----- start of data -----
u32 GPS_time;
s32 Time2;
s16 GPS_week;
u8 Nav_mode; // 0x00=no fix, 0x03=3d fix
u8 Flags; // 0x01=GPS_ok
s32 X_Pos; // in cm (ECEF frame)
s32 Y_Pos;
s32 Z_Pos;
u32 Pos_Accuracy;
s32 X_Speed; // in cm/s
s32 Y_Speed;
s32 Z_Speed;
u32 Speed_Accuracy;
u16 PDOP; // Scaling: 0.01
u8 Reserved1;
u8 Number_SV;
u32 Reserved2;
// ------ end of data -------
u8 CK_A;
u8 CK_B;
} __attribute__((packed));
// Satfix types for GPS_Data.SatFix
#define SATFIX_NONE 0x00
#define SATFIX_DEADRECKOING 0x01
#define SATFIX_2D 0x02
#define SATFIX_3D 0x03
#define SATFIX_GPS_DEADRECKOING 0x04
#define SATFIX_TIMEONLY 0x05
// Flags for interpretation of the GPS_Data.Flags
#define FLAG_GPSfixOK 0x01 // (i.e. within DOP & ACC Masks)
#define FLAG_DiffSoln 0x02 // (is DGPS used)
#define FLAG_WKNSET 0x04 // (is Week Number valid)
#define FLAG_TOWSET 0x08 // (is Time of Week valid)
 
typedef enum
{
INVALID,
NEWDATA,
PROCESSED
} Status_t;
 
struct str_ubx_nav_velned
typedef struct
{
u16 Header; // 0xB5 0x62
u16 ID; // 0x01 0x12
u16 Length; // 42 Bytes (+ Header - ohne ChkSum) Total: 44 Bytes
// ----- start of data -----
u32 GPS_time;
s32 N_Speed;
s32 E_Speed;
s32 DownSpeed;
u32 Speed3D;
u32 GroundSpeed;
s32 Heading;
u32 SpeedAccuracy;
u32 HeadingAccuracy;
// ------ end of data -------
u8 CK_A;
u8 CK_B;
} __attribute__((packed));
u8 Flags; // Status Flags
u8 NumOfSats; // number of satelites
u8 SatFix; // type of satfix
s32 Longitude; // in 1e-07 deg
s32 Latitude; // in 1e-07 deg
s32 Altitude; // in mm
u32 Position_Accuracy; // in cm 3d position accuracy
s32 Speed_North; // in cm/s
s32 Speed_East; // in cm/s
s32 Speed_Top; // in cm/s
u32 Speed_Ground; // 2D ground speed in cm/s
u32 Speed_Accuracy; // in cm/s 3d velocity accuracy
u32 UpdateTime; // in ms is the time since the last data frame was received (typical 200 ms)
Status_t Status; // status of data
} __attribute__((packed)) gps_data_t;
 
struct str_ubx_nav_posllh
{
u16 Header; // 0xB5 0x62
u16 ID; // 0x01 0x02
u16 Length; // 42 Bytes (+ Header - ohne ChkSum) Total: 44 Bytes
// ----- start of data -----
u32 GPS_time;
s32 Longitude; // 1e-07 deg Longitude
s32 Latitude; // 1e-07 deg Latitude
s32 Height; // mm Height
s32 HeightSL; // mm Height above mean sea level
u32 HorizontalAccuracy; // mm Horizontal Accuracy Estimate
u32 VerticalAccuracy; // mm Vertical Accuracy Estimate
// ------ end of data -------
u8 CK_A;
u8 CK_B;
} __attribute__((packed));
// The data are valid if the GPS_Data.Status is NEWDATA or PROCESSED.
// To achieve new data after reading the GPS_Data.Status should be set to PROCESSED.
extern gps_data_t GPS_Data;
 
struct str_ubx_nav_posutm
{
u16 Header; // 0xB5 0x62
u16 ID; // 0x01 0x08
u16 Length; // 18 Bytes (+ Header - ohne ChkSum)
// ----- start of data -----
u32 GPS_time;
s32 East; // in cm
s32 North; // in cm
s32 Altitude; // in cm
s16 UTM_ZoneNumber;
s16 HemisphereSector; // 0 = North, 1 = South
// ------ end of data -------
u8 CK_A;
u8 CK_B;
} __attribute__((packed));
 
 
struct str_gps_nav_data
{
s32 Longitude;
s32 Latitude;
s32 TargetLongitude;
s32 TargetLatitude;
s32 HomeLongitude;
s32 HomeLatitude;
s32 North2Target;
s32 East2Target;
s32 Distance2Target;
s32 Angle2Target;
s32 Height;
s32 HeightSL;
s32 DownSpeed;
u32 GPS_time;
u16 GPS_week;
s32 North;
s32 East;
s32 N_Speed;
s32 E_Speed;
s32 SpeedAccuracy;
u32 HorizontalAccuracy;
u32 VerticalAccuracy;
u8 Flags; // 0x01=GPS_ok
s32 DataPerSecond;
s32 Altitude;
u8 Used_Sat;
} __attribute__((packed));
 
 
#define GPS_RX_SIZE (sizeof(struct str_ubx_nav_velned) + sizeof(struct str_ubx_nav_posutm) + sizeof(struct str_ubx_nav_sol) + sizeof(struct str_ubx_nav_posllh) + 10)
 
// ----------------------------------------------------------------
extern u8 NewGPSDataAvail;
extern u8 NewPCTargetGPSPosition;
extern struct str_gps_nav_data GPS_Data;
 
// initialize the UART to the UBLOX module.
extern void GPS_UART0_Init (void);
 
#endif
#endif //__GPSUART_H
 
/branches/V0.1 killagreg/Navi-Ctrl.Uv2
31,22 → 31,24
File 1,1,<.\usb_istr.c><usb_istr.c>
File 1,1,<.\usb_prop.c><usb_prop.c>
File 1,1,<.\usb_pwr.c><usb_pwr.c>
File 1,1,<.\led.c><led.c>
File 2,5,<.\ramfunc.h><ramfunc.h>
File 2,5,<.\main.h><main.h>
File 2,5,<.\uart.h><uart.h>
File 2,5,<.\menu.h><menu.h>
File 2,5,<.\printf_P.h><printf_P.h>
File 2,5,<.\91x_conf.h><91x_conf.h>
File 2,5,<.\settings.h><settings.h>
File 2,5,<.\timer.h><timer.h>
File 2,5,<.\usb.h><usb.h>
File 2,5,<.\spi_slave.h><spi_slave.h>
File 2,5,<.\GPSUart.h><GPSUart.h>
File 2,5,<.\i2c.h><i2c.h>
File 2,5,<.\sdc.h><sdc.h>
File 2,5,<.\ssc.h><ssc.h>
File 2,5,<.\fat16.h><fat16.h>
File 2,5,<.\GPS.h><GPS.h>
File 2,5,<.\GPSUart.h><GPSUart.h>
File 2,5,<.\settings.h><settings.h>
File 2,5,<.\libstr91x\include\91x_lib.h><91x_lib.h>
File 2,5,<.\led.h><led.h>
File 3,2,<.\startup912.s><startup912.s>
File 4,1,<.\libstr91x\src\91x_scu.c><91x_scu.c>
File 4,1,<.\libstr91x\src\91x_gpio.c><91x_gpio.c>
69,13 → 71,13
 
 
Options 1,0,0 // Target 'Navi-Ctrl'
Device (STR910FM32)
Device (STR911FM44)
Vendor (STMicroelectronics)
Cpu (IRAM(0x04000000-0x0400FFFF) IROM(0x0-0x3FFFF) IROM2(0x400000-0x407FFF) CLOCK(25000000) CPUTYPE(ARM9E))
Cpu (IRAM(0x04000000-0x04017FFF) IROM(0x0-0x7FFFF) IROM2(0x400000-0x407FFF) CLOCK(25000000) CPUTYPE(ARM9E))
FlashUt ()
StupF ("STARTUP\ST\STR91x.s" ("STR91x Startup Code"))
FlashDR (UL2ARM(-UU0639C7E -O15 -S0 -C1 -N00("ST uPSD Flash") -D00(04570041) -L00(8) -N01("ARM966E-S Core") -D01(25966041) -L01(4) -N02("ST Boundary Scan") -D02(0457F041) -L02(5) -FO15 -FD4000000 -FC800 -FN2 -FF0STR91xFxx2 -FS00 -FL040000 -FF1STR91xB1 -FS1400000 -FL18000))
DevID (4118)
FlashDR (UL2ARM(-UU0639C7E -O15 -S0 -C1 -N00("ST uPSD Flash") -D00(04570041) -L00(8) -N01("ARM966E-S Core") -D01(25966041) -L01(4) -N02("ST Boundary Scan") -D02(0457F041) -L02(5) -FO15 -FD4000000 -FC800 -FN2 -FF0STR91xFxx4 -FS00 -FL080000 -FF1STR91xB1 -FS1400000 -FL18000))
DevID (4069)
Rgf (91x_lib.H)
Mem ()
C ()
94,7 → 96,7
OrgReg (ÿST\91x\)
TgStat=16
OutDir (.\Obj\)
OutName (Navi-Ctrl_STR9)
OutName (Navi-Ctrl_STR9_killagreg)
GenApp=1
GenLib=0
GenHex=1
115,8 → 117,8
GCPUTYP (ARM9E)
TFlagsA { 0,12,32,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 }
OCMARM { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 }
OCMARAM { 0,0,0,0,4,0,0,1,0 }
OCMAROM { 1,0,0,0,0,0,0,4,0 }
OCMARAM { 0,0,0,0,4,0,128,1,0 }
OCMAROM { 1,0,0,0,0,0,0,8,0 }
OCMXRAM { 0,0,0,0,0,0,0,0,0 }
OCMIRAM2 { 0,0,0,48,0,0,16,0,0 }
OCMIROM2 { 1,0,0,64,0,0,128,0,0 }
147,11 → 149,11
ALDICDR ()
ALDMISC ()
ALDSCAT (.\scripts\flash_str9.ld)
OPTDL (SARM.DLL)(-cSTR91x)(DARMST9.DLL)(-pSTR910)(SARM.DLL)(-cSTR91x)(TARMST9.DLL)(-pSTR910)
OPTDL (SARM.DLL)(-cSTR91x)(DARMST9.DLL)(-pSTR911)(SARM.DLL)(-cSTR91x)(TARMST9.DLL)(-pSTR911)
OPTDBG 48126,0,()()()()()()()()()() (BIN\UL2ARM.DLL)()()()
FLASH1 { 9,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 }
FLASH1 { 9,0,0,0,1,0,0,0,0,16,0,0,0,0,0,0,0,0,0,0 }
FLASH2 (BIN\UL2ARM.DLL)
FLASH3 ()
FLASH3 ("" ())
FLASH4 ()
EndOpt
 
/branches/V0.1 killagreg/fat16.c
54,8 → 54,11
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <stdio.h>
#include "91x_lib.h"
#include "fat16.h"
#include "sdc.h"
 
#include "main.h"
 
//________________________________________________________________________________________________________________________________________
// Module name: fat16.c
/branches/V0.1 killagreg/i2c.c
54,55 → 54,66
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "91x_lib.h"
#include "i2c.h"
#include "uart.h"
#include "timer.h"
#include "main.h"
 
u8 I2C_RxBufferSize, I2C_TxBufferSize;
u8 *I2C_TxBuffer, *I2C_RxBuffer;
u8 Tx_Idx=0, Rx_Idx=0, I2C_Direction;
// rxbuffer
u8 I2C_RxBufferSize;
u8 *I2C_RxBuffer;
u8 Rx_Idx = 0;
// txbuffer
u8 I2C_TxBufferSize;
u8 *I2C_TxBuffer;
u8 Tx_Idx = 0;
 
u8 I2C_Direction;
u8 I2C_Command;
 
struct str_I2C_Heading I2C_Heading;
struct str_I2C_WriteNickRoll I2C_WriteNickRoll;
struct str_I2C_Mag I2C_Mag;
struct str_I2C_EEPROM I2C_ReadEEPROM, I2C_WriteEEPROM;
struct str_I2C_Version I2C_Version;
struct str_I2C_WriteCal I2C_WriteCal;
I2C_Heading_t I2C_Heading;
I2C_WriteAttitude_t I2C_WriteAttitude;
I2C_Mag_t I2C_Mag;
I2C_EEPROM_t I2C_ReadEEPROM, I2C_WriteEEPROM;
I2C_Version_t I2C_Version;
I2C_WriteCal_t I2C_WriteCal;
 
u8 CompassUpdateActiv = 0;
u8 CompassCalState = 0;
 
volatile u8 I2C_ReadRequest = 0;
 
//--------------------------------------------------------------
void I2C1_Init(void)
{
I2C_InitTypeDef I2C_Struct;
GPIO_InitTypeDef GPIO_Struct;
I2C_InitTypeDef I2C_Struct;
GPIO_InitTypeDef GPIO_Struct;
SerialPutString("I2C init...");
SCU_APBPeriphClockConfig(__I2C1,ENABLE);
I2C_DeInit(I2C1);
SerialPutString("I2C init...");
SCU_APBPeriphClockConfig(__I2C1,ENABLE);
I2C_DeInit(I2C1);
 
GPIO_Struct.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3;
GPIO_Struct.GPIO_Type = GPIO_Type_OpenCollector;
GPIO_Struct.GPIO_IPConnected = GPIO_IPConnected_Enable;
GPIO_Struct.GPIO_Alternate=GPIO_OutputAlt2;
GPIO_Init(GPIO2, &GPIO_Struct);
GPIO_Struct.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3;
GPIO_Struct.GPIO_Type = GPIO_Type_OpenCollector;
GPIO_Struct.GPIO_IPConnected = GPIO_IPConnected_Enable;
GPIO_Struct.GPIO_Alternate=GPIO_OutputAlt2;
GPIO_Init(GPIO2, &GPIO_Struct);
 
I2C_Struct.I2C_GeneralCall = I2C_GeneralCall_Disable;
I2C_Struct.I2C_Ack = I2C_Ack_Enable;
I2C_Struct.I2C_CLKSpeed = 20000;
I2C_Struct.I2C_OwnAddress = I2C_SLAVE_ADDRESS;
I2C_Init(I2C1, &I2C_Struct);
I2C_Struct.I2C_GeneralCall = I2C_GeneralCall_Disable;
I2C_Struct.I2C_Ack = I2C_Ack_Enable;
I2C_Struct.I2C_CLKSpeed = 20000;
I2C_Struct.I2C_OwnAddress = I2C_SLAVE_ADDRESS;
I2C_Init(I2C1, &I2C_Struct);
I2C_Cmd(I2C1, ENABLE);
I2C_ITConfig(I2C1, ENABLE);
I2C_Cmd(I2C1, ENABLE);
I2C_ITConfig(I2C1, ENABLE);
VIC_Config(I2C1_ITLine,VIC_IRQ , 8);
VIC_ITCmd(I2C1_ITLine, ENABLE);
VIC_Config(I2C1_ITLine, VIC_IRQ , 8);
VIC_ITCmd(I2C1_ITLine, ENABLE);
 
I2C_Heading.Heading = 0;
SerialPutString("ok\n\r");
I2C_Heading.Heading = -1;
SerialPutString("ok\n\r");
}
 
//--------------------------------------------------------------
109,66 → 120,63
void I2C1_IRQHandler(void)
{
switch (I2C_GetLastEvent(I2C1))
{
case I2C_EVENT_MASTER_MODE_SELECT: // EV5
Tx_Idx = 0;
Rx_Idx = 0;
I2C_Send7bitAddress(I2C1, I2C_SLAVE_ADDRESS, I2C_Direction);
// DebugOut.Analog[16]++;
break;
switch (I2C_GetLastEvent(I2C1))
{
case I2C_EVENT_MASTER_MODE_SELECT: // EV5
Tx_Idx = 0;
Rx_Idx = 0;
I2C_Send7bitAddress(I2C1, I2C_SLAVE_ADDRESS, I2C_Direction);
break;
 
case I2C_EVENT_MASTER_MODE_SELECTED: // EV6
// Clear EV6 by set again the PE bit
I2C1->CR |= 0x20;
if (I2C_Direction == I2C_MODE_TRANSMITTER)
{
I2C_SendData(I2C1, I2C_Command);//EV8 just after EV6
}
break;
case I2C_EVENT_MASTER_MODE_SELECTED: // EV6
// Clear EV6 by set again the PE bit
I2C1->CR |= 0x20;
if (I2C_Direction == I2C_MODE_TRANSMITTER)
{
I2C_SendData(I2C1, I2C_Command); //EV8 just after EV6
}
break;
 
case I2C_EVENT_MASTER_BYTE_TRANSMITTED: // EV8
// DebugOut.Analog[17]++;
if ( Tx_Idx >= I2C_TxBufferSize )
{
I2C_GenerateSTOP (I2C1, ENABLE);
Tx_Idx = 0;
if (I2C_RxBufferSize > 0) // wird Antwort erwartet ?
{
I2C_ReadRequest = 1;
TimerI2CReadDelay = SetDelay(10);
}
}
else
{
I2C_SendData(I2C1, I2C_TxBuffer[Tx_Idx]);
Tx_Idx++;
}
break;
case I2C_EVENT_MASTER_BYTE_TRANSMITTED: // EV8
if ( Tx_Idx >= I2C_TxBufferSize )
{
I2C_GenerateSTOP (I2C1, ENABLE);
Tx_Idx = 0;
if (I2C_RxBufferSize > 0) // is answer expected?
{
I2C_ReadRequest = 1;
TimerI2CReadDelay = SetDelay(10);
}
}
else
{
I2C_SendData(I2C1, I2C_TxBuffer[Tx_Idx]);
Tx_Idx++;
}
break;
case I2C_EVENT_MASTER_BYTE_RECEIVED: // EV7
DebugOut.Analog[16]++;
//if (Rx_Idx == 1) DebugOut.Analog[19] = I2C_RxBuffer[0];
if (Rx_Idx < I2C_RxBufferSize)
{ I2C_RxBuffer[Rx_Idx] = I2C_ReceiveData(I2C1);
} else I2C_ReceiveData(I2C1);
Rx_Idx++;
if ( Rx_Idx == I2C_RxBufferSize-2 )
{
I2C_AcknowledgeConfig (I2C1, DISABLE);
}
if ( Rx_Idx == I2C_RxBufferSize -1 )
{
I2C_GenerateSTOP(I2C1, ENABLE);
if (I2C_Command == I2C_CMD_READ_HEADING) CompassUpdateActiv = 0;
}
break;
case I2C_EVENT_MASTER_BYTE_RECEIVED: // EV7
DebugOut.Analog[16]++;
if (Rx_Idx < I2C_RxBufferSize)
{
I2C_RxBuffer[Rx_Idx] = I2C_ReceiveData(I2C1);
}
else I2C_ReceiveData(I2C1);
Rx_Idx++;
if ( Rx_Idx == I2C_RxBufferSize-2 )
{
I2C_AcknowledgeConfig (I2C1, DISABLE);
}
if ( Rx_Idx == I2C_RxBufferSize -1 )
{
I2C_GenerateSTOP(I2C1, ENABLE);
if (I2C_Command == I2C_CMD_READ_HEADING) CompassUpdateActiv = 0;
}
break;
 
default:
break;
}
default:
break;
}
}
//----------------------------------------------------------------
void SendI2C_Command(u8 command)
175,9 → 183,8
{
I2C_Command = command;
//I2C_GenerateSTOP(I2C1, ENABLE);
 
switch (command)
{
switch (command)
{
case I2C_CMD_VERSION:
I2C_RxBuffer = (u8 *)&I2C_Version;
I2C_RxBufferSize = sizeof(I2C_Version);
195,7 → 202,7
I2C_TxBufferSize = sizeof(I2C_WriteCal);
break;
case I2C_CMD_READ_EEPROM:
I2C_RxBuffer = (u8 *)&I2C_ReadEEPROM.Inhalt;
I2C_RxBuffer = (u8 *)&I2C_ReadEEPROM.Content;
I2C_RxBufferSize = 2;
I2C_TxBuffer = (u8 *)&I2C_ReadEEPROM;
I2C_TxBufferSize = 1;
209,20 → 216,19
CompassUpdateActiv = 1;
I2C_RxBuffer = (u8 *)&I2C_Heading;
I2C_RxBufferSize = sizeof(I2C_Heading);
I2C_TxBuffer = (u8 *)&I2C_WriteNickRoll;
I2C_TxBufferSize = sizeof(I2C_WriteNickRoll);
I2C_TxBuffer = (u8 *)&I2C_WriteAttitude;
I2C_TxBufferSize = sizeof(I2C_WriteAttitude);
break;
 
}
if (I2C_RxBufferSize > 0)
{
I2C_RxBufferSize++;
if (I2C_RxBufferSize < 3) I2C_RxBufferSize = 3;
}
 
I2C_AcknowledgeConfig (I2C1, ENABLE);
I2C_Direction = I2C_MODE_TRANSMITTER;
I2C_GenerateStart(I2C1, ENABLE);
 
}
if (I2C_RxBufferSize > 0)
{
I2C_RxBufferSize++;
if (I2C_RxBufferSize < 3) I2C_RxBufferSize = 3;
}
I2C_AcknowledgeConfig (I2C1, ENABLE);
I2C_Direction = I2C_MODE_TRANSMITTER;
I2C_GenerateStart(I2C1, ENABLE);
}
/branches/V0.1 killagreg/i2c.h
1,62 → 1,69
#ifndef __I2C
#define __I2C
#ifndef __I2C_H
#define __I2C_H
 
 
#define I2C_SLAVE_ADDRESS 0x50
#define I2C_SLAVE_ADDRESS 0x50
 
#define I2C_CMD_VERSION 0x01
struct str_I2C_Version
#define I2C_CMD_VERSION 0x01
#define I2C_CMD_READ_MAG 0x02
#define I2C_CMD_READ_HEADING 0x03
#define I2C_CMD_WRITE_CAL 0x04
#define I2C_CMD_WRITE_EEPROM 0x0A
#define I2C_CMD_READ_EEPROM 0x0B
 
 
typedef struct
{
u8 Hauptversion;
u8 Nebenversion;
u8 Comp;
} __attribute__((packed));
u8 Major;
u8 Minor;
u8 Patch;
} __attribute__((packed)) I2C_Version_t;
 
#define I2C_CMD_WRITE_EEPROM 0x0A
#define I2C_CMD_READ_EEPROM 0x0B
struct str_I2C_EEPROM
 
typedef struct
{
u8 Adresse;
u16 Inhalt;
} __attribute__((packed));
u8 Address;
u16 Content;
} __attribute__((packed)) I2C_EEPROM_t;
 
#define I2C_CMD_READ_MAG 0x02
struct str_I2C_Mag
 
typedef struct
{
u16 MagX;
u16 MagY;
u16 MagZ;
} __attribute__((packed));
} __attribute__((packed)) I2C_Mag_t;
 
#define I2C_CMD_READ_HEADING 0x03
struct str_I2C_WriteNickRoll
 
typedef struct
{
s16 Nick;
s16 Pitch;
s16 Roll;
} __attribute__((packed));
} __attribute__((packed)) I2C_WriteAttitude_t;
 
#define I2C_CMD_WRITE_CAL 0x04
struct str_I2C_WriteCal
 
typedef struct
{
u8 CalByte;
u8 Dummy1;
u8 Dummy2;
} __attribute__((packed));
} __attribute__((packed)) I2C_WriteCal_t;
 
struct str_I2C_Heading
typedef struct
{
u16 Heading;
} __attribute__((packed));
} __attribute__((packed)) I2C_Heading_t;
 
 
extern u8 Tx_Idx, Rx_Idx, I2C_Direction;
 
extern struct str_I2C_Heading I2C_Heading;
extern struct str_I2C_WriteNickRoll I2C_WriteNickRoll;
extern struct str_I2C_Mag I2C_Mag;
extern struct str_I2C_EEPROM I2C_ReadEEPROM, I2C_WriteEEPROM;
extern struct str_I2C_Version I2C_Version;
extern struct str_I2C_WriteCal I2C_WriteCal;
extern I2C_Heading_t I2C_Heading;
extern I2C_WriteAttitude_t I2C_WriteAttitude;
extern I2C_Mag_t I2C_Mag;
extern I2C_EEPROM_t I2C_ReadEEPROM, I2C_WriteEEPROM;
extern I2C_Version_t I2C_Version;
extern I2C_WriteCal_t I2C_WriteCal;
 
extern void I2C1_Init(void);
extern void SendI2C_Command(u8 command);
extern u8 CompassUpdateActiv;
64,5 → 71,5
extern u8 CompassCalState;
 
 
#endif
#endif // I2C_H
 
/branches/V0.1 killagreg/interrupt.c
54,8 → 54,9
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
 
//#include "main.h"
#include "91x_lib.h"
#include "usb_lib.h"
#define global extern /* to declare external variables and functions */
 
extern void USB_Istr(void);
72,7 → 73,7
VIC1->VAR = 0XFF;
}
 
void InitInterrupt(void)
void Interrupt_Init(void)
{
VIC0->DVAR = (u32)Dummy_Handler;
VIC1->DVAR = (u32)Dummy_Handler;
/branches/V0.1 killagreg/main.c
56,7 → 56,17
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
const unsigned long _Main_Crystal = 25000;
 
#include <stdio.h>
#include "91x_lib.h"
#include "led.h"
#include "GPSUart.h"
#include "GPS.h"
#include "uart.h"
#include "i2c.h"
#include "timer.h"
#include "spi_slave.h"
#include "sdc.h"
#include "usb.h"
#include "main.h"
 
u32 TimerCompassUpdate;
65,142 → 75,92
 
u8 BeepTime;
void Init_Undef(void);
u8 Parameter_UserParam1;
u8 Parameter_UserParam2;
u8 Parameter_UserParam3;
u8 Parameter_UserParam4;
u8 Parameter_UserParam5;
u8 Parameter_UserParam6;
u8 Parameter_UserParam7;
u8 Parameter_UserParam8;
s32 FC_StickNick;
s32 FC_StickRoll;
s32 FC_StickGier;
s32 FC_StickGas;
s32 FC_Poti1;
s32 FC_Poti2;
s32 FC_Poti3;
s32 FC_Poti4;
s32 SenderOkay = 0;
u8 text[20];
 
//----------------------------------------------------------------------------------------------------
void Leds_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
Param_t Parameter;
FC_t FC;
u8 RC_Quality = 0;
 
SCU_APBPeriphClockConfig(__GPIO6, ENABLE);
SCU_APBPeriphClockConfig(__GPIO5, ENABLE);
//GPIO_DeInit(GPIO5);
GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt1 ;
GPIO_Init(GPIO5, &GPIO_InitStructure);
 
GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;
GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1;
GPIO_Init(GPIO5, &GPIO_InitStructure);
 
}
//
//----------------------------------------------------------------------------------------------------
 
void OutputStartupData(void)
{
char text[20];
{
u8 text[20];
SerialPutString("\n\r--------------\n\r");
sprintf(text,"NaviCtrl V%d.%d\n\r", VersionInfo.Hauptversion, VersionInfo.Nebenversion); SerialPutString(text);
 
sprintf(text,"NaviCtrl V%d.%d\n\r", VersionInfo.Major, VersionInfo.Minor);
SerialPutString(text);
}
 
//----------------------------------------------------------------------------------------------------
int main(void)
{
static u8 oldCompassCalState = 0;
static u8 oldCompassCalState = 0;
 
// SCU_MCLKSourceConfig(SCU_MCLK_OSC);
// SCU_PCLKDivisorConfig(SCU_PCLK_Div2);
// SCU_MCLKSourceConfig(SCU_MCLK_PLL);
/* configure PLL and set it as master clock source */
SCU_MCLKSourceConfig(SCU_MCLK_OSC); // set master clock source to external oscillator clock (25MHz) before diabling the PLL
SCU_PLLCmd(DISABLE); // now disable the PLL
SCU_RCLKDivisorConfig(SCU_RCLK_Div1); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)
SCU_HCLKDivisorConfig(SCU_HCLK_Div1); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)
SCU_PCLKDivisorConfig(SCU_PCLK_Div2); // set PCLK (APB bus clock) divisor to 2 (half Reference Clock)
SCU_PLLFactorsConfig(192,25,3); // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3
SCU_PLLCmd(ENABLE); // Enable PLL (is disabled by SCU_PLLFactorsConfig)
SCU_MCLKSourceConfig(SCU_MCLK_PLL); // set master clock source to PLL
 
SCU_MCLKSourceConfig(SCU_MCLK_OSC);
SCU_PLLCmd(DISABLE); // PLL aus
SCU_RCLKDivisorConfig(SCU_RCLK_Div1);
SCU_HCLKDivisorConfig(SCU_HCLK_Div1);
SCU_PCLKDivisorConfig(SCU_PCLK_Div2);
// SCU_FMICLKDivisorConfig(SCU_FMICLK_Div1);
// FMI_Config(FMI_READ_WAIT_STATE_3,FMI_WRITE_WAIT_STATE_0, FMI_PWD_ENABLE, FMI_LVD_ENABLE,FMI_FREQ_HIGH);
SCU_PLLFactorsConfig(192,25,3); // PLL = 48 MHz
/* Fill Version Info Structure */
VersionInfo.Major = VERSION_MAJOR;
VersionInfo.Minor = VERSION_MINOR;
VersionInfo.Patch = VERSION_PATCH;
/* init VIC (Vectored Interrupt Controller) */
SCU_AHBPeriphClockConfig(__VIC,ENABLE); // enable AHB bus clock for VIC
SCU_AHBPeriphReset(__VIC, DISABLE); // disable reset state for VIC
VIC_DeInit(); // deinitializes the VIC module registers to their default reset values.
// initialize the interrupt handler
Interrupt_Init();
// initialize the LEDs
Led_Init();
// initialize the debug UART1
UART1_Init();
OutputStartupData();
// initialize the uart to MKGPS module
GPS_UART0_Init();
GPS_Init();
 
SCU_PLLCmd(ENABLE); // PLL Enabled
 
SCU_MCLKSourceConfig(SCU_MCLK_PLL); // MCLK = PLL
 
VersionInfo.Hauptversion = VERSION_HAUPTVERSION;
VersionInfo.Nebenversion = VERSION_NEBENVERSION;
VersionInfo.PCKompatibel = VERSION_KOMPATIBEL;
SCU_AHBPeriphClockConfig(__VIC,ENABLE);
SCU_AHBPeriphReset(__VIC,DISABLE);
VIC_DeInit();
USB_ConfigInit();
SPI0_Init();
TIMER1_Init();
I2C1_Init();
I2C_Version.Major = 0xFF;
SendI2C_Command(I2C_CMD_VERSION);
TimerCompassUpdate = SetDelay(50);
while (!CheckDelay(TimerCompassUpdate));
TimerCompassUpdate = SetDelay(5);
TimerI2CReadDelay = SetDelay(5);
/*
InitFat16();
ReadSetting(1);
*/
SerialPutString("Init done\n\r.");
SerialPutString("--------------\n\n\r");
InitInterrupt();
for (;;)
{
if(rxd_buffer_locked)
{
UART1_ProcessRxData();
UART1_TransmitTxData();
}
UpdateSPI_Buffer();
SPI_CheckSlaveSelect();
UART1_Transmit(); // empty txd buffer
Leds_Init();
Debug_UART1_Init();
OutputStartupData();
 
GPS_UART0_Init();
USB_ConfigInit();
SPI0_Init();
 
TIMER1_Init();
I2C1_Init();
I2C_Version.Hauptversion = 0xff;
 
SendI2C_Command(I2C_CMD_VERSION);
 
TimerCompassUpdate = SetDelay(50);
while (!CheckDelay(TimerCompassUpdate));
TimerCompassUpdate = SetDelay(5);
TimerI2CReadDelay = SetDelay(5);
 
/*
InitFat16();
 
ReadSetting(1);
*/
SerialPutString("Init done\n\r.");
SerialPutString("--------------\n\n\r");
for (;;)
{
if(NeuerDatensatzEmpfangen)
{
BearbeiteRxDaten();
DatenUebertragung();
// if (GPIO_ReadBit(GPIO5, GPIO_Pin_3)) SerialPutString("keine SD\n\r"); else SerialPutString("SD eingesteckt!\n\r");
}
UpdateSPI_Buffer();
SPI_CheckSlaveSelect();
UART1_Transmit();
// ----------- I2C Timing -------------------------
if (CheckDelay(TimerCompassUpdate))
{
212,8 → 172,7
}
else
{
GPIO_ToggleBit(GPIO5, GPIO_Pin_6);
GPIO_ToggleBit(GPIO5, GPIO_Pin_7);
LED_GRN_TOGGLE;
TimerCompassUpdate = SetDelay(25);
SendI2C_Command(I2C_CMD_READ_HEADING);
}
233,9 → 192,9
}
 
if (CheckDelay(TimerDebugDataDelay))
{ SendGPSPosAnforderung = 1;
DatenUebertragung();
 
{
Request_GPS_Position = 1;
UART1_TransmitTxData();
TimerDebugDataDelay = SetDelay(500);
}
// ----------------------------------------------------
242,3 → 201,4
}
}
 
 
/branches/V0.1 killagreg/main.h
1,59 → 1,47
#ifndef _MAIN_H
#define _MAIN_H
 
#include "91x_conf.h"
#include "91x_lib.h"
#include "uart.h"
#include "ramfunc.h"
#include "menu.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "printf_P.h"
#include "timer.h"
#include "spi_slave.h"
#include "i2c.h"
#include "GPS.h"
#include "GPSUart.h"
#include "sdc.h"
#include "ssc.h"
#include "fat16.h"
 
#include "usb.h"
#define VERSION_MAJOR 0
#define VERSION_MINOR 1
#define VERSION_PATCH 7
 
#include "string.h"
#include "settings.h"
extern u32 TimerCompassUpdate;
extern u32 TimerI2CReadDelay;
extern u8 BeepTime;
void Interrupt_Init(void);
 
typedef struct
{
u8 User1;
u8 User2;
u8 User3;
u8 User4;
u8 User5;
u8 User6;
u8 User7;
u8 User8;
} __attribute__((packed)) Param_t;
 
typedef struct
{
s8 StickPitch;
s8 StickRoll;
s8 StickYaw;
s8 StickThrust;
u8 Poti1;
u8 Poti2;
u8 Poti3;
u8 Poti4;
} __attribute__((packed)) FC_t;
 
 
//#include "hw_config.h"
extern Param_t Parameter;
extern FC_t FC;
 
#define VERSION_HAUPTVERSION 0
#define VERSION_NEBENVERSION 1
#define VERSION_KOMPATIBEL 7
extern u8 RC_Quality;
 
#define GPIO_ToggleBit(GPIO, Pin) if (GPIO_ReadBit(GPIO, Pin)) GPIO_WriteBit(GPIO, Pin, Bit_RESET); else GPIO_WriteBit(GPIO, Pin, Bit_SET);
 
 
extern u32 TimerCompassUpdate;
extern u32 TimerI2CReadDelay;
extern u8 BeepTime;
void InitInterrupt(void);
 
extern u8 Parameter_UserParam1;
extern u8 Parameter_UserParam2;
extern u8 Parameter_UserParam3;
extern u8 Parameter_UserParam4;
extern u8 Parameter_UserParam5;
extern u8 Parameter_UserParam6;
extern u8 Parameter_UserParam7;
extern u8 Parameter_UserParam8;
extern s32 FC_StickNick;
extern s32 FC_StickRoll;
extern s32 FC_StickGier;
extern s32 FC_StickGas;
extern s32 FC_Poti1;
extern s32 FC_Poti2;
extern s32 FC_Poti3;
extern s32 FC_Poti4;
extern s32 SenderOkay;
extern u8 text[20];
#endif // _MAIN_H
/branches/V0.1 killagreg/menu.c
54,14 → 54,20
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <stdlib.h>
#include "91x_lib.h"
#include "printf_P.h"
#include "main.h"
#include "GPSUart.h"
#include "i2c.h"
#include "spi_slave.h"
#include "menu.h"
 
u16 TestInt = 0;
#define ARRAYGROESSE 10
u8 Array[ARRAYGROESSE] = {1,2,3,4,5,6,7,8,9,10};
s8 DisplayBuff[80] = "Hallo Welt";
#define ARRAYSIZE 10
u8 Array[ARRAYSIZE] = {1,2,3,4,5,6,7,8,9,10};
s8 DisplayBuff[DISPLAYBUFFSIZE] = "Hello World";
u8 DispPtr = 0;
u8 RemoteTasten = 0;
u8 RemoteButtons = 0;
 
#define KEY1 0x01
#define KEY2 0x02
69,33 → 75,132
#define KEY4 0x08
#define KEY5 0x10
 
void LcdClear(void)
/************************************/
/* Clear LCD Buffer */
/************************************/
void LCD_Clear(void)
{
u8 i;
for(i=0;i<80;i++) DisplayBuff[i] = ' ';
u8 i;
for( i = 0; i < DISPLAYBUFFSIZE; i++) DisplayBuff[i] = ' ';
}
 
void Menu(void)
{
static u8 MaxMenue = 1,MenuePunkt=0;
if(RemoteTasten & KEY1) { if(MenuePunkt) MenuePunkt--; else MenuePunkt = MaxMenue; LcdClear(); }
if(RemoteTasten & KEY2) { MenuePunkt++; LcdClear(); }
if((RemoteTasten & KEY1) && (RemoteTasten & KEY2)) MenuePunkt = 0;
LCD_printfxy(17,0,"[%i]",MenuePunkt);
switch(MenuePunkt)
{
case 0:
LCD_printfxy(0,0,"++ Navi-Ctrl ++");
LCD_printfxy(0,1,"SW:%d.%d",VERSION_HAUPTVERSION, VERSION_NEBENVERSION);
// LCD_printfxy(0,2,"Setting: %d ",GetActiveParamSetNumber());
LCD_printfxy(0,3,"(c) I.B. H.B. ");
// if(RemoteTasten & KEY3) TestInt--;
// if(RemoteTasten & KEY4) TestInt++;
break;
default: MaxMenue = MenuePunkt - 1;
MenuePunkt = 0;
break;
 
/************************************/
/* Update Menu on LCD */
/************************************/
// Display with 20 characters in 4 lines
void LCD_PrintMenu(void)
{
static u8 MaxMenuItem = 7;
static u8 MenuItem=0;
// if KEY1 is activated goto previous menu item
if(RemoteButtons & KEY1)
{
if(MenuItem) MenuItem--;
else MenuItem = MaxMenuItem;
LCD_Clear();
}
// if KEY2 is activated goto next menu item
if(RemoteButtons & KEY2)
{
if (MenuItem == MaxMenuItem) MenuItem = 0;
else MenuItem++;
LCD_Clear();
}
// if KEY1 and KEY2 is activated goto initial menu item
if((RemoteButtons & KEY1) && (RemoteButtons & KEY2)) MenuItem = 0;
// print menu item number in the upper right corner
if(MenuItem < 10)
{
LCD_printfxy(17,0,"[%i]",MenuItem);
}
else
{
LCD_printfxy(16,0,"[%i]",MenuItem);
}
 
switch(MenuItem)
{
// Version Info
case 0:
LCD_printfxy(0,0,"++ Navi-Ctrl ++");
LCD_printfxy(0,1,"SW:%d.%d",VERSION_MAJOR, VERSION_MINOR);
LCD_printfxy(0,3,"(c) I.B. H.B. ");
break;
case 1:
if (GPS_Data.Status == INVALID)
{
LCD_printfxy(0,0,"No GPS data!");
}
else // newdata or processed
{
switch (GPS_Data.SatFix)
{
case SATFIX_NONE:
LCD_printfxy(0,0,"Sats: %d Fix: None", GPS_Data.NumOfSats);
break;
case SATFIX_2D:
LCD_printfxy(0,0,"Sats: %d Fix: 2D", GPS_Data.NumOfSats);
break;
case SATFIX_3D:
LCD_printfxy(0,0,"Sats: %d Fix: 3D", GPS_Data.NumOfSats);
break;
default:
LCD_printfxy(0,0,"Sats: %d Fix: ??", GPS_Data.NumOfSats);
break;
}
s16 i1,i2,i3;
i1 = (s16)(GPS_Data.Longitude/10000000L);
i2 = abs((s16)((GPS_Data.Longitude%10000000L)/10000L));
i3 = abs((s16)(((GPS_Data.Longitude%10000000L)%10000L)/10L));
LCD_printfxy(0,1,"Lon: %d.%.3d%.3d deg",i1, i2, i3);
i1 = (s16)(GPS_Data.Latitude/10000000L);
i2 = abs((s16)((GPS_Data.Latitude%10000000L)/10000L));
i3 = abs((s16)(((GPS_Data.Latitude%10000000L)%10000L)/10L));
LCD_printfxy(0,2,"Lat: %d.%.3d%.3d deg",i1, i2, i3);
i1 = (s16)(GPS_Data.Altitude/1000L);
i2 = abs((s16)(GPS_Data.Altitude%1000L));
LCD_printfxy(0,3,"Alt: %d.%.3d m",i1, i2);
}
break;
case 2: // RC stick controls from FC
LCD_printfxy(0,0,"RC-Sticks" );
LCD_printfxy(0,1,"Pi:%4i Ro:%4i ",FC.StickPitch, FC.StickRoll);
LCD_printfxy(0,2,"Th:%4i Ya:%4i ",FC.StickThrust, FC.StickYaw);
break;
case 3: // RC poti controls from FC
LCD_printfxy(0,0,"RC-Potis" );
LCD_printfxy(0,1,"Po1:%3i Po2:%3i ",FC.Poti1, FC.Poti2);
LCD_printfxy(0,2,"Po3:%3i Po4:%3i ",FC.Poti3, FC.Poti4);
break;
case 4: // attitude from FC
LCD_printfxy(0,0,"IntPitch: %5i", FromFlightCtrl.IntegralPitch);
LCD_printfxy(0,1,"IntRoll: %5i", FromFlightCtrl.IntegralRoll);
LCD_printfxy(0,2,"AccPitch: %5i", FromFlightCtrl.AccPitch);
LCD_printfxy(0,3,"AccRoll: %5i", FromFlightCtrl.AccRoll);
break;
case 5: // gyros from FC
LCD_printfxy(0,0,"GyroPitch: %4i", FromFlightCtrl.GyroPitch);
LCD_printfxy(0,1,"GyroRoll: %4i", FromFlightCtrl.GyroRoll);
LCD_printfxy(0,2,"GyroYaw: %4i", FromFlightCtrl.GyroYaw);
break;
case 6: // Remote Control Level from FC
LCD_printfxy(0,0,"RC-Level: %3i", RC_Quality);
LCD_printfxy(0,1,"CompHeading: %3i", I2C_Heading.Heading);
LCD_printfxy(0,2,"GyroHeading: %3i", FromFlightCtrl.GyroHeading);
break;
case 7: // User Parameter
LCD_printfxy(0,0,"UP1:%3i UP2:%3i ",Parameter.User1,Parameter.User2);
LCD_printfxy(0,1,"UP3:%3i UP4:%3i ",Parameter.User3,Parameter.User4);
LCD_printfxy(0,2,"UP5:%3i UP6:%3i ",Parameter.User5,Parameter.User6);
LCD_printfxy(0,3,"UP7:%3i UP8:%3i ",Parameter.User7,Parameter.User8);
break;
default:
MaxMenuItem = MenuItem - 1;
MenuItem = 0;
break;
}
RemoteTasten = 0;
RemoteButtons = 0;
}
/branches/V0.1 killagreg/menu.h
1,6 → 1,16
#ifndef _MENU_H
#define _MENU_H
 
#include "91x_lib.h"
 
#define DISPLAYBUFFSIZE 80
 
extern void Menu(void);
extern s8 DisplayBuff[80];
extern void LcdClear(void);
 
extern void LCD_PrintMenu(void);
extern void LCD_Clear(void);
extern s8 DisplayBuff[DISPLAYBUFFSIZE];
extern u8 DispPtr;
extern u8 RemoteTasten;
extern u8 RemoteButtons;
 
#endif // _MENU_H
/branches/V0.1 killagreg/printf_P.c
89,7 → 89,7
#include <varargs.h>
#endif
 
#include "main.h"
#include "menu.h"
 
 
//#define LIGHTPRINTF
/branches/V0.1 killagreg/ramfunc.c
55,9 → 55,9
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//----------------------------------------------------------------------------------------------------
#include "91x_lib.h"
#include "ramfunc.h"
 
#include "main.h"
 
__ramfunc void Execute_Bootloader(void)
{
pFunction Jump_To_Bootloader;
/branches/V0.1 killagreg/ramfunc.h
1,5 → 1,5
#ifndef _RAMFUNC_H
#define _RAMFUNC_H
#define _RAMFUNC_H
 
typedef void (*pFunction)(void);
#define __ramfunc __attribute__ ((long_call, section (".ramfunc")))
/branches/V0.1 killagreg/sdc.c
54,9 → 54,13
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
 
#include "91x_lib.h"
#include "uart.h"
#include "sdc.h"
#include "ssc.h"
 
 
//________________________________________________________________________________________________________________________________________
// Module name: mmc.c
// Compiler used: avr-gcc 3.4.5
98,6 → 102,19
 
u8 SDC_Init(void)
{
 
GPIO_InitTypeDef GPIO_InitStructure;
 
SCU_APBPeriphClockConfig(__GPIO5, ENABLE); // Enable the GPIO5 Clock
 
/* Configure SD_SWITCH at pin GPIO5.3*/
GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull;
GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1;
GPIO_Init(GPIO5, &GPIO_InitStructure);
 
 
u16 Timeout = 0;
u8 CMD[] = {0x40,0x00,0x00,0x00,0x00,0x95};
 
/branches/V0.1 killagreg/settings.c
54,7 → 54,13
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "91x_lib.h"
#include "fat16.h"
#include "settings.h"
#include "uart.h"
 
struct str_setting_parameter CFG_Parameter[] =
{
/branches/V0.1 killagreg/spi_slave.c
7,14 → 7,14
// + FOR NON COMMERCIAL USE ONLY
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
25,21 → 25,21
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die PORTIERUNG der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + Die PORTIERUNG der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// + from this software without specific prior written permission.
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
// + for non-commercial use (directly or indirectly)
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// + with our written permission
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// + clearly linked as origin
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// + clearly linked as origin
// + * PORTING this software (or part of it) to systems (other than hardware from www.mikrokopter.de) is NOT allowed
//
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
52,295 → 52,314
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#include <string.h>
#include "91x_lib.h"
#include "led.h"
#include "GPSUart.h"
#include "GPS.h"
#include "uart.h"
#include "spi_slave.h"
#include "i2c.h"
#include "timer.h"
#include "main.h"
 
struct str_FromFlightCtrl FromFlightCtrl;
struct str_ToFlightCtrl ToFlightCtrl;
#define SPI_RXSYNCBYTE1 0xAA
#define SPI_RXSYNCBYTE2 0x83
#define SPI_TXSYNCBYTE1 0x81
#define SPI_TXSYNCBYTE2 0x55
 
struct str_ToFlightCtrl SPI_TxBuffer;
typedef enum
{
SPI_SYNC1,
SPI_SYNC2,
SPI_DATA
} SPI_State_t;
 
volatile unsigned char SPI_Buffer[sizeof(FromFlightCtrl)];
volatile unsigned char SPI_BufferIndex = 0;
volatile unsigned char SPI_TXBufferIndex = 0;
//communication packets
FromFlightCtrl_t FromFlightCtrl;
ToFlightCtrl_t ToFlightCtrl;
 
s16 CompassValue;
// tx packet buffer
#define SPI_TXBUFFER_LEN (2 + sizeof(ToFlightCtrl)) // 2 bytes at start are for synchronization
volatile u8 SPI_TxBuffer[SPI_TXBUFFER_LEN];
volatile u8 SPI_TxBufferIndex = 0;
u8 *Ptr_TxChksum = NULL ; // pointer to checksum in TxBuffer
 
volatile struct str_MicroMag MicroMag;
 
// rx packet buffer
#define SPI_RXBUFFER_LEN sizeof(FromFlightCtrl)
volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN];
volatile u8 SPI_RxBufferIndex = 0;
volatile u8 SPI_RxBuffer_Request = 0;
#define SPI_COMMAND_INDEX 0
unsigned char *Ptr_buffer_Tx = (unsigned char *) &SPI_TxBuffer;
//unsigned char *Ptr_buffer_Tx = (unsigned char *) &ToFlightCtrl;
unsigned char *Ptr_buffer_Rx = (unsigned char *) &FromFlightCtrl;
volatile unsigned char SPI_state = 0, SPI_TXUpdatebufferRequest = 0, SPI_RXUpdatebufferRequest = 0;
 
unsigned char SPI_CommandSequence[] = { SPI_CMD_OSD_DATA, SPI_CMD_GPS_POS, SPI_CMD_GPS_TARGET};
unsigned char SPI_CommandCounter = 0;
 
u8 SPI_CommandSequence[] = { SPI_CMD_OSD_DATA, SPI_CMD_GPS_POS, SPI_CMD_GPS_TARGET};
u8 SPI_CommandCounter = 0;
 
 
//--------------------------------------------------------------
void SSP0_IRQHandler(void)
{
static u8 chksum = 0;
u8 data;
GPIO_ToggleBit(GPIO5, GPIO_Pin_7);
SSP_ClearITPendingBit(SSP0, SSP_IT_RxTimeOut);
while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
{
if (SPI_TXBufferIndex < sizeof(ToFlightCtrl)) // still data to send ?
{ SSP0->DR = Ptr_buffer_Tx[SPI_TXBufferIndex];
SPI_TxBuffer.Chksum += Ptr_buffer_Tx[SPI_TXBufferIndex];
SPI_TXBufferIndex++;
}
else
{
SPI_TXBufferIndex = 0;
SPI_TXUpdatebufferRequest = 1;
ToFlightCtrl.Chksum = 0;
ToFlightCtrl.BeepTime = BeepTime;
BeepTime = 0;
memcpy((unsigned char *) &SPI_TxBuffer, (unsigned char *) &ToFlightCtrl, sizeof(ToFlightCtrl));
}
}
while (SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty)==SET)
{
static u8 rxchksum = 0;
u8 rxdata;
static SPI_State_t SPI_State = SPI_SYNC1;
 
data = SSP0->DR;
// clear pending bit
SSP_ClearITPendingBit(SSP0, SSP_IT_RxTimeOut);
// Fill TxFIFO while its not full or end of packet is reached
while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
{
if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) // still data to send ?
{
SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex]; // send a byte
*Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum
SPI_TxBufferIndex++; // pointer to next byte
}
else // TxBuffer end is reached then reset and copy data to tx buffer
{
SPI_TxBufferIndex = 0; // reset buffer index
ToFlightCtrl.Chksum = 0; // initialize checksum
ToFlightCtrl.BeepTime = BeepTime; // set beeptime
BeepTime = 0; // reset local beeptime
// copy contents of ToFlightCtrl->SPI_TxBuffer
memcpy( (u8 *) &(SPI_TxBuffer[2]), (u8 *) &ToFlightCtrl, sizeof(ToFlightCtrl));
}
}
// while RxFIFO not empty
while (SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty)==SET)
{
rxdata = SSP0->DR; // catch the received byte
// Fill TxFIFO while its not full or end of packet is reached
while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
{
if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) // still data to send ?
{
SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex]; // send a byte
*Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum
SPI_TxBufferIndex++; // pointer to next byte
}
else // end of packet is reached reset and copy data to tx buffer
{
SPI_TxBufferIndex = 0; // reset buffer index
ToFlightCtrl.Chksum = 0; // initialize checksum
ToFlightCtrl.BeepTime = BeepTime; // set beeptime
BeepTime = 0; // reset local beeptime
// copy contents of ToFlightCtrl->SPI_TxBuffer
memcpy((u8 *) &(SPI_TxBuffer[2]), (u8 *) &ToFlightCtrl, sizeof(ToFlightCtrl));
}
}
switch (SPI_State)
{
case SPI_SYNC1:
SPI_RxBufferIndex = 0; // reset buffer index
rxchksum = rxdata; // init checksum
if (rxdata == SPI_RXSYNCBYTE1)
{ // 1st syncbyte ok
SPI_State = SPI_SYNC2; // step to sync2
}
break;
case SPI_SYNC2:
if (rxdata == SPI_RXSYNCBYTE2)
{ // 2nd Syncbyte ok
rxchksum += rxdata;
SPI_State = SPI_DATA;
} // 2nd Syncbyte does not match
else
{
SPI_State = SPI_SYNC1; //jump back to sync1
}
break;
case SPI_DATA:
SPI_RxBuffer[SPI_RxBufferIndex++]= rxdata; // copy databyte to rx buffer
if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) // end of packet is reached
{
if (rxdata == rxchksum) // verify checksum byte
{
// copy SPI_RxBuffer -> FromFlightCtrl
if(!SPI_RxBuffer_Request) // block writing to FromFlightCtrl on reading access
{
memcpy((u8 *) &FromFlightCtrl, (u8 *) SPI_RxBuffer, sizeof(FromFlightCtrl));
SPI_RxBuffer_Request = 1;
}
DebugOut.Analog[13]++;
}
else // bad checksum byte
{
DebugOut.Analog[12]++; // increase SPI chksum error counter
}
SPI_State = SPI_SYNC1; // reset state
}
else // end of packet not reached
{
rxchksum += rxdata; // update checksum
}
break;
default:
SPI_State = SPI_SYNC1;
break;
}
}
}
 
while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
{
if (SPI_TXBufferIndex < sizeof(ToFlightCtrl)) // still data to send ?
{ SSP0->DR = Ptr_buffer_Tx[SPI_TXBufferIndex];
SPI_TxBuffer.Chksum += Ptr_buffer_Tx[SPI_TXBufferIndex];
SPI_TXBufferIndex++;
}
else
{
SPI_TXBufferIndex = 0;
ToFlightCtrl.Chksum = 0;
SPI_TXUpdatebufferRequest = 1;
ToFlightCtrl.BeepTime = BeepTime;
BeepTime = 0;
memcpy((unsigned char *) &SPI_TxBuffer, (unsigned char *) &ToFlightCtrl, sizeof(ToFlightCtrl));
}
//--------------------------------------------------------------
void SPI0_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
SSP_InitTypeDef SSP_InitStructure;
 
}
switch (SPI_state )
{
case 0:
SPI_BufferIndex = 0;
chksum = data;
if (data == 0xAA && !SPI_RXUpdatebufferRequest) { SPI_state = 1; } // 1. Syncbyte ok
break;
SerialPutString("SPI init...");
 
case 1:
if (data == 0x83) { chksum += data; SPI_state = 2; } // 2. Syncbyte ok
else SPI_state = 0;
break;
case 2:
SPI_Buffer[SPI_BufferIndex++]= data; // get data
if (SPI_BufferIndex >= sizeof(SPI_Buffer)) // end of packet
{
if (data == chksum)
{
u8 i;
SPI_RXUpdatebufferRequest = 1;
for (i=0;i<sizeof(SPI_Buffer);i++) { Ptr_buffer_Rx[i] = SPI_Buffer[i]; }
SCU_APBPeriphClockConfig(__GPIO2 ,ENABLE);
SCU_APBPeriphClockConfig(__SSP0 ,ENABLE);
 
DebugOut.Analog[13]++;
}
else { DebugOut.Analog[12]++; }
SPI_state = 0;
}
else chksum += data;
break;
GPIO_DeInit(GPIO2);
//SSP0_CLK, SSP0_MOSI, SSP0_SS pins
GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;
GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1 ;
GPIO_Init (GPIO2, &GPIO_InitStructure);
 
default: SPI_state = 0; break;
}
}
}
// SSP0_MISO pin GPIO2.6
GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;
GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2 ;
GPIO_Init (GPIO2, &GPIO_InitStructure);
 
//--------------------------------------------------------------
void SPI0_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
SSP_InitTypeDef SSP_InitStructure;
SSP_DeInit(SSP0);
SSP_StructInit(&SSP_InitStructure);
SSP_InitStructure.SSP_FrameFormat = SSP_FrameFormat_Motorola;
SSP_InitStructure.SSP_Mode = SSP_Mode_Slave;
SSP_InitStructure.SSP_SlaveOutput = SSP_SlaveOutput_Enable;
SSP_InitStructure.SSP_CPHA = SSP_CPHA_1Edge;
SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low;
SSP_InitStructure.SSP_ClockRate = 0;
 
SerialPutString("SPI init...");
SCU_APBPeriphClockConfig(__GPIO2 ,ENABLE);
SCU_APBPeriphClockConfig(__SSP0 ,ENABLE);
GPIO_DeInit(GPIO2);
//SSP0_CLK, SSP0_MOSI, SSP0_SS pins
GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;
GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1 ;
GPIO_Init (GPIO2, &GPIO_InitStructure);
SSP_Init(SSP0, &SSP_InitStructure);
 
// SSP0_MISO pin GPIO2.6
GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;
GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2 ;
GPIO_Init (GPIO2, &GPIO_InitStructure);
SSP_DeInit(SSP0);
SSP_StructInit(&SSP_InitStructure);
SSP_InitStructure.SSP_FrameFormat = SSP_FrameFormat_Motorola;
SSP_InitStructure.SSP_Mode = SSP_Mode_Slave;
SSP_InitStructure.SSP_SlaveOutput = SSP_SlaveOutput_Enable;
SSP_InitStructure.SSP_CPHA = SSP_CPHA_1Edge;
SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low;
SSP_InitStructure.SSP_ClockRate = 0;
SSP_Init(SSP0, &SSP_InitStructure);
SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_TxFifo | SSP_IT_RxTimeOut, ENABLE);
SSP_Cmd(SSP0, ENABLE);
SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_TxFifo | SSP_IT_RxTimeOut, ENABLE);
 
ToFlightCtrl.Sync1 = 0x81;
ToFlightCtrl.Sync2 = 0x55;
VIC_Config(SSP0_ITLine, VIC_IRQ, 1);
VIC_ITCmd(SSP0_ITLine, ENABLE);
SSP_Cmd(SSP0, ENABLE);
// initialize the syncbytes in the tx buffer
SPI_TxBuffer[0] = SPI_TXSYNCBYTE1;
SPI_TxBuffer[1] = SPI_TXSYNCBYTE2;
// set the pointer to the checksum byte in the tx buffer
Ptr_TxChksum = (u8 *) &(((ToFlightCtrl_t *) &(SPI_TxBuffer[2]))->Chksum);
 
SerialPutString("ok\n\r");
VIC_Config(SSP0_ITLine, VIC_IRQ, 1);
VIC_ITCmd(SSP0_ITLine, ENABLE);
 
SerialPutString("ok\n\r");
}
 
//------------------------------------------------------
void SPI_CheckSlaveSelect(void)
{
 
//if (SS_PIN) { SPI_BufferIndex = 0; }
DebugOut.Analog[0] = FromFlightCtrl.IntegralNick;
DebugOut.Analog[0] = FromFlightCtrl.IntegralPitch;
DebugOut.Analog[1] = FromFlightCtrl.IntegralRoll;
DebugOut.Analog[2] = (30*FromFlightCtrl.AccNick)/108;
DebugOut.Analog[2] = (30*FromFlightCtrl.AccPitch)/108;
DebugOut.Analog[3] = (30*FromFlightCtrl.AccRoll)/108;
DebugOut.Analog[25] = FromFlightCtrl.GyroHeading;
}
 
/* DebugOut.Analog[2] = FromFlightCtrl.StickNick;
DebugOut.Analog[3] = FromFlightCtrl.Command;
DebugOut.Analog[4] = FromFlightCtrl.StickRoll;
DebugOut.Analog[5] = FromFlightCtrl.StickGier;
*/
DebugOut.Analog[25] = FromFlightCtrl.GyroCompass;
 
}
//------------------------------------------------------
void UpdateSPI_Buffer(void)
{
if (SPI_RXUpdatebufferRequest)
{
if (CompassUpdateActiv) return; // testweise deaktiviert
if (SPI_RxBuffer_Request)
{
if (CompassUpdateActiv) return; // testweise deaktiviert
 
VIC_ITCmd(SSP0_ITLine, DISABLE);
// avoid sending data via SPI during the update of the ToFlightCtrl structure
VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
 
ToFlightCtrl.CompassValue = I2C_Heading.Heading;
ToFlightCtrl.GPS_Nick = GPS_Nick;
ToFlightCtrl.GPS_Roll = GPS_Roll;
DebugOut.Analog[26] = I2C_Heading.Heading;
ToFlightCtrl.CompassHeading = I2C_Heading.Heading;
ToFlightCtrl.GPS_Pitch = GPS_Pitch;
ToFlightCtrl.GPS_Roll = GPS_Roll;
ToFlightCtrl.GPS_Yaw = GPS_Yaw;
DebugOut.Analog[26] = I2C_Heading.Heading;
// cycle spi commands
ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
 
ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
switch (ToFlightCtrl.Command)
{
case SPI_CMD_OSD_DATA:
ToFlightCtrl.Param.Byte[0] = OsdBar;
ToFlightCtrl.Param.Int[1] = OsdDistance;
break;
switch (ToFlightCtrl.Command)
{
case SPI_CMD_OSD_DATA:
ToFlightCtrl.Param.Byte[0] = OsdBar;
ToFlightCtrl.Param.Int[1] = OsdDistance;
break;
 
case SPI_CMD_GPS_POS:
ToFlightCtrl.Param.Long[0] = GPS_Data.Longitude;
ToFlightCtrl.Param.Long[1] = GPS_Data.Latitude;
break;
case SPI_CMD_GPS_POS:
ToFlightCtrl.Param.Long[0] = GPS_Data.Longitude;
ToFlightCtrl.Param.Long[1] = GPS_Data.Latitude;
break;
 
case SPI_CMD_GPS_TARGET:
ToFlightCtrl.Param.Long[0] = GPS_Data.TargetLongitude;
ToFlightCtrl.Param.Long[1] = GPS_Data.TargetLatitude;
break;
case SPI_CMD_GPS_TARGET:
ToFlightCtrl.Param.Long[0] = GPSTargetPosition.Longitude;
ToFlightCtrl.Param.Long[1] = GPSTargetPosition.Latitude;
break;
 
default:
break;
}
VIC_ITCmd(SSP0_ITLine, ENABLE);
default:
break;
}
VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt
 
if (I2C_Heading.Heading <= 359)
{
}
else
{
if (I2C_Version.Hauptversion != 0xff) TimerCompassUpdate = SetDelay(1);
return;
}
if (I2C_Heading.Heading <= 359)
{
// nothing?
}
else
{
if (I2C_Version.Major != 0xFF) TimerCompassUpdate = SetDelay(1);
return;
}
 
SPI_TXUpdatebufferRequest = 0;
switch(FromFlightCtrl.Command)
{
case SPI_CMD_USER:
Parameter.User1 = FromFlightCtrl.Param.Byte[0];
Parameter.User2 = FromFlightCtrl.Param.Byte[1];
Parameter.User3 = FromFlightCtrl.Param.Byte[2];
Parameter.User4 = FromFlightCtrl.Param.Byte[3];
Parameter.User5 = FromFlightCtrl.Param.Byte[4];
Parameter.User6 = FromFlightCtrl.Param.Byte[5];
Parameter.User7 = FromFlightCtrl.Param.Byte[6];
Parameter.User8 = FromFlightCtrl.Param.Byte[7];
break;
case SPI_CMD_STICK:
FC.StickThrust = FromFlightCtrl.Param.sByte[0];
FC.StickYaw = FromFlightCtrl.Param.sByte[1];
FC.StickRoll = FromFlightCtrl.Param.sByte[2];
FC.StickPitch = FromFlightCtrl.Param.sByte[3];
FC.Poti1 = FromFlightCtrl.Param.Byte[4];
FC.Poti2 = FromFlightCtrl.Param.Byte[5];
FC.Poti3 = FromFlightCtrl.Param.Byte[6];
FC.Poti4 = FromFlightCtrl.Param.Byte[7];
RC_Quality = FromFlightCtrl.Param.Byte[8];
break;
 
//----------------
case SPI_CMD_CAL_COMPASS:
CompassCalState = FromFlightCtrl.Param.Byte[0];
break;
 
// DebugOut.Analog[11] = FromFlightCtrl.Command;
switch(FromFlightCtrl.Command)
{
case SPI_CMD_USER:
DebugOut.Analog[7]++;
Parameter_UserParam1 = FromFlightCtrl.Param.Byte[0];
Parameter_UserParam2 = FromFlightCtrl.Param.Byte[1];
Parameter_UserParam3 = FromFlightCtrl.Param.Byte[2];
Parameter_UserParam4 = FromFlightCtrl.Param.Byte[3];
Parameter_UserParam5 = FromFlightCtrl.Param.Byte[4];
Parameter_UserParam6 = FromFlightCtrl.Param.Byte[5];
Parameter_UserParam7 = FromFlightCtrl.Param.Byte[6];
break;
case SPI_CMD_STICK:
DebugOut.Analog[8]++;
FC_StickGas = (s32) FromFlightCtrl.Param.sByte[0];
FC_StickGier = (s32) FromFlightCtrl.Param.sByte[1];
FC_StickNick = (s32) FromFlightCtrl.Param.sByte[2];
FC_StickRoll = (s32) FromFlightCtrl.Param.sByte[3];
FC_Poti1 = (s32) FromFlightCtrl.Param.Byte[4];
FC_Poti2 = (s32) FromFlightCtrl.Param.Byte[5];
FC_Poti3 = (s32) FromFlightCtrl.Param.Byte[6];
FC_Poti4 = (s32) FromFlightCtrl.Param.Byte[7];
SenderOkay = (s32) FromFlightCtrl.Param.Byte[8];
break;
default:
break;
}
 
case SPI_CMD_CAL_COMPASS:
DebugOut.Analog[9]++;
CompassCalState = FromFlightCtrl.Param.Byte[0];
break;
 
default:
break;
}
//------------
I2C_WriteAttitude.Roll = FromFlightCtrl.IntegralRoll;
I2C_WriteAttitude.Pitch = FromFlightCtrl.IntegralPitch;
 
// every time we got new data from the FC via SPI call the navigation routine
Navigation();
 
//------------
I2C_WriteNickRoll.Roll = FromFlightCtrl.IntegralRoll;
I2C_WriteNickRoll.Nick = FromFlightCtrl.IntegralNick;
 
Navigation();
 
SPI_RXUpdatebufferRequest = 0;
}
SPI_RxBuffer_Request = 0;
}
}
 
/branches/V0.1 killagreg/spi_slave.h
1,5 → 1,5
#ifndef _SPI_H
#define _SPI_H
#ifndef _SPI_SLAVE_H
#define _SPI_SLAVE_H
 
#define SPI_PROTOCOL_COMP 1
 
6,78 → 6,61
 
#define SS_PIN GPIO_ReadBit(GPIO2, GPIO_Pin_7)
 
// IMPORTANT: no syncbytes in structure !
#define SPI_CMD_USER 10
#define SPI_CMD_STICK 11
#define SPI_CMD_CAL_COMPASS 12
 
struct str_FromFlightCtrl
typedef struct
{
u8 Command;
s16 IntegralNick;
s16 IntegralPitch;
s16 IntegralRoll;
s16 AccNick;
s16 AccPitch;
s16 AccRoll;
s16 GyroCompass;
s16 GyroNick;
s16 GyroHeading;
s16 GyroPitch;
s16 GyroRoll;
s16 GyroGier;
s16 GyroYaw;
union
{ u8 Byte[12];
s8 sByte[12];
s16 Int[6];
{
s8 sByte[12];
u8 Byte[12];
s16 Int[6];
s32 Long[3];
float Float[3];
int Long[3];
} Param;
u8 Chksum;
} __attribute__((packed));
} __attribute__((packed)) FromFlightCtrl_t;
#define SPI_CMD_OSD_DATA 100
#define SPI_CMD_GPS_POS 101
#define SPI_CMD_GPS_TARGET 102
 
struct str_ToFlightCtrl
{
unsigned char Sync1, Sync2;
unsigned char Command;
s16 GPS_Nick;
s16 GPS_Roll;
s16 GPS_Gier;
s16 CompassValue;
s16 Status;
unsigned char BeepTime;
union
{ u8 Byte[12];
s16 Int[6];
float Float[3];
int Long[3];
} Param;
unsigned char Chksum;
} __attribute__((packed));
typedef struct
{
u8 Command;
s16 GPS_Pitch;
s16 GPS_Roll;
s16 GPS_Yaw;
s16 CompassHeading;
s16 Status;
u8 BeepTime;
union
{
s8 sByte[12];
u8 Byte[12];
s16 Int[6];
s32 Long[3];
float Float[3];
}Param;
u8 Chksum;
} __attribute__((packed)) ToFlightCtrl_t;
extern FromFlightCtrl_t FromFlightCtrl;
extern ToFlightCtrl_t ToFlightCtrl;
 
#define X_AXIS 1
#define Y_AXIS 2
#define Z_AXIS 3
 
 
 
struct str_MicroMag
{
unsigned char ReadAxis, WaitingforMeasurement;
s16 Axis[3];
s16 Heading;
} __attribute__((packed));
 
 
extern struct str_FromFlightCtrl FromFlightCtrl;
extern struct str_ToFlightCtrl ToFlightCtrl;
 
extern volatile struct str_MicroMag MicroMag;
 
extern s16 GPS_Nick, GPS_Roll, CompassValue;
 
extern void SPI0_Init(void);
extern void SPI_CheckSlaveSelect(void);
extern void UpdateSPI_Buffer(void);
#endif
 
#endif //_SPI_SLAVE_H
/branches/V0.1 killagreg/ssc.c
54,10 → 54,11
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
#include "91x_lib.h"
#include "uart.h"
 
//________________________________________________________________________________________________________________________________________
// Module name: fat16.c
// Module name: ssc.c
// Compiler used: avr-gcc 3.4.5
// Last Modifikation: 24.07.2007
// Version: 1.02
83,6 → 84,22
 
 
//________________________________________________________________________________________________________________________________________
// Funtion: SSC_Disable(void);
//
// Description: This function enables chipselect of the sdcard (active low)
//
//
// Returnvalue: none
//________________________________________________________________________________________________________________________________________
 
void SSC_Disable(void)
{
//MMC_Write &= ~(1<<MMC_Chip_Select); // disable chipselect of the sdcard (active low).
GPIO_WriteBit(GPIO5, GPIO_Pin_4 , Bit_SET);
}
 
 
//________________________________________________________________________________________________________________________________________
// Funtion: SSC_Init(void);
//
// Description: This function initialises the synchronus serial channel to the sdcard.
101,7 → 118,6
 
SCU_APBPeriphClockConfig(__SSP1 ,ENABLE);
// GPIO_DeInit(GPIO2);
GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_6;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
206,20 → 222,7
}
 
 
//________________________________________________________________________________________________________________________________________
// Funtion: SSC_Disable(void);
//
// Description: This function enables chipselect of the sdcard (active low)
//
//
// Returnvalue: none
//________________________________________________________________________________________________________________________________________
 
void SSC_Disable(void)
{
//MMC_Write &= ~(1<<MMC_Chip_Select); // disable chipselect of the sdcard (active low).
GPIO_WriteBit(GPIO5, GPIO_Pin_4 , Bit_SET);
}
 
 
 
/branches/V0.1 killagreg/ssc.h
1,38 → 1,6
#ifndef __SSC_H
#define __SSC_H
 
#define MMC_Write PORTB //Port an der die MMC/SD-Karte angeschlossen ist also des SPI
#define MMC_Read PINB
#define MMC_Direction_REG DDRB
 
 
#if defined (__AVR_ATmega128__)
#define SPI_DI 3 //Port Pin an dem Data Output der MMC/SD-Karte angeschlossen ist
#define SPI_DO 2 //Port Pin an dem Data Input der MMC/SD-Karte angeschlossen ist
#define SPI_Clock 1 //Port Pin an dem die Clock der MMC/SD-Karte angeschlossen ist (clk)
#define MMC_Chip_Select 4 //Port Pin an dem Chip Select der MMC/SD-Karte angeschlossen ist
#define SPI_SS 0 //Nicht Benutz muß aber definiert werden
#endif
 
 
#if defined (__AVR_ATmega32__)
#define SPI_DI 6 //Port Pin an dem Data Output der MMC/SD-Karte angeschlossen ist
#define SPI_DO 5 //Port Pin an dem Data Input der MMC/SD-Karte angeschlossen ist
#define SPI_Clock 7 //Port Pin an dem die Clock der MMC/SD-Karte angeschlossen ist (clk)
#define MMC_Chip_Select 3 //Port Pin an dem Chip Select der MMC/SD-Karte angeschlossen ist
#define SPI_SS 4 //Nicht Benutz muß aber definiert werden
#endif
 
 
#if defined (__AVR_ATmega644__)
#define SPI_DI 6 //Port Pin an dem Data Output der MMC/SD-Karte angeschlossen ist
#define SPI_DO 5 //Port Pin an dem Data Input der MMC/SD-Karte angeschlossen ist
#define SPI_Clock 7 //Port Pin an dem die Clock der MMC/SD-Karte angeschlossen ist (clk)
#define MMC_Chip_Select 4 //Port Pin an dem Chip Select der MMC/SD-Karte angeschlossen ist
#define SPI_SS 3 //Nicht Benutz muß aber definiert werden
#endif
 
 
//________________________________________________________________________________________________________________________________________
//
// Functions needed for accessing the sdcard.
39,12 → 7,12
//
//________________________________________________________________________________________________________________________________________
 
extern void SSC_Init(void);
extern unsigned char SSC_GetChar (void);
extern void SSC_PutChar (unsigned char);
extern void SSC_Enable(void);
extern void SSC_Disable(void);
extern void SSC_ClearRxFifo(void);
extern void SSC_Init(void);
extern u8 SSC_GetChar (void);
extern void SSC_PutChar (u8);
extern void SSC_Enable(void);
extern void SSC_Disable(void);
extern void SSC_ClearRxFifo(void);
 
 
#endif
/branches/V0.1 killagreg/timer.c
54,7 → 54,8
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
#include "91x_lib.h"
#include "uart.h"
 
u32 CountMilliseconds;
 
66,7 → 67,7
CountMilliseconds++;
//if (GPIO_ReadBit(GPIO6, GPIO_Pin_3)) GPIO_WriteBit(GPIO6, GPIO_Pin_3, Bit_RESET); else GPIO_WriteBit(GPIO6, GPIO_Pin_3, Bit_SET);
TIM_ClearFlag(TIM1, TIM_FLAG_OC1);
TIM_ClearFlag(TIM1, TIM_FLAG_OC1); // clear irq pending bit
}
 
//----------------------------------------------------------------------------------------------------
74,32 → 75,30
//----------------------------------------------------------------------------------------------------
void TIMER1_Init(void)
{
TIM_InitTypeDef TIM_InitStructure;
TIM_InitTypeDef TIM_InitStructure;
SerialPutString("Timer init...");
SerialPutString("Timer init...");
#define TIM1_FREQ 200000
// TimerOCR set in IntHandler
#define TIM1_FREQ 200000 // 200kHz
// TimerOCR set in IntHandler
 
SCU_APBPeriphClockConfig(__TIM01, ENABLE);
SCU_APBPeriphClockConfig(__TIM01, ENABLE);
TIM_StructInit(&TIM_InitStructure);
TIM_InitStructure.TIM_Mode = TIM_OCM_CHANNEL_1;
TIM_InitStructure.TIM_OC1_Modes = TIM_TIMING;
TIM_InitStructure.TIM_Clock_Source = TIM_CLK_APB;
TIM_InitStructure.TIM_Prescaler = (SCU_GetPCLKFreqValue() * 1000) / TIM1_FREQ; // klappt nur bis 48 MHz !
TIM_Init (TIM1, &TIM_InitStructure);
TIM_StructInit(&TIM_InitStructure);
TIM_InitStructure.TIM_Mode = TIM_OCM_CHANNEL_1;
TIM_InitStructure.TIM_OC1_Modes = TIM_TIMING;
TIM_InitStructure.TIM_Clock_Source = TIM_CLK_APB;
TIM_InitStructure.TIM_Prescaler = (SCU_GetPCLKFreqValue() * 1000) / TIM1_FREQ; // is only valid up to 48 MHz !
TIM_Init (TIM1, &TIM_InitStructure);
 
TIM_ITConfig(TIM1, TIM_IT_OC1, ENABLE);
TIM_CounterCmd(TIM1, TIM_START);
TIM_ITConfig(TIM1, TIM_IT_OC1, ENABLE);
TIM_CounterCmd(TIM1, TIM_START);
 
VIC_Config(TIM1_ITLine, VIC_IRQ, 5);
VIC_ITCmd(TIM1_ITLine, ENABLE);
VIC_Config(TIM1_ITLine, VIC_IRQ, 5);
VIC_ITCmd(TIM1_ITLine, ENABLE);
 
// DebugOut.Analog[27] = SCU_GetPCLKFreqValue()/1000;
 
CountMilliseconds = 0;
SerialPutString("ok\n\r");
CountMilliseconds = 0;
SerialPutString("ok\n\r");
}
 
106,19 → 105,19
// -----------------------------------------------------------------------
u32 SetDelay (u32 t)
{
return(CountMilliseconds + t -1);
return(CountMilliseconds + t -1);
}
 
// -----------------------------------------------------------------------
u8 CheckDelay(u32 t)
{
return(((t - CountMilliseconds)& 0x80000000) >> 27);
return(((t - CountMilliseconds)& 0x80000000) >> 27);
}
 
// -----------------------------------------------------------------------
void Delay_ms(u32 w)
void Delay_ms(u32 wait)
{
u32 akt;
akt = SetDelay(w);
while (!CheckDelay(akt));
u32 akt;
akt = SetDelay(wait);
while (!CheckDelay(akt));
}
/branches/V0.1 killagreg/timer.h
1,5 → 1,5
#ifndef _TIMER_H
#define _TIMER_H
#define _TIMER_H
 
extern u32 CountMilliseconds;
 
/branches/V0.1 killagreg/uart.c
55,369 → 55,405
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
 
#include "91x_lib.h"
#include "ramfunc.h"
#include "menu.h"
#include "printf_P.h"
#include "GPS.h"
#include "uart.h"
#include "timer.h"
#include "usb.h"
#include "main.h"
 
u8 DebugGetAnforderung = 0,DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 0;
unsigned volatile char SioTmp = 0;
unsigned volatile char SendeBuffer[MAX_SENDE_BUFF];
unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF];
unsigned volatile char NMEABuffer[MAX_EMPFANGS_BUFF];
unsigned volatile char NeuerDatensatzEmpfangen = 0;
unsigned volatile char NeueKoordinateEmpfangen = 0;
unsigned volatile char UebertragungAbgeschlossen = 1;
unsigned volatile char SendGPSPosAnforderung = 0;
unsigned volatile char CntCrcError = 0;
unsigned volatile char AnzahlEmpfangsBytes = 0;
unsigned volatile char PC_DebugTimeout = 0;
volatile unsigned char DebugTextAnforderung = 255;
unsigned char NurKanalAnforderung = 0;
#define FALSE 0
#define TRUE 1
 
UART_TypeDef *DebugUART = UART1;
u8 Request_VerInfo = FALSE;
u8 Request_ExternalControl = FALSE;
u8 Request_Display = FALSE;
u8 Request_DebugData = FALSE;
u8 Request_DebugLabel = 255;
u8 Request_ChannelOnly = FALSE;
u8 Request_GPS_Position = FALSE;
u8 Remote_PollDisplayLine = 0;
 
unsigned char RemotePollDisplayLine = 0;
u8 PcZugriff = 100;
volatile u8 txd_buffer[TXD_BUFFER_LEN];
volatile u8 rxd_buffer_locked = FALSE;
volatile u8 rxd_buffer[RXD_BUFFER_LEN];
volatile u8 txd_complete = TRUE;
volatile u8 ReceivedBytes = 0;
 
volatile u8 CntCrcError = 0;
u8 text[20];
 
u8 PcAccess = 100;
u8 MotorTest[4] = {0,0,0,0};
u8 MeineSlaveAdresse;
volatile struct str_DebugOut DebugOut;
struct str_ExternControl ExternControl;
struct str_GPSPosition GPS_Position;
struct str_VersionInfo VersionInfo;
struct str_PCTargetGPSPosition PCTargetGPSPosition;
u8 MySlaveAddr = 0;
u8 ConfirmFrame;
 
DebugOut_t DebugOut;
ExternControl_t ExternControl;
VersionInfo_t VersionInfo;
GPSPosition_t GPS_Position;
 
s32 Debug_Timer;
static u16 ptr = 0;
unsigned char ConfirmFrame;
 
#define FIFO_TX_LEVEL 2
static u16 ptr_txd_buffer = 0;
 
UART_InitTypeDef UART_InitStructure;
 
const unsigned char ANALOG_TEXT[32][16] =
const u8 ANALOG_LABEL[32][16] =
{
//1234567890123456
"WinkelNick ", //0
"WinkelRoll ",
"AccNick ",
"AnglePitch ", //0
"AngleRoll ",
"AccPitch ",
"AccRoll ",
"AN4 ",
"AN5 ", //5
"GPS-Data Counter",
"7 ",
"8 ",
"9 ",
"10 ", //10
"Poti1 ",
" ",
" ", //5
" ",
" ",
" ",
" ",
" ", //10
" ",
"SPI Error ",
"SPI Okay ",
"Poti2 ",
"Poti3 ", //15
" ",
" ", //15
"I2C_ReadByte ",
"ACC_Speed_N ",
"ACC_Speed_E ",
" ",
" ", //20
// "Distance_N ",
// "Distance_E ", //20
"N_Speed ",
"E_Speed ",
"I_North ",
"I_East ",
"GyroKompass ", //25
"Heading ",
// "Distance2Target ",
// "Direction2Target",
"GyroHeading ", //25
"CompassHeading ",
"Distance N ",
"Distance E ",
"GPS_NICK ",
"GPS_ROLL ", //30
"Used_Sat "
"GPS_Pitch ",
"GPS_Roll ", //30
"Used_Sats "
};
 
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++ Sende- und Empfangs-Part der Datenübertragung, incl. CRC-Auswertung
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void UART1_IRQHandler(void)
/********************************************************/
/* Initialization the UART1 */
/********************************************************/
void UART1_Init (void)
{
// ---------------------------- Receive ------------------------------------
if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET)|| (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )
{
static u16 crc;
static u8 crc1,crc2,buf_ptr;
static u8 UartState = 0;
u8 CrcOkay = 0;
GPIO_InitTypeDef GPIO_InitStructure;
UART_InitTypeDef UART_InitStructure;
 
if (DebugUART != UART1)
{
while (UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
UART_SendData(DebugUART, UART_ReceiveData(UART1));
}
else
while ((UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)&& (!NeuerDatensatzEmpfangen))
{
SioTmp = UART_ReceiveData(UART1);
SCU_APBPeriphClockConfig(__UART1, ENABLE); // Enable the UART1 Clock
SCU_APBPeriphClockConfig(__GPIO3, ENABLE); // Enable the GPIO3 Clock
 
if(buf_ptr >= MAX_EMPFANGS_BUFF) UartState = 0;
if(SioTmp == '\r' && UartState == 2)
{
UartState = 0;
crc -= RxdBuffer[buf_ptr-2];
crc -= RxdBuffer[buf_ptr-1];
crc %= 4096;
crc1 = '=' + crc / 64;
crc2 = '=' + crc % 64;
CrcOkay = 0;
if((crc1 == RxdBuffer[buf_ptr-2]) && (crc2 == RxdBuffer[buf_ptr-1])) CrcOkay = 1; else { CrcOkay = 0; CntCrcError++;};
if(!NeuerDatensatzEmpfangen && CrcOkay) // Datensatz schon verarbeitet
{
NeuerDatensatzEmpfangen = 1;
// GPIO_ToggleBit(GPIO6, GPIO_Pin_2);
/*Configure UART1_Rx pin GPIO3.2*/
GPIO_DeInit(GPIO3);
GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;
GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1 ;
GPIO_Init(GPIO3, &GPIO_InitStructure);
 
AnzahlEmpfangsBytes = buf_ptr;
RxdBuffer[buf_ptr] = '\r';
if(RxdBuffer[2] == 'R')
{
PowerOff();
VIC_DeInit();
Execute_Bootloader(); // Reset-Commando - Bootloader starten
}
// while (UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET) SioTmp = UART_ReceiveData(UART1);
//BearbeiteRxDaten();
break;
}
}
else
switch(UartState)
{
case 0:
if(SioTmp == '#' && !NeuerDatensatzEmpfangen) UartState = 1; // Startzeichen und Daten schon verarbeitet
buf_ptr = 0;
RxdBuffer[buf_ptr++] = SioTmp;
crc = SioTmp;
break;
case 1: // Adresse auswerten
UartState++;
RxdBuffer[buf_ptr++] = SioTmp;
crc += SioTmp;
break;
case 2: // Eingangsdaten sammeln
RxdBuffer[buf_ptr] = SioTmp;
if(buf_ptr < MAX_EMPFANGS_BUFF) buf_ptr++;
else UartState = 0;
crc += SioTmp;
break;
default:
UartState = 0;
break;
}
}
}
/*Configure UART1_Tx pin GPIO3.3*/
GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2 ;
GPIO_Init(GPIO3, &GPIO_InitStructure);
 
UART_ClearITPendingBit(UART1, UART_IT_Receive);
UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);
/* UART1 configured as follow:
- Word Length = 8 Bits
- One Stop Bit
- No parity
- BaudRate = 57600 baud
- Hardware flow control Disabled
- Receive and transmit enabled
- Receive and transmit FIFOs are Disabled
*/
UART_InitStructure.UART_WordLength = UART_WordLength_8D;
UART_InitStructure.UART_StopBits = UART_StopBits_1;
UART_InitStructure.UART_Parity = UART_Parity_No ;
UART_InitStructure.UART_BaudRate = BAUD_RATE ;
UART_InitStructure. UART_HardwareFlowControl = UART_HardwareFlowControl_None;
UART_InitStructure.UART_Mode = UART_Mode_Tx_Rx;
UART_InitStructure.UART_FIFO = UART_FIFO_Enable;
UART_InitStructure.UART_TxFIFOLevel = UART_FIFOLevel_1_2;
UART_InitStructure.UART_RxFIFOLevel = UART_FIFOLevel_1_2;
 
UART_DeInit(UART1); // reset uart 1 to default
UART_Init(UART1, &UART_InitStructure); // initialize uart 1
// enable uart 1 interrupts selective
UART_ITConfig(UART1, UART_IT_Receive | UART_IT_ReceiveTimeOut /*| UART_IT_Transmit */, ENABLE);
UART_Cmd(UART1, ENABLE); // enable uart 1
// configure the uart 1 interupt line as an IRQ with priority 4 (0 is highest)
VIC_Config(UART1_ITLine, VIC_IRQ, 4);
// enable the uart 1 IRQ
VIC_ITCmd(UART1_ITLine, ENABLE);
// initialize the debug timer
Debug_Timer = SetDelay(3000);
// unlock rxd_buffer
rxd_buffer_locked = FALSE;
// no bytes to send
txd_complete = TRUE;
}
 
// --------------------------------------------------------------------------
void UART1_Transmit(void)
 
/****************************************************************/
/* USART1 receiver ISR */
/****************************************************************/
void UART1_IRQHandler(void)
{
u8 tmp_tx;
static u16 crc;
static u8 ptr_rxd_buffer = 0;
static u8 crc1, crc2;
u8 c;
 
if((!UebertragungAbgeschlossen) && (UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) == RESET))
{
tmp_tx = SendeBuffer[ptr];
if((tmp_tx == '\r') || (ptr == MAX_SENDE_BUFF))
{
ptr = 0;
UebertragungAbgeschlossen = 1;
}
UART_SendData(UART1, tmp_tx);
if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET) || (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )
{
while ((UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET) && (!rxd_buffer_locked))
{ // some byes in the fifo and rxd buffer not locked
// get byte from fifo
c = UART_ReceiveData(UART1);
if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
{
rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
crc = c; // init crc
}
#if 0
else if (ptr_rxd_buffer == 1) // handle address
{
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
crc += c; // update crc
}
#endif
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // rxd buffer not full
{
if (c != '\r') // no termination character received
{
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
crc += c; // update crc
}
else // termination character received
{
// the last 2 bytes are no subject for checksum calculation
// they are the checksum itself
crc -= rxd_buffer[ptr_rxd_buffer-2];
crc -= rxd_buffer[ptr_rxd_buffer-1];
// calculate checksum from transmitted data
crc %= 4096;
crc1 = '=' + crc / 64;
crc2 = '=' + crc % 64;
// compare checksum to transmitted checksum bytes
if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
{ // checksum valid
rxd_buffer_locked = TRUE; // lock the rxd buffer
ReceivedBytes = ptr_rxd_buffer; // store number of received bytes
rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
// if 2nd byte is an 'R' start bootloader
if(rxd_buffer[2] == 'R')
{
PowerOff();
VIC_DeInit();
Execute_Bootloader(); // Reset-Commando - Bootloader starten
}
} // eof checksum valid
else
{ // checksum invalid
rxd_buffer_locked = FALSE; // unlock rxd buffer
} // eof checksum invalid
ptr_rxd_buffer = 0; // reset rxd buffer pointer
} // eof termination character received
} // rxd buffer not full
else // rxd buffer overrun
{
ptr_rxd_buffer = 0; // reset rxd buffer pointer
rxd_buffer_locked = FALSE; // unlock rxd buffer
} // eof rxd buffer overrrun
} // some byes in the fifo and rxd buffer not locked
// clear the pending bits
UART_ClearITPendingBit(UART1, UART_IT_Receive);
UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);
}
}
 
ptr++;
}
/**************************************************************/
/* Transmit tx buffer via debug uart */
/**************************************************************/
void UART1_Transmit(void)
{
u8 tmp_tx;
// if something has to be send and the txd fifo is not full
if((!txd_complete) && (UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) == RESET))
{
tmp_tx = txd_buffer[ptr_txd_buffer]; // read byte from txd buffer
// if terminating character or end of txd buffer reached
if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN))
{
ptr_txd_buffer = 0; // reset txd buffer pointer
txd_complete = TRUE;// set complete flag
}
UART_SendData(UART1, tmp_tx); // put character to txd fifo
// set pointer to next byte
ptr_txd_buffer++;
}
}
// --------------------------------------------------------------------------
void AddCRC(u16 wieviele)
/**************************************************************/
/* Add CRC and initiate transmission via debug uart */
/**************************************************************/
void AddCRC(u16 datalen)
{
u16 tmpCRC = 0,i;
//u8 count = FIFO_TX_LEVEL + 2 ;
 
for(i = 0; i < wieviele;i++)
{
tmpCRC += SendeBuffer[i];
}
tmpCRC %= 4096;
SendeBuffer[i++] = '=' + tmpCRC / 64;
SendeBuffer[i++] = '=' + tmpCRC % 64;
SendeBuffer[i++] = '\r';
 
ptr = 0;
// USB_Send_Data((u8 *) SendeBuffer,i);
{
UART_SendData(UART1,SendeBuffer[ptr++]);
UebertragungAbgeschlossen = 0;
}
u16 tmpCRC = 0, i;
for(i = 0; i < datalen; i++)
{
tmpCRC += txd_buffer[i];
}
tmpCRC %= 4096;
txd_buffer[i++] = '=' + tmpCRC / 64;
txd_buffer[i++] = '=' + tmpCRC % 64;
txd_buffer[i++] = '\r';
ptr_txd_buffer = 0;
txd_complete = FALSE;
UART_SendData(UART1,txd_buffer[ptr_txd_buffer++]); // send first byte
}
 
 
 
// --------------------------------------------------------------------------
void SendOutData(u8 cmd,u8 modul, u8 *snd, u8 len)
/**************************************************************/
/* Code output data */
/**************************************************************/
void SendOutData(u8 cmd, u8 module, u8 *snd, u8 len)
{
u16 pt = 0;
u8 a,b,c;
u8 ptr = 0;
 
SendeBuffer[pt++] = '#'; // Startzeichen
SendeBuffer[pt++] = modul; // Adresse (a=0; b=1,...)
SendeBuffer[pt++] = cmd; // Commando
 
while(len)
{
if(len) { a = snd[ptr++]; len--;} else a = 0;
if(len) { b = snd[ptr++]; len--;} else b = 0;
if(len) { c = snd[ptr++]; len--;} else c = 0;
SendeBuffer[pt++] = '=' + (a >> 2);
SendeBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
SendeBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
SendeBuffer[pt++] = '=' + ( c & 0x3f);
}
AddCRC(pt);
u16 pt = 0;
u8 a,b,c;
u8 ptr = 0;
txd_buffer[pt++] = '#'; // Start character
txd_buffer[pt++] = module; // Address (a=0; b=1,...)
txd_buffer[pt++] = cmd; // Command
while(len)
{
if(len) { a = snd[ptr++]; len--;} else a = 0;
if(len) { b = snd[ptr++]; len--;} else b = 0;
if(len) { c = snd[ptr++]; len--;} else c = 0;
txd_buffer[pt++] = '=' + (a >> 2);
txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
txd_buffer[pt++] = '=' + ( c & 0x3f);
}
AddCRC(pt); // add checksum after data block and initates the transmission
}
 
 
// --------------------------------------------------------------------------
void Decode64(u8 *ptrOut, u8 len, u8 ptrIn,u8 max) // Wohin mit den Daten; Wie lang; Wo im RxdBuffer
/**************************************************************/
/* Decode data */
/**************************************************************/
void Decode64(u8 *ptrOut, u8 len, u8 ptrIn ,u8 max)
{
u8 a,b,c,d;
u8 ptr = 0;
u8 x,y,z;
while(len)
{
a = RxdBuffer[ptrIn++] - '=';
b = RxdBuffer[ptrIn++] - '=';
c = RxdBuffer[ptrIn++] - '=';
d = RxdBuffer[ptrIn++] - '=';
if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden
 
x = (a << 2) | (b >> 4);
y = ((b & 0x0f) << 4) | (c >> 2);
z = ((c & 0x03) << 6) | d;
 
if(len--) ptrOut[ptr++] = x; else break;
if(len--) ptrOut[ptr++] = y; else break;
if(len--) ptrOut[ptr++] = z; else break;
}
 
u8 a,b,c,d;
u8 ptr = 0;
u8 x,y,z;
while(len)
{
a = rxd_buffer[ptrIn++] - '=';
b = rxd_buffer[ptrIn++] - '=';
c = rxd_buffer[ptrIn++] - '=';
d = rxd_buffer[ptrIn++] - '=';
if(ptrIn > max - 2) break;
x = (a << 2) | (b >> 4);
y = ((b & 0x0f) << 4) | (c >> 2);
z = ((c & 0x03) << 6) | d;
if(len--) ptrOut[ptr++] = x; else break;
if(len--) ptrOut[ptr++] = y; else break;
if(len--) ptrOut[ptr++] = z; else break;
}
}
 
// --------------------------------------------------------------------------
void BearbeiteRxDaten(void)
/**************************************************************/
/* Process incomming data from debug uart */
/**************************************************************/
void UART1_ProcessRxData(void)
{
if(!NeuerDatensatzEmpfangen) return;
// if data in the rxd buffer are not locked immediately return
if(!rxd_buffer_locked) return;
// u16 tmp_int_arr1[1];
// u16 tmp_int_arr2[2];
// u16 tmp_int_arr3[3];
u8 tmp_char_arr2[2];
s32 tmp_long_arr2[2];
// u8 tmp_char_arr3[3];
// u8 tmp_char_arr4[4];
//if(!MotorenEin)
PcZugriff = 255;
switch(RxdBuffer[2])
{
case 'a':// Texte der Analogwerte
Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);
DebugTextAnforderung = tmp_char_arr2[0];
u8 tmp_char_arr2[2];
s32 tmp_long_arr3[2];
 
PcAccess = 255;
switch(rxd_buffer[2])
{
case 'a':// Labels of the Analog Debug outputs
Decode64((u8 *) &tmp_char_arr2[0], sizeof(tmp_char_arr2),3 ,ReceivedBytes);
Request_DebugLabel = tmp_char_arr2[0];
break;
case 'b':
Decode64((unsigned char *) &ExternControl,sizeof(ExternControl),3,AnzahlEmpfangsBytes);
RemoteTasten |= ExternControl.RemoteTasten;
ConfirmFrame = ExternControl.Frame;
break;
case 'c':
Decode64((unsigned char *) &ExternControl,sizeof(ExternControl),3,AnzahlEmpfangsBytes);
RemoteTasten |= ExternControl.RemoteTasten;
ConfirmFrame = ExternControl.Frame;
DebugDataAnforderung = 1;
break;
case 'h':// x-1 Displayzeilen
Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);
RemoteTasten |= tmp_char_arr2[0];
if(tmp_char_arr2[1] == 255) NurKanalAnforderung = 1; else NurKanalAnforderung = 0; // keine Displaydaten
DebugDisplayAnforderung = 1;
case 'b': // extern control
Decode64((u8 *) &ExternControl, sizeof(ExternControl), 3 ,ReceivedBytes);
RemoteButtons |= ExternControl.RemoteButtons;
ConfirmFrame = ExternControl.Frame;
break;
case 't':// Motortest
Decode64((unsigned char *) &MotorTest[0],sizeof(MotorTest),3,AnzahlEmpfangsBytes);
case 'c': // extern control with debug request
Decode64((u8 *) &ExternControl, sizeof(ExternControl), 3, ReceivedBytes);
RemoteButtons |= ExternControl.RemoteButtons;
ConfirmFrame = ExternControl.Frame;
Request_DebugData = TRUE;
break;
case 's':// neue PCTargetGPSPosition
Decode64((unsigned char *) &tmp_long_arr2[0],sizeof(tmp_long_arr2),3,AnzahlEmpfangsBytes);
PCTargetGPSPosition.Longitude = tmp_long_arr2[0];
PCTargetGPSPosition.Latitude = tmp_long_arr2[1];
NewPCTargetGPSPosition = 1;
case 'h':// x-1 display columns
Decode64((u8 *) &tmp_char_arr2[0], sizeof(tmp_char_arr2), 3, ReceivedBytes);
RemoteButtons |= tmp_char_arr2[0];
if(tmp_char_arr2[1] == 255) Request_ChannelOnly = 1; else Request_ChannelOnly = 0; // no displaydata
Request_Display = TRUE;
break;
case 'k':// Keys von DubWise
// Decode64((unsigned char *) &DubWiseKeys[0],sizeof(DubWiseKeys),3,AnzahlEmpfangsBytes);
ConfirmFrame = 1;
case 's':// new PCGPSTargetPosition
Decode64((unsigned char *) &tmp_long_arr3[0], sizeof(tmp_long_arr3), 3,ReceivedBytes);
PCGPSTargetPosition.Longitude = tmp_long_arr3[0];
PCGPSTargetPosition.Latitude = tmp_long_arr3[1];
PCGPSTargetPosition.Status = NEWDATA;
break;
case 'v': // Version-Anforderung und Ausbaustufe
GetVersionAnforderung = 1;
case 'v': // get version
Request_VerInfo = TRUE;
break;
case 'g':// "Get"-Anforderung für Debug-Daten
// Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
DebugGetAnforderung = 1;
case 'g':// get external control data
Request_ExternalControl = TRUE;
break;
case 'l':
case 'm':
case 'n':
case 'o':
case 'p': // Parametersatz speichern
/* Decode64((u8 *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE,3,AnzahlEmpfangsBytes);
WriteParameterSet(RxdBuffer[2] - 'l' + 1, (u8 *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], RxdBuffer[2] - 'l' + 1); // aktiven Datensatz merken
Piep(GetActiveParamSetNumber());*/
break;
}
// DebugOut.AnzahlZyklen = Debug_Timer_Intervall;
NeuerDatensatzEmpfangen = 0;
case 'l':
case 'm':
case 'n':
case 'o':
case 'p':
break;
}
// unlock the rxd buffer after processing
rxd_buffer_locked = FALSE;
}
 
//############################################################################
//Routine für die Serielle Ausgabe
/*****************************************************/
/* Send a character */
/*****************************************************/
s16 uart_putchar (char c)
//############################################################################
{
if (c == '\n')
uart_putchar('\r');
// wait until txd fifo is not full
while (UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != RESET);
UART_SendData(UART1, c);
// transmit byte
UART_SendData(UART1, c);
return (0);
}
// --------------------------------------------------------------------------
 
/*****************************************************/
/* Send a sting to the debug uart */
/*****************************************************/
void SerialPutString(u8 *s)
{
while (*s != '\0')
427,116 → 463,51
}
}
 
//############################################################################
//Init der Seriellen Schnittstelle
void Debug_UART1_Init (void)
//############################################################################
{
GPIO_InitTypeDef GPIO_InitStructure;
 
SCU_APBPeriphClockConfig(__UART1, ENABLE); // Enable the UART1 Clock
SCU_APBPeriphClockConfig(__GPIO3, ENABLE); // Enable the GPIO3 Clock
 
/*Configure UART1_Rx pin GPIO3.2*/
GPIO_DeInit(GPIO3);
GPIO_InitStructure.GPIO_Direction = GPIO_PinInput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
GPIO_InitStructure.GPIO_IPConnected = GPIO_IPConnected_Enable;
GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1 ;
GPIO_Init(GPIO3, &GPIO_InitStructure);
 
/*Configure UART1_Tx pin GPIO3.3*/
GPIO_InitStructure.GPIO_Direction = GPIO_PinOutput;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
GPIO_InitStructure.GPIO_Type = GPIO_Type_PushPull ;
GPIO_InitStructure.GPIO_Alternate = GPIO_OutputAlt2 ;
GPIO_Init(GPIO3, &GPIO_InitStructure);
 
/* UART1 configured as follow:
- Word Length = 8 Bits
- One Stop Bit
- No parity
- BaudRate = 57600 baud
- Hardware flow control Disabled
- Receive and transmit enabled
- Receive and transmit FIFOs are Disabled
*/
UART_InitStructure.UART_WordLength = UART_WordLength_8D;
UART_InitStructure.UART_StopBits = UART_StopBits_1;
UART_InitStructure.UART_Parity = UART_Parity_No ;
UART_InitStructure.UART_BaudRate = BAUD_RATE ;
UART_InitStructure. UART_HardwareFlowControl = UART_HardwareFlowControl_None;
UART_InitStructure.UART_Mode = UART_Mode_Tx_Rx;
//UART_InitStructure.UART_FIFO = UART_FIFO_Disable;
UART_InitStructure.UART_FIFO = UART_FIFO_Enable;
UART_InitStructure.UART_TxFIFOLevel = UART_FIFOLevel_1_2;
UART_InitStructure.UART_RxFIFOLevel = UART_FIFOLevel_1_2;
 
UART_DeInit(UART1);
UART_Init(UART1, &UART_InitStructure);
 
UART_ITConfig(UART1, UART_IT_Receive | UART_IT_ReceiveTimeOut /*| UART_IT_Transmit */, ENABLE);
//UART_ITConfig(UART1, UART_IT_Receive , ENABLE);
 
 
UART_Cmd(UART1, ENABLE);
 
VIC_Config(UART1_ITLine, VIC_IRQ, 4);
VIC_ITCmd(UART1_ITLine, ENABLE);
 
Debug_Timer = SetDelay(3000);
//----------------------------------------
 
}
 
 
//---------------------------------------------------------------------------------------------
void DatenUebertragung(void)
/**************************************************************/
/* Send the answers to incomming commands at the debug uart */
/**************************************************************/
void UART1_TransmitTxData(void)
{
if(!UebertragungAbgeschlossen) return;
if(!txd_complete) return;
 
if(DebugGetAnforderung && UebertragungAbgeschlossen) // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
{
SendOutData('G',MeineSlaveAdresse,(unsigned char *) &ExternControl,sizeof(ExternControl));
DebugGetAnforderung = 0;
}
 
if((CheckDelay(Debug_Timer) || DebugDataAnforderung) && UebertragungAbgeschlossen)
{
SendOutData('D',MeineSlaveAdresse,(unsigned char *) &DebugOut,sizeof(DebugOut));
DebugDataAnforderung = 0;
Debug_Timer = SetDelay(2000);
}
if(DebugTextAnforderung != 255) // Texte für die Analogdaten
{
SendOutData('A',DebugTextAnforderung + '0',(unsigned char *) ANALOG_TEXT[DebugTextAnforderung],16);
DebugTextAnforderung = 255;
}
if(DebugDisplayAnforderung && UebertragungAbgeschlossen)
{
Menu();
DebugDisplayAnforderung = 0;
if(++RemotePollDisplayLine == 4 || NurKanalAnforderung)
{
SendOutData('4',0,(unsigned char *) &VersionInfo,sizeof(VersionInfo)); // DisplayZeile übertragen
RemotePollDisplayLine = -1;
}
else SendOutData('0' + RemotePollDisplayLine,0,(unsigned char *)&DisplayBuff[20 * RemotePollDisplayLine],20); // DisplayZeile übertragen
}
if(GetVersionAnforderung && UebertragungAbgeschlossen)
{
SendOutData('V',MeineSlaveAdresse,(unsigned char *) &VersionInfo,sizeof(VersionInfo));
GetVersionAnforderung = 0;
}
if(SendGPSPosAnforderung && UebertragungAbgeschlossen)
{
SendOutData('Q',MeineSlaveAdresse,(unsigned char *) &GPS_Position,sizeof(GPS_Position));
SendGPSPosAnforderung = 0;
}
 
if(Request_ExternalControl && txd_complete) // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
{
SendOutData('G',MySlaveAddr,(u8 *) &ExternControl,sizeof(ExternControl));
Request_ExternalControl = FALSE;
}
if((CheckDelay(Debug_Timer) || Request_DebugData) && txd_complete)
{
SendOutData('D',MySlaveAddr,(u8 *) &DebugOut,sizeof(DebugOut));
Request_DebugData = FALSE;
Debug_Timer = SetDelay(2000);
}
if(Request_DebugLabel != 255) // Texte für die Analogdaten
{
SendOutData('A',Request_DebugLabel + '0',(u8 *) ANALOG_LABEL[Request_DebugLabel],16);
Request_DebugLabel = 255;
}
if(Request_Display && txd_complete)
{
LCD_PrintMenu();
Request_Display = FALSE;
if(++Remote_PollDisplayLine == 4 || Request_ChannelOnly)
{
SendOutData('4',0,(u8 *) &VersionInfo,sizeof(VersionInfo)); // DisplayZeile übertragen
Remote_PollDisplayLine = -1;
}
else SendOutData('0' + Remote_PollDisplayLine,0,(u8 *)&DisplayBuff[20 * Remote_PollDisplayLine],20); // DisplayZeile übertragen
}
if(Request_VerInfo && txd_complete)
{
SendOutData('V',MySlaveAddr,(u8 *) &VersionInfo,sizeof(VersionInfo));
Request_VerInfo = FALSE;
}
if(Request_GPS_Position && txd_complete)
{
SendOutData('Q',MySlaveAddr,(u8 *) &GPS_Position,sizeof(GPS_Position));
Request_GPS_Position = FALSE;
}
}
 
/branches/V0.1 killagreg/uart.h
1,52 → 1,35
#ifndef _UART_H
#define _UART_H
#define _UART_H
 
#define MAX_SENDE_BUFF 150
#define MAX_EMPFANGS_BUFF 150
extern u8 DebugGetAnforderung;
extern unsigned volatile char SendeBuffer[MAX_SENDE_BUFF];
extern unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF];
extern unsigned volatile char UebertragungAbgeschlossen;
extern unsigned volatile char PC_DebugTimeout;
extern unsigned volatile char NeueKoordinateEmpfangen;
extern unsigned volatile char AnzahlEmpfangsBytes;
extern unsigned volatile char SendGPSPosAnforderung;
extern u8 MeineSlaveAdresse;
extern u8 PcZugriff;
extern s32 Debug_Timer;
extern void UART1_Init(void);
extern s16 uart_putchar (char c);
extern void DatenUebertragung(void);
extern void DecodeNMEA(void);
extern u8 MotorTest[4];
#define TXD_BUFFER_LEN 150
#define RXD_BUFFER_LEN 150
#define BAUD_RATE 57600 //Baud Rate for the serial interfaces
 
extern UART_TypeDef *DebugUART;
 
struct str_DebugOut
typedef struct
{
u8 Digital[2];
u16 Analog[32]; // Debugwerte
} __attribute__((packed));
} __attribute__((packed)) DebugOut_t;
 
extern volatile struct str_DebugOut DebugOut;
extern DebugOut_t DebugOut;
 
struct str_ExternControl
typedef struct
{
unsigned char Digital[2];
unsigned char RemoteTasten;
signed char Nick;
signed char Roll;
signed char Gier;
unsigned char Gas;
signed char Hight;
unsigned char free;
unsigned char Frame;
unsigned char Config;
} __attribute__((packed));
extern struct str_ExternControl ExternControl;
u8 Digital[2];
u8 RemoteButtons;
s8 Pitch;
s8 Roll;
s8 Yaw;
u8 Thrust;
s8 Height;
u8 free;
u8 Frame;
u8 Config;
} __attribute__((packed)) ExternControl_t;
 
struct str_GPSPosition
extern ExternControl_t ExternControl;
 
typedef struct GPSPosition
{
s32 Longitude;
s32 Latitude;
55,37 → 38,36
s32 Distance2Target;
s32 Angle2Target;
u8 Used_Sat;
} __attribute__((packed));
} __attribute__((packed)) GPSPosition_t;
 
extern struct str_GPSPosition GPS_Position;
extern u8 Request_GPS_Position;
extern GPSPosition_t GPS_Position;
 
struct str_PCTargetGPSPosition
typedef struct
{
signed long Longitude;
signed long Latitude;
};
struct str_PCTargetGPSPosition PCTargetGPSPosition;
u8 Major;
u8 Minor;
u8 Patch;
u8 Reserved[7];
} __attribute__((packed)) VersionInfo_t;
 
struct str_VersionInfo
{
u8 Hauptversion;
u8 Nebenversion;
u8 PCKompatibel;
u8 Rserved[7];
} __attribute__((packed));
extern struct str_VersionInfo VersionInfo;
extern VersionInfo_t VersionInfo;
 
extern unsigned volatile char NeuerDatensatzEmpfangen;
extern volatile u8 rxd_buffer[RXD_BUFFER_LEN];
extern volatile u8 ReceivedBytes;
extern volatile u8 rxd_buffer_locked;
 
 
extern void BearbeiteRxDaten(void);
extern void UART1_IRQHandler_Fkt(void);
 
extern void UART1_Init(void);
extern void UART1_Transmit(void);
extern void Debug_UART1_Init(void);
extern void UART1_TransmitTxData(void);
extern void UART1_ProcessRxData(void);
 
extern s16 uart_putchar (char c);
extern void SerialPutString(u8 *s);
extern u8 text[20];
 
#define BAUD_RATE 57600 //Baud Rate für die Serielle Schnittstelle
 
 
 
#endif //_UART_H
/branches/V0.1 killagreg/usb.c
54,7 → 54,9
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
#include "91x_lib.h"
#include "usb.h"
#include "uart.h"
 
//-----------------------------------------------------------------
void USB_ConfigInit(void)
128,10 → 130,7
//-----------------------------------------------------------------
void USB_Send_Data(u8 *data, u16 count)
{
u16 timeout = 0;
u8 i;
 
count++;
for (i=0;i< (count/64)+1;i++)
/branches/V0.1 killagreg/usb.h
1,5 → 1,5
#ifndef _USB_H
#define _USB_H
#define _USB_H
 
#include "usb_lib.h"
#include "usb_conf.h"
/branches/V0.1 killagreg/usb_desc.c
15,7 → 15,8
* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*******************************************************************************/
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "usb_lib.h"
#include "usb_desc.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/branches/V0.1 killagreg/usb_endp.c
15,7 → 15,10
* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*******************************************************************************/
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include <stdio.h>
#include "usb_lib.h"
#include "usb_desc.h"
#include "uart.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
36,13 → 39,13
void EP3_OUT_Callback(void)
{
//USB_BufferRxCount= GetEPRxCount(ENDP3);
AnzahlEmpfangsBytes = GetEPRxCount(ENDP3);
ReceivedBytes = GetEPRxCount(ENDP3);
//PMAToUserBufferCopy(USB_BufferRx, ENDP3_RXADDR, AnzahlEmpfangsBytes );
PMAToUserBufferCopy((u8*) RxdBuffer, ENDP3_RXADDR, AnzahlEmpfangsBytes);
NeuerDatensatzEmpfangen = 1;
PMAToUserBufferCopy((u8*) rxd_buffer, ENDP3_RXADDR, ReceivedBytes);
rxd_buffer_locked = 1;
//USB_BufferRx[AnzahlEmpfangsBytes] = 0;
SetEPRxValid(ENDP3);
sprintf(text,"USB (%d): %s\n\r", AnzahlEmpfangsBytes,RxdBuffer ); SerialPutString(text);
sprintf(text,"USB (%d): %s\n\r", ReceivedBytes, rxd_buffer ); SerialPutString(text);
// USB_Send_Data(RxdBuffer, AnzahlEmpfangsBytes);
// USB_Send_String("Rx.\0");
/branches/V0.1 killagreg/usb_istr.c
15,7 → 15,8
* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*******************************************************************************/
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "usb_lib.h"
#include "usb_pwr.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/branches/V0.1 killagreg/usb_prop.c
16,7 → 16,10
*******************************************************************************/
 
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "usb_lib.h"
#include "usb_desc.h"
#include "usb_pwr.h"
#include "usb_prop.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/branches/V0.1 killagreg/usb_pwr.c
15,7 → 15,9
* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*******************************************************************************/
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "usb_lib.h"
#include "hw_config.h"
#include "usb_pwr.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/