Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 725 → Rev 726

/branches/V0.68d Code Redesign killagreg/GPS.c
1,30 → 1,116
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + only for non-profit use
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
#include <inttypes.h>
 
signed int GPS_Pitch = 0;
signed int GPS_Roll = 0;
long GpsReading_X = 0;
long GpsReading_Y = 0;
long GpsTarget_X = 0;
long GpsTarget_Y = 0;
#include "fc.h"
#include "ubx.h"
#include "mymath.h"
 
void GPS_Neutral(void)
int16_t GPS_Pitch = 0;
int16_t GPS_Roll = 0;
 
uint8_t GPS_P_Factor = 0;
uint8_t GPS_D_Factor = 0;
 
typedef struct
{
GpsTarget_X = GpsReading_X;
GpsTarget_Y = GpsReading_Y;
}
int32_t Northing;
int32_t Easting;
uint8_t Status;
 
void GPS_CalcTargetDirection(void)
} GPS_Pos_t;
 
// GPS coordinates for hold position
GPS_Pos_t GPSHoldPoint = {0,0, INVALID};
// GPS coordinates for flying home
GPS_Pos_t GPSHomePoint = {0,0, INVALID};;
 
 
 
// ---------------------------------------------------------------------------------
 
 
void GPS_Main(void)
{
GPS_Pitch = 0;
GPS_Roll = 0;
}
int32_t coscompass, sincompass;
int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
int32_t PD_North = 0, PD_East = 0;
int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
 
// poti2 enables the gps feature
if(Poti2 > 70) // run GPS function only if Poti 2 is larger than 70 (switch on)
{
switch(GPSInfo.status)
{
case INVALID: // invalid gps data
GPS_Pitch = 0;
GPS_Roll = 0;
break;
case PROCESSED: // if gps data are already processed
// downcount timeout
if(GPSTimeout) GPSTimeout--;
// if no new data arrived within timeout set current data invalid
// and therefore disable GPS
else
{
GPS_Pitch = 0;
GPS_Roll = 0;
GPSInfo.status = INVALID;
}
break;
case VALID: // new valid data from gps device
// if the gps data quality is sufficient
if (GPSInfo.satfix == SATFIX_3D)
{
// if sticks are centered and hold position is valid enable position hold
if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (GPSHoldPoint.Status == VALID))
{
coscompass = (int32_t)c_cos_8192(CompassHeading);
sincompass = (int32_t)c_sin_8192(CompassHeading);
 
// Calculate deviation from hold position
GPSPosDev_North = GPSInfo.utmnorth - GPSHoldPoint.Northing;
GPSPosDev_East = GPSInfo.utmeast - GPSHoldPoint.Easting;
 
//Calculate PD-components of the controller
P_North = (GPS_P_Factor * GPSPosDev_North)/2048;
D_North = (GPS_D_Factor * GPSInfo.velnorth)/255;
P_East = (GPS_P_Factor * GPSPosDev_East)/2048;
D_East = (GPS_D_Factor * GPSInfo.veleast)/255;
 
// PD-controller
PD_North = (-P_North + D_North);
PD_East = (P_East - D_East);
 
// GPS to pitch and roll
GPS_Pitch = (int16_t)((coscompass * PD_North - sincompass * PD_East) / 8192);
GPS_Roll = (int16_t)((sincompass * PD_North + coscompass * PD_East) / 8192);
 
// limit GPS controls
if (GPS_Pitch > 35) GPS_Pitch = 35;
if (GPS_Pitch < -35) GPS_Pitch = -35;
if (GPS_Roll > 35) GPS_Roll = 35;
if (GPS_Roll < -35) GPS_Roll = -35;
}
else // MK controlled by user
{
// update hold point to current gps position
GPSHoldPoint.Northing = GPSInfo.utmnorth;
GPSHoldPoint.Easting = GPSInfo.utmeast;
GPSHoldPoint.Status = VALID;
// disable gps control
GPS_Pitch = 0;
GPS_Roll = 0;
}
} // eof 3D-FIX
else // no 3D-SATFIX
{ // diable gps control
GPS_Pitch = 0;
GPS_Roll = 0;
}
// set current data as processed to avoid further calculations on the same gps data
GPSInfo.status = PROCESSED;
break;
} // eof GPSInfo.status
}
return;
}
 
