/branches/V0.68d Code Redesign killagreg/GPS.c |
---|
1,5 → 1,4 |
#include <inttypes.h> |
#include "fc.h" |
#include "ubx.h" |
#include "mymath.h" |
10,6 → 9,8 |
#define TSK_HOLD 1 |
#define TSK_HOME 2 |
#define GPS_STICK_SENSE 12 |
#define GPS_STICK_LIMIT 45 |
int16_t GPS_Pitch = 0; |
int16_t GPS_Roll = 0; |
43,6 → 44,11 |
if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set |
} |
// clear home position |
void GPS_ClearHomePosition(void) |
{ |
HomePosition.Status = INVALID; |
} |
// disable GPS control sticks |
void GPS_Neutral(void) |
51,12 → 57,21 |
GPS_Roll = 0; |
} |
// calculates the GPS control sticks values from the position deviation |
void GPS_PDController(void) |
// calculates the GPS control sticks values from the deviation to target position |
void GPS_PDController(GPS_Pos_t *TargetPos) |
{ |
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; |
if( (TargetPos->Status == VALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D)) |
{ |
GPSPosDev_North = GPSInfo.utmnorth - TargetPos->Northing; |
GPSPosDev_East = GPSInfo.utmeast - TargetPos->Easting; |
DebugOut.Analog[12] = GPSPosDev_North; |
DebugOut.Analog[13] = GPSPosDev_East; |
//Calculate PD-components of the controller (negative sign for compensation) |
P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
96,23 → 111,28 |
GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
} |
// limit GPS controls |
#define GPS_CTRL_LIMIT 35 |
if (GPS_Pitch > GPS_CTRL_LIMIT) GPS_Pitch = GPS_CTRL_LIMIT; |
if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT; |
if (GPS_Roll > GPS_CTRL_LIMIT) GPS_Roll = GPS_CTRL_LIMIT; |
if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT; |
if (GPS_Pitch > GPS_STICK_LIMIT) GPS_Pitch = GPS_STICK_LIMIT; |
if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT; |
if (GPS_Roll > GPS_STICK_LIMIT) GPS_Roll = GPS_STICK_LIMIT; |
if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT; |
} |
else // invalid input data |
{ |
BeepTime = 50; |
GPS_Neutral(); |
} |
} |
void GPS_Main(void) |
void GPS_Main(uint8_t ctrl) |
{ |
static uint8_t GPS_Task = TSK_IDLE; |
int16_t satbeep; |
// poti2 enables the gps feature |
if(Poti2 < 70) GPS_Task = TSK_IDLE; |
else if (Poti2 < 160) GPS_Task = TSK_HOLD; |
else GPS_Task = TSK_HOME; // Poti2 >= 160 |
// ctrl enables the gps feature |
if(ctrl < 70) GPS_Task = TSK_IDLE; |
else if (ctrl < 160) GPS_Task = TSK_HOLD; |
else GPS_Task = TSK_HOME; // ctrl >= 160 |
switch(GPSInfo.status) |
150,16 → 170,12 |
GPS_Neutral(); |
break; // eof TSK_IDLE |
case TSK_HOLD: |
// if sticks are centered and hold position is valid enable position hold control |
if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID)) |
if(HoldPosition.Status == VALID) |
{ |
// Calculate deviation from hold position |
GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing; |
GPSPosDev_East = GPSInfo.utmeast - HoldPosition.Easting; |
DebugOut.Analog[12] = GPSPosDev_North; |
DebugOut.Analog[13] = GPSPosDev_East; |
GPS_PDController(); |
// if sticks are centered (no manual control) |
if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
{ |
GPS_PDController(&HoldPosition); |
} |
else // MK controlled by user |
{ |
166,32 → 182,36 |
// update hold point to current gps position |
HoldPosition.Northing = GPSInfo.utmnorth; |
HoldPosition.Easting = GPSInfo.utmeast; |
HoldPosition.Status = VALID; |
HoldPosition.Status = GPSInfo.status; |
// disable gps control |
GPS_Neutral(); |
} |
} |
break; // eof TSK_HOLD |
case TSK_HOME: |
if(HomePosition.Status == VALID) |
{ |
// update hold point to current gps position |
// to avoid a flight back if home comming is deactivated |
HoldPosition.Northing = GPSInfo.utmnorth; |
HoldPosition.Easting = GPSInfo.utmeast; |
HoldPosition.Status = VALID; |
// Calculate deviation from home position |
GPSPosDev_North = GPSInfo.utmnorth - HomePosition.Northing; |
GPSPosDev_East = GPSInfo.utmeast - HomePosition.Easting; |
DebugOut.Analog[12] = GPSPosDev_North; |
DebugOut.Analog[13] = GPSPosDev_East; |
GPS_PDController(); |
HoldPosition.Status = GPSInfo.status; |
// if sticks are centered (no manual control) |
if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE)) |
{ |
GPS_PDController(&HomePosition); |
} |
} |
else // bad home position |
{ |
GPS_Neutral(); |
BeepTime = 50; // signal invalid home position |
// try to hold at least the position as a fallback option |
// if sticks are centered (no manual control) |
if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID)) |
{ |
GPS_PDController(&HoldPosition); |
} |
} |
break; // eof TSK_HOME |
default: // unhandled task |
GPS_Neutral(); |
/branches/V0.68d Code Redesign killagreg/cmps03.c |
---|
10,9 → 10,9 |
/*********************************************/ |
void CMPS03_Init(void) |
{ |
// Port PC4 connected to PWM output from compass module |
// Port PC5 connected to PWM output from compass module |
DDRC &= ~(1<<DDC4); // set as input |
PORTC |= (1<<PORTC4); // pull up to increase PWM counter aslo if nowthing is connected |
PORTC |= (1<<PORTC4); // pull up to increase PWM counter allo if nothing is connected |
PWMTimeout = 0; |
} |
/branches/V0.68d Code Redesign killagreg/eeprom.c |
---|
83,7 → 83,7 |
ParamSet.GyroAccTrim = 16; // 1/k |
ParamSet.DriftComp = 4; |
ParamSet.DynamicStability = 100; |
memcpy(ParamSet.Name, "Sport\0", 12); |
memcpy(ParamSet.Name, "Sport\0",6); |
} |
146,7 → 146,7 |
ParamSet.GyroAccTrim = 32; // 1/k |
ParamSet.DriftComp = 4; |
ParamSet.DynamicStability = 75; |
memcpy(ParamSet.Name, "Normal\0", 12); |
memcpy(ParamSet.Name, "Normal\0", 7); |
} |
209,7 → 209,7 |
ParamSet.GyroAccTrim = 32; // 1/k |
ParamSet.DriftComp = 4; |
ParamSet.DynamicStability = 50; |
memcpy(ParamSet.Name, "Beginner\0", 12); |
memcpy(ParamSet.Name, "Beginner\0", 9); |
} |
/***************************************************/ |
/branches/V0.68d Code Redesign killagreg/fc.c |
---|
70,6 → 70,7 |
#ifdef USE_CMPS03 |
#include "cmps03.h" |
#endif |
#include "led.h" |
volatile uint16_t I2CTimeout = 100; |
// gyro readings |
114,7 → 115,7 |
volatile int16_t DiffPitch, DiffRoll; |
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
// setpoints for motors |
volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; |
362,11 → 363,23 |
if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--; |
if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--; |
if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--; |
//PPM24-Extension |
if(Poti5 < PPM_in[9] + 110) Poti5++; else if(Poti5 > PPM_in[9] + 110 && Poti5) Poti5--; |
if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--; |
if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--; |
if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--; |
//limit poti values |
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
//PPM24-Extension |
if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255; |
if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255; |
if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255; |
if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255; |
TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
407,7 → 420,9 |
/************************************************************************/ |
void ParameterMapping(void) |
{ |
if(SenderOkay > 140) // do the mapping of RC-Potis only if the rc-signal is ok |
// else the last updated values are used |
{ |
#define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;} |
CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255); |
CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100); |
430,9 → 445,9 |
CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255); |
CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255); |
CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255); |
Ki = (float) FCParam.I_Factor * FACTOR_I; |
} |
} |
/************************************************************************/ |
456,8 → 471,8 |
static int32_t CorrectionPitch, CorrectionRoll; |
Mean(); |
GRN_ON; |
GRN_ON; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// determine thrust value |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
500,7 → 515,7 |
PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
} |
else MotorsOn = 0; // switch of all motors |
} |
} // eof SenderOkay < 100 |
else |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// RC-signal is good |
624,12 → 639,13 |
delay_stopmotors = 200; // do not repeat if once executed |
Modell_Is_Flying = 0; |
MotorsOn = 0; |
GPS_ClearHomePosition(); |
} |
} |
else delay_stopmotors = 0; // reset delay timer if sticks are not in this position |
} |
} |
// remapping of paameters only if the signal rc-sigbnal conditions are good |
} // eof SenderOkay > 140 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// new values from RC |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
637,7 → 653,6 |
{ |
int tmp_int; |
ParameterMapping(); // remapping params (online poti replacement) |
// calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4; |
StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D; |
754,6 → 769,14 |
if(ThrustMixFraction > ParamSet.LoopThrustLimit) ThrustMixFraction = ParamSet.LoopThrustLimit; |
} |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ LED Control on J16/J17 |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
LED_OffTime = FCParam.UserParam7; |
LED_OnTime = FCParam.UserParam8; |
LED_Update(); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// in case of emergency landing |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1056,11 → 1079,12 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// GPS |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if((ParamSet.GlobalConfig & CFG_GPS_ACTIVE) && !EmergencyLanding) |
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 |
if(EmergencyLanding) GPS_Main(230); // enables Comming Home |
else GPS_Main(Poti3); |
} |
else |
{ |
/branches/V0.68d Code Redesign killagreg/fc.h |
---|
74,7 → 74,7 |
void Beep(uint8_t numbeeps); |
extern int16_t Poti1, Poti2, Poti3, Poti4; |
extern int16_t Poti1, Poti2, Poti3, Poti4, Poti5, Poti6, Poti7, Poti8; |
// setpoints for motors |
extern volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; //used by twimaster isr |
/branches/V0.68d Code Redesign killagreg/gps.h |
---|
9,8 → 9,9 |
extern uint8_t GPS_P_Factor; |
extern uint8_t GPS_D_Factor; |
extern void GPS_Main(void); |
extern void GPS_Main(uint8_t ctrl); |
extern void GPS_SetHomePosition(void); |
extern void GPS_ClearHomePosition(void); |
extern void GPS_Neutral(void); |
#endif //_GPS_H |
/branches/V0.68d Code Redesign killagreg/main.c |
---|
61,6 → 61,7 |
#if defined (__AVR_ATmega644P__) |
#include "uart1.h" |
#endif |
#include "led.h" |
#include "menu.h" |
#include "ubx.h" |
#include "fc.h" |
105,11 → 106,6 |
ROT_ON; |
GRN_OFF; |
// set PC2 & PC3 as output (ctrl J16 & J17) |
DDRC |= (1<<DDC2)|(1<<DDC3); |
J16_OFF; |
J17_OFF; |
// disable watchdog |
MCUSR &=~(1<<WDRF); |
WDTCSR |= (1<<WDCE)|(1<<WDE); |
125,6 → 121,7 |
ROT_OFF; |
// initalize modules |
LED_Init(); |
TIMER0_Init(); |
TIMER2_Init(); |
USART0_Init(); |
/branches/V0.68d Code Redesign killagreg/main.h |
---|
1,6 → 1,8 |
#ifndef _MAIN_H |
#define _MAIN_H |
#include <avr/io.h> |
//Hier die Quarz Frequenz einstellen |
#if defined (__AVR_ATmega32__) |
#define SYSCLK 20000000L //Quarz Frequenz in Hz |
25,11 → 27,6 |
#define GRN_ON PORTB |= (1<<PORTB1) |
#define GRN_FLASH PORTB ^= (1<<PORTB1) |
#define J16_ON PORTC |= (1<<PORTC2) |
#define J16_OFF PORTC &= ~(1<<PORTC2) |
#define J17_ON PORTC |= (1<<PORTC3) |
#define J17_OFF PORTC &= ~(1<<PORTC3) |
#include <inttypes.h> |
extern uint8_t BoardRelease; |
/branches/V0.68d Code Redesign killagreg/makefile |
---|
80,7 → 80,7 |
########################################################################################################## |
# List C source files here. (C dependencies are automatically generated.) |
SRC = main.c uart.c printf_P.c timer0.c timer2.c analog.c menu.c |
SRC = main.c uart.c printf_P.c timer0.c timer2.c analog.c menu.c led.c |
SRC += twimaster.c rc.c fc.c GPS.c spi.c eeprom.c mymath.c ubx.c fifo.c |
ifeq ($(MCU), atmega644p) |
SRC += uart1.c |
/branches/V0.68d Code Redesign killagreg/menu.c |
---|
161,10 → 161,10 |
LCD_printfxy(0,3,"OffCourse: %5i",CompassOffCourse); |
break; |
case 9:// Poti Menu Item |
LCD_printfxy(0,0,"Poti1: %3i",Poti1); |
LCD_printfxy(0,1,"Poti2: %3i",Poti2); |
LCD_printfxy(0,2,"Poti3: %3i",Poti3); |
LCD_printfxy(0,3,"Poti4: %3i",Poti4); |
LCD_printfxy(0,0,"Po1: %3i Po5: %3i" ,Poti1,Poti5); //PPM24-Extesion |
LCD_printfxy(0,1,"Po2: %3i Po6: %3i" ,Poti2,Poti6); //PPM24-Extesion |
LCD_printfxy(0,2,"Po3: %3i Po7: %3i" ,Poti3,Poti7); //PPM24-Extesion |
LCD_printfxy(0,3,"Po4: %3i Po8: %3i" ,Poti4,Poti8); //PPM24-Extesion |
break; |
case 10:// Servo Menu Item |
LCD_printfxy(0,0,"Servo " ); |
/branches/V0.68d Code Redesign killagreg/rc.c |
---|
114,7 → 114,7 |
{ |
// if a sync gap happens and there where at least 4 channels decoded before |
// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array. |
if(index >= 4) NewPpmData = 0; // Null means NewData |
if(index >= 4) NewPpmData = 0; // Null means NewData for the first 4 channels |
// synchronize channel index |
index = 1; |
} |