/branches/V0.68d Code Redesign killagreg/GPS.c |
---|
1,116 → 1,30 |
#include <inttypes.h> |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "fc.h" |
#include "ubx.h" |
#include "mymath.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; |
int16_t GPS_Pitch = 0; |
int16_t GPS_Roll = 0; |
uint8_t GPS_P_Factor = 0; |
uint8_t GPS_D_Factor = 0; |
typedef struct |
void GPS_Neutral(void) |
{ |
int32_t Northing; |
int32_t Easting; |
uint8_t Status; |
GpsTarget_X = GpsReading_X; |
GpsTarget_Y = GpsReading_Y; |
} |
} 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) |
void GPS_CalcTargetDirection(void) |
{ |
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,6 → 201,7 |
HightD = 0; |
Reading_Integral_Top = 0; |
CompassCourse = CompassHeading; |
GPS_Neutral(); |
BeepTime = 50; |
TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
391,7 → 392,7 |
//Start I2C Interrupt Mode |
twi_state = 0; |
motor = 0; |
I2C_Start(); |
i2c_start(); |
} |
1017,7 → 1018,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; |
1041,21 → 1042,6 |
} |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// 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,15 → 74,14 |
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,15 → 1,11 |
#ifndef _GPS_H |
#define _GPS_H |
#include <inttypes.h> |
extern signed int GPS_Pitch; |
extern signed int GPS_Roll; |
extern int16_t GPS_Pitch; |
extern int16_t GPS_Roll; |
void GPS_Neutral(void); |
void GPS_CalcTargetDirection(void); |
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,11 → 115,12 |
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 |
149,7 → 150,7 |
if(PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] > 100 && PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 100) |
{ |
printf("\n\rCalibrating Compass"); |
MM3_Calibrate(); |
MM3_calibrate(); |
} |
205,7 → 206,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_Update() // called every 102.4 ms by timer 0 ISR |
void MM3_timer0() // 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_Update(void); |
void MM3_Calibrate(void); |
int16_t MM3_Heading(void); |
void MM3_init(void); |
void MM3_timer0(void); |
void MM3_calibrate(void); |
int16_t MM3_heading(void); |
#endif //_MM3_H |
/branches/V0.68d Code Redesign killagreg/mymath.h |
---|
1,10 → 1,8 |
#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(int16_t y, int16_t x); |
extern int16_t c_atan2(signed int y, signed int 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_Update(); // read out mm3 board |
MM3_timer0(); // 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_WriteByte(0); |
i2c_init(); |
i2c_start(); |
i2c_write_byte(0); |
} |
/****************************************/ |
/* Write to I2C */ |
/****************************************/ |
void I2C_WriteByte(int8_t byte) |
void i2c_write_byte(int8_t byte) |
{ |
// move byte to send into TWI Data Register |
TWDR = byte; |
107,7 → 107,7 |
/****************************************/ |
/* Receive byte and send ACK */ |
/****************************************/ |
void I2C_ReceiveByte(void) |
void i2c_receive_byte(void) |
{ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA); |
} |
115,7 → 115,7 |
/****************************************/ |
/* I2C receive last byte and send no ACK*/ |
/****************************************/ |
void I2C_ReceiveLastByte(void) |
void i2c_receive_last_byte(void) |
{ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
} |
133,41 → 133,41 |
{ |
// Master Transmit |
case 0: // Send SLA-W |
I2C_WriteByte(0x52+(motor*2)); |
i2c_write_byte(0x52+(motor*2)); |
break; |
case 1: // Send Data to Salve |
switch(motor++) |
{ |
case 0: |
I2C_WriteByte(Motor_Front); |
i2c_write_byte(Motor_Front); |
break; |
case 1: |
I2C_WriteByte(Motor_Rear); |
i2c_write_byte(Motor_Rear); |
break; |
case 2: |
I2C_WriteByte(Motor_Right); |
i2c_write_byte(Motor_Right); |
break; |
case 3: |
I2C_WriteByte(Motor_Left); |
i2c_write_byte(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_WriteByte(0x53+(motorread*2)); |
i2c_write_byte(0x53+(motorread*2)); |
break; |
case 4: |
//Transmit 1st byte |
I2C_ReceiveByte(); |
i2c_receive_byte(); |
break; |
case 5: //Read 1st byte and transmit 2nd Byte |
motor_rx[motorread] = TWDR; |
I2C_ReceiveLastByte(); |
i2c_receive_last_byte(); |
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,10 → 24,11 |
extern volatile uint8_t motorread; |
extern volatile uint8_t motor_rx[8]; |
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 |
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); |
#endif |
/branches/V0.68d Code Redesign killagreg/ubx.c |
---|
71,13 → 71,12 |
} GPS_VELNED_t; |
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_STATUS_t GpsStatus; |
GPS_POSLLH_t GpsPosLlh; |
GPS_POSUTM_t GpsPosUtm; |
GPS_VELNED_t GpsVelNed; |
volatile uint8_t GPSTimeout = 0; |
GPS_INFO_t GPSInfo; |
void UpdateGPSInfo (void) |
{ |
113,6 → 112,14 |
} |
} |
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) |
221,7 → 228,6 |
*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,8 → 1,6 |
#ifndef _UBX_H |
#define _UBX_H |
#include <inttypes.h> |
#define INVALID 0x00 |
#define VALID 0x01 |
#define PROCESSED 0x02 |
41,9 → 39,6 |
//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,14 → 123,5 |
(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 |