| /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; |
| } |