/branches/V0.68d Code Redesign killagreg/fc.c
201,7 → 201,6
HightD = 0;
Reading_Integral_Top = 0;
CompassCourse = CompassHeading;
GPS_Neutral();
BeepTime = 50;
TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
392,7 → 391,7
//Start I2C Interrupt Mode
twi_state = 0;
motor = 0;
i2c_start();
I2C_Start();
}
 
 
1018,7 → 1017,7
{
updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)
// get current compass heading (angule between MK head and magnetic north)
CompassHeading = MM3_heading();
CompassHeading = MM3_Heading();
// calculate OffCourse (angular deviation from heading to course)
CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
 
1042,6 → 1041,21
}
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// GPS
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
{
GPS_P_Factor = FCParam.UserParam5;
GPS_D_Factor = FCParam.UserParam6;
GPS_Main(); // updates GPS_Pitch and GPS_Roll on new GPS data
}
else
{
GPS_Pitch = 0;
GPS_Roll = 0;
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!TimerDebugOut--)
/branches/V0.68d Code Redesign killagreg/fc.h
74,14 → 74,15
void Beep(uint8_t numbeeps);
 
 
//extern unsigned char h,m,s;
 
extern int16_t Poti1, Poti2, Poti3, Poti4;
 
// setpoints for motors
extern volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; //used by twimaster isr
 
// current stick values
extern int16_t StickPitch, StickRoll, StickYaw;
// current stick elongations
extern int16_t MaxStickPitch, MaxStickRoll, MaxStickYaw;
extern uint8_t MotorsOn;
 
 
/branches/V0.68d Code Redesign killagreg/gps.h
1,11 → 1,15
#ifndef _GPS_H
#define _GPS_H
 
extern signed int GPS_Pitch;
extern signed int GPS_Roll;
#include <inttypes.h>
 
void GPS_Neutral(void);
void GPS_CalcTargetDirection(void);
extern int16_t GPS_Pitch;
extern int16_t GPS_Roll;
 
extern uint8_t GPS_P_Factor;
extern uint8_t GPS_D_Factor;
 
extern void GPS_Main(void);
 
#endif //_GPS_H
 
/branches/V0.68d Code Redesign killagreg/main.c
115,12 → 115,11
TIMER0_Init();
TIMER2_Init();
USART0_Init();
ubx_init();
if (BoardRelease == 11) USART1_Init();
RC_Init();
ADC_Init();
i2c_init();
MM3_init();
I2C_Init();
MM3_Init();
//SPI_MasterInit();
 
