Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 763 → Rev 764

/branches/V0.68d Code Redesign killagreg/GPS.c
1,4 → 1,5
#include <inttypes.h>
#include <stdlib.h>
#include "fc.h"
#include "ubx.h"
#include "mymath.h"
11,6 → 12,8
 
#define GPS_STICK_SENSE 12
#define GPS_STICK_LIMIT 45
#define GPS_D_LIMIT_DIST 500 // for position deviations grater than 500 cm (5m) the D-Part of the PD-Controller is limited
#define GPS_D_LIMIT 50 // PD-Controntroller D-Limit.
 
int16_t GPS_Pitch = 0;
int16_t GPS_Roll = 0;
78,6 → 81,14
P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;
D_East = -(GPS_D_Factor * GPSInfo.veleast)/512;
 
if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) || (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) )
{
if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT;
else if (D_North < -GPS_D_LIMIT) D_North = -GPS_D_LIMIT;
if (D_East > GPS_D_LIMIT) D_East = GPS_D_LIMIT;
else if (D_East < -GPS_D_LIMIT) D_East = -GPS_D_LIMIT;
}
 
// PD-controller
PD_North = P_North + D_North;
PD_East = P_East + D_East;
112,9 → 123,9
}
// limit GPS controls
if (GPS_Pitch > GPS_STICK_LIMIT) GPS_Pitch = GPS_STICK_LIMIT;
if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT;
else 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 if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT;
}
else // invalid input data
{
/branches/V0.68d Code Redesign killagreg/cmps03.c
1,7 → 1,6
#include <avr/io.h>
 
 
 
int32_t PWMHeading = -1;
uint8_t PWMTimeout = 0;
 
10,9 → 9,9
/*********************************************/
void CMPS03_Init(void)
{
// Port PC5 connected to PWM output from compass module
// Port PC4 connected to PWM output from compass module
DDRC &= ~(1<<DDC4); // set as input
PORTC |= (1<<PORTC4); // pull up to increase PWM counter allo if nothing is connected
PORTC |= (1<<PORTC4); // pull up to increase PWM counter also if nothing is connected
 
PWMTimeout = 0;
}
/branches/V0.68d Code Redesign killagreg/fc.c
463,7 → 463,7
static int32_t IntegralErrorRoll = 0;
static uint16_t RcLostTimer;
static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0;
static uint16_t Modell_Is_Flying = 0;
static uint16_t Model_Is_Flying = 0;
static uint8_t HeightControlActive = 0;
static int16_t HeightControlThrust = 0;
static int8_t TimerDebugOut = 0;
503,7 → 503,7
EmergencyLanding = 0; // emergency landing is over
}
ROT_ON; // set red led
if(Modell_Is_Flying > 2000) // wahrscheinlich in der Luft --> langsam absenken
if(Model_Is_Flying > 2000) // wahrscheinlich in der Luft --> langsam absenken
{
ThrustMixFraction = ParamSet.EmergencyThrust; // set emergency thrust
EmergencyLanding = 1; // enable emergency landing
527,9 → 527,9
RcLostTimer = ParamSet.EmergencyThrustDuration * 50;
if(ThrustMixFraction > 40)
{
if(Modell_Is_Flying < 0xFFFF) Modell_Is_Flying++;
if(Model_Is_Flying < 0xFFFF) Model_Is_Flying++;
}
if((Modell_Is_Flying < 200) || (ThrustMixFraction < 40))
if((Model_Is_Flying < 200) || (ThrustMixFraction < 40))
{
SumPitch = 0;
SumRoll = 0;
549,7 → 549,7
{
delay_neutral = 0;
GRN_OFF;
Modell_Is_Flying = 0;
Model_Is_Flying = 0;
// check roll/pitch stick position
// if pitch stick is topmost or roll stick is leftmost --> change parameter setting
// according to roll/pitch stick position
590,7 → 590,7
delay_neutral = 0;
GRN_OFF;
SetParamWord(PID_ACC_PITCH, 0xFFFF); // make value invalid
Modell_Is_Flying = 0;
Model_Is_Flying = 0;
SetNeutral();
// Save ACC neutral settings to eeprom
SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX);
614,7 → 614,7
if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
{
delay_startmotors = 200; // do not repeat if once executed
Modell_Is_Flying = 1;
Model_Is_Flying = 1;
MotorsOn = 1;
SetPointYaw = 0;
Reading_IntegralGyroYaw = 0;
637,7 → 637,7
if(++delay_stopmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
{
delay_stopmotors = 200; // do not repeat if once executed
Modell_Is_Flying = 0;
Model_Is_Flying = 0;
MotorsOn = 0;
GPS_ClearHomePosition();
}