// enable interrupts global
150,7 → 149,7
if(PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] > 100 && PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 100)
{
printf("\n\rCalibrating Compass");
MM3_calibrate();
MM3_Calibrate();
}
 
 
206,7 → 205,7
if(!I2CTimeout)
{
I2CTimeout = 5;
i2c_reset();
I2C_Reset();
if((BeepModulation == 0xFFFF) && MotorsOn)
{
BeepTime = 10000; // 1 second
/branches/V0.68d Code Redesign killagreg/mm3.c
67,7 → 67,7
/*********************************************/
/* Initialize Interface to MM3 Compass */
/*********************************************/
void MM3_init(void)
void MM3_Init(void)
{
uint8_t sreg = SREG;
 
114,7 → 114,7
/*********************************************/
/* Get Data from MM3 */
/*********************************************/
void MM3_timer0() // called every 102.4 ms by timer 0 ISR
void MM3_Update() // called every 102.4 ms by timer 0 ISR
{
switch (MM3.STATE)
{
202,7 → 202,7
/*********************************************/
/* Calibrate Compass */
/*********************************************/
void MM3_calibrate(void)
void MM3_Calibrate(void)
{
int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0;
uint8_t measurement = 50, beeper = 0;
263,7 → 263,7
/*********************************************/
/* Calculate north direction (heading) */
/*********************************************/
int16_t MM3_heading(void)
int16_t MM3_Heading(void)
{
int32_t sin_pitch, cos_pitch, sin_roll, cos_roll, sin_yaw, cos_yaw;
int32_t Hx, Hy, Hz, Hx_corr, Hy_corr;
/branches/V0.68d Code Redesign killagreg/mm3.h
15,10 → 15,10
 
extern MM3_calib_t MM3_calib;
 
void MM3_init(void);
void MM3_timer0(void);
void MM3_calibrate(void);
int16_t MM3_heading(void);
void MM3_Init(void);
void MM3_Update(void);
void MM3_Calibrate(void);
int16_t MM3_Heading(void);
 
#endif //_MM3_H
 
/branches/V0.68d Code Redesign killagreg/mymath.h
1,8 → 1,10
#ifndef _MYMATH_H
#define _MYMATH_H
 
#include <inttypes.h>
 
extern int16_t c_sin_8192(int16_t angle);
extern int16_t c_cos_8192(int16_t angle);
extern int16_t c_atan2(signed int y, signed int x);
extern int16_t c_atan2(int16_t y, int16_t x);
 
#endif // _MYMATH_H
/branches/V0.68d Code Redesign killagreg/timer0.c
138,7 → 138,7
// update compass value if this option is enabled in the settings
if(ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE)
{
MM3_timer0(); // read out mm3 board
MM3_Update(); // read out mm3 board
}
}
 
/branches/V0.68d Code Redesign killagreg/twimaster.c
15,7 → 15,7
/**************************************************/
/* Initialize I2C (TWI) */
/**************************************************/
void i2c_init(void)
void I2C_Init(void)
{
uint8_t sreg = SREG;
cli();
40,7 → 40,7
/****************************************/
/* Start I2C */
/****************************************/
void i2c_start(void)
void I2C_Start(void)
{
// TWI Control Register
// clear TWI interrupt flag (TWINT=1)
56,7 → 56,7
/****************************************/
/* Stop I2C */
/****************************************/
void i2c_stop(void)
void I2C_Stop(void)
{
// TWI Control Register
// clear TWI interrupt flag (TWINT=1)
72,10 → 72,10
/****************************************/
/* Reset I2C */
/****************************************/
void i2c_reset(void)
void I2C_Reset(void)
{
// stop i2c bus
i2c_stop();
I2C_Stop();
twi_state = 0;
motor = TWDR; // ??
motor = 0;
85,15 → 85,15
TWDR = 0;
TWSR = 0;
TWBR = 0;
i2c_init();
i2c_start();
i2c_write_byte(0);
I2C_Init();
I2C_Start();
I2C_WriteByte(0);
}
 
/****************************************/
/* Write to I2C */
/****************************************/
void i2c_write_byte(int8_t byte)
void I2C_WriteByte(int8_t byte)
{
// move byte to send into TWI Data Register
TWDR = byte;
107,7 → 107,7
/****************************************/
/* Receive byte and send ACK */
/****************************************/
void i2c_receive_byte(void)
void I2C_ReceiveByte(void)
{
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);
}
115,7 → 115,7
/****************************************/
/* I2C receive last byte and send no ACK*/
/****************************************/
void i2c_receive_last_byte(void)
void I2C_ReceiveLastByte(void)
{
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
}
133,41 → 133,41
{
// Master Transmit
case 0: // Send SLA-W
i2c_write_byte(0x52+(motor*2));
I2C_WriteByte(0x52+(motor*2));
break;
case 1: // Send Data to Salve
switch(motor++)
{
case 0:
i2c_write_byte(Motor_Front);
I2C_WriteByte(Motor_Front);
break;
case 1:
i2c_write_byte(Motor_Rear);
I2C_WriteByte(Motor_Rear);
break;
case 2:
i2c_write_byte(Motor_Right);
I2C_WriteByte(Motor_Right);
break;
case 3:
i2c_write_byte(Motor_Left);
I2C_WriteByte(Motor_Left);
break;
}
break;
case 2: // repeat case 0+1 for all Slaves
if (motor<4) twi_state = 0;
i2c_start(); // Repeated start -> switch salve or switch Master Transmit -> Master Receive
I2C_Start(); // Repeated start -> switch salve or switch Master Transmit -> Master Receive
break;
 
// Master Receive
case 3: // Send SLA-R
i2c_write_byte(0x53+(motorread*2));
I2C_WriteByte(0x53+(motorread*2));
break;
case 4:
//Transmit 1st byte
i2c_receive_byte();
I2C_ReceiveByte();
break;
case 5: //Read 1st byte and transmit 2nd Byte
motor_rx[motorread] = TWDR;
i2c_receive_last_byte();
I2C_ReceiveLastByte();
break;
case 6:
//Read 2nd byte
176,7 → 176,7
if (motorread > 3) motorread=0;
 
default:
i2c_stop();
I2C_Stop();
twi_state = 0;
I2CTimeout = 10;
motor = 0;
/branches/V0.68d Code Redesign killagreg/twimaster.h
24,11 → 24,10
extern volatile uint8_t motorread;
extern volatile uint8_t motor_rx[8];
 
void i2c_reset(void);
extern void i2c_init (void); // I2C initialisieren
extern void i2c_start (void); // Start I2C
extern void i2c_stop (void); // Stop I2C
extern void i2c_write_byte (int8_t byte); // 1 Byte schreiben
extern void i2c_reset(void);
extern void I2C_Init (void); // Initialize I2C
extern void I2C_Start (void); // Start I2C
extern void I2C_Stop (void); // Stop I2C
extern void I2C_WriteByte (int8_t byte); // Write 1 Byte
extern void I2C_Reset(void); // Reset I2C
 
#endif
/branches/V0.68d Code Redesign killagreg/ubx.c
71,12 → 71,13
} GPS_VELNED_t;
 
 
GPS_STATUS_t GpsStatus;
GPS_POSLLH_t GpsPosLlh;
GPS_POSUTM_t GpsPosUtm;
GPS_VELNED_t GpsVelNed;
GPS_STATUS_t GpsStatus = {0,0,0,0,0,0,0, INVALID};
GPS_POSLLH_t GpsPosLlh = {0,0,0,0,0,0,0, INVALID};
GPS_POSUTM_t GpsPosUtm = {0,0,0,0,0,0, INVALID};
GPS_VELNED_t GpsVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
GPS_INFO_t GPSInfo = {0,0,0,0,0,0,0,0,0,0, INVALID};
 
GPS_INFO_t GPSInfo;
volatile uint8_t GPSTimeout = 0;
 
void UpdateGPSInfo (void)
{
83,7 → 84,7
if (GpsStatus.Status == VALID) // valid packet
{
GPSInfo.satfix = GpsStatus.GPSfix;
GpsStatus.Status = PROCESSED; // never update old data
GpsStatus.Status = PROCESSED; // never update old data
}
if (GpsPosLlh.Status == VALID) // valid packet
{
90,7 → 91,7
GPSInfo.longitude = GpsPosLlh.LON;
GPSInfo.latitude = GpsPosLlh.LAT;
GPSInfo.altitude = GpsPosLlh.HEIGHT;
GpsPosLlh.Status = PROCESSED; // never update old data
GpsPosLlh.Status = PROCESSED; // never update old data
}
if (GpsPosUtm.Status == VALID) // valid packet
{
97,7 → 98,7
GPSInfo.utmeast = GpsPosUtm.EAST;
GPSInfo.utmnorth = GpsPosUtm.NORTH;
GPSInfo.utmalt = GpsPosUtm.ALT;
GpsPosUtm.Status = PROCESSED; // never update old data
GpsPosUtm.Status = PROCESSED; // never update old data
}
if (GpsVelNed.Status == VALID) // valid packet
{
104,22 → 105,14
GPSInfo.veleast = GpsVelNed.VEL_E;
GPSInfo.velnorth = GpsVelNed.VEL_N;
GPSInfo.veltop = -GpsVelNed.VEL_D;
GpsVelNed.Status = PROCESSED; // never update old data
GpsVelNed.Status = PROCESSED; // never update old data
}
if (GpsStatus.Status != INVALID)
{
GPSInfo.status = VALID; // set valid if data are updated
GPSInfo.status = VALID; // set valid if data are updated
}
}
 
void ubx_init(void)
{
GpsStatus.Status = INVALID;
GpsPosLlh.Status = INVALID;
GpsPosUtm.Status = INVALID;
GpsVelNed.Status = INVALID;
GPSInfo.status = INVALID;
}
 
// this function should be called within the UART RX ISR
void ubx_parser(uint8_t c)
228,6 → 221,7
*ubxSp = VALID; // new data are valid
ROT_FLASH;
UpdateGPSInfo(); //update GPS info respectively
GPSTimeout = 255;
}
else
{ // if checksum not fit then set data invalid
/branches/V0.68d Code Redesign killagreg/ubx.h
1,6 → 1,8
#ifndef _UBX_H
#define _UBX_H
 
#include <inttypes.h>
 
#define INVALID 0x00
#define VALID 0x01
#define PROCESSED 0x02
39,6 → 41,9
//here you will find the current gps info
extern GPS_INFO_t GPSInfo; // measured position (last gps record)
 
// this variable should be decremted by the application
extern volatile uint8_t GPSTimeout; // is reset to 255 if a new UBX msg was received
 
// initialized the upx parser
extern void ubx_init(void);
 
/branches/V0.68d Code Redesign killagreg/version.txt
123,5 → 123,14
(Userparameter3 zur Skalierung der Lageintegrale zu Winkeln - Richtwert 170)
(Userparameter4 zur Angabe des Winkels vom MM3-Borad bzgl. der Nase des MK)
- eineige unbedeutende kleine Bugfixes
- GPS-Hold-Funktion hinzugefügt
- Poti2 is used for Enabling the GPS Hold Feature (Value > 70)
 
- User Parameters:
Parameter 3 --> Calibration factor for transforming Gyro Integrals to angular degrees
Parameter 4 --> Angle between the MM3 Board (Arrow) and the MK head
Parameter 5 --> P-Factor for GPS PD controller
Parameter 6 --> D-Factor for GPS PD controller