Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 726 → Rev 727

/branches/V0.68d Code Redesign killagreg/GPS.c
3,12 → 3,13
#include "fc.h"
#include "ubx.h"
#include "mymath.h"
#include "timer0.h"
 
int16_t GPS_Pitch = 0;
int16_t GPS_Roll = 0;
 
uint8_t GPS_P_Factor = 0;
uint8_t GPS_D_Factor = 0;
int32_t GPS_P_Factor = 0;
int32_t GPS_D_Factor = 0;
 
typedef struct
{
20,8 → 21,6
 
// GPS coordinates for hold position
GPS_Pos_t GPSHoldPoint = {0,0, INVALID};
// GPS coordinates for flying home
GPS_Pos_t GPSHomePoint = {0,0, INVALID};;
 
 
 
43,6 → 42,7
case INVALID: // invalid gps data
GPS_Pitch = 0;
GPS_Roll = 0;
BeepTime = 50;
break;
case PROCESSED: // if gps data are already processed
// downcount timeout
60,35 → 60,52
// 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 sticks are centered and hold position is valid enable position hold control
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;
//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;
P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;
D_East = -(GPS_D_Factor * GPSInfo.veleast)/512;
 
// PD-controller
PD_North = (-P_North + D_North);
PD_East = (P_East - D_East);
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);
// GPS to pitch and roll settings
 
//A positive pitch angle moves head downwards (flying forward).
//A positive roll angle tilts left side downwards (flying left).
 
// If compass heading is 0 the head of the copter is in north direction.
// A positive pitch angle will fly to north and a positive roll angle will fly to west.
// In case of a positive north deviation/velocity the
// copter should fly to south (negative pitch).
// In case of a positive east position deviation and a positive east velocity the
// copter should fly to west (positive roll).
 
// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
// in the fc.c. Therefore a positive north deviation/velocity should result in a positive
// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
 
// rotation transformation to compass heading to match any copter orientation
coscompass = (int32_t)c_cos_8192(CompassHeading);
sincompass = (int32_t)c_sin_8192(CompassHeading);
 
GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 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;
#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;
}
else // MK controlled by user
{
105,6 → 122,7
{ // diable gps control
GPS_Pitch = 0;
GPS_Roll = 0;
BeepTime = 50;
}
// set current data as processed to avoid further calculations on the same gps data
GPSInfo.status = PROCESSED;
/branches/V0.68d Code Redesign killagreg/fc.c
205,6 → 205,8
TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
ExternHightValue = 0;
GPS_Pitch = 0;
GPS_Roll = 0;
}
 
/************************************************************************/
1037,7 → 1039,7
w = FCParam.CompassYawEffect - w; // reduce commpass effect with increasing declination
if(w > 0) // if there is any compass effect (avoid negative compass feedback)
{
Reading_IntegralGyroYaw -= (CompassOffCourse * w) / 32;
Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32;
}
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1123,7 → 1125,7
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Hight Control
// The higth control algorithm reduces the thrust but does not increas the thrust.
// The higth control algorithm reduces the thrust but does not increase the thrust.
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// If hight control is activated and no emergency landing is activre
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) )
/branches/V0.68d Code Redesign killagreg/main.c
138,10 → 138,10
 
if(GetParamByte(PID_ACC_PITCH) > 4)
{
printf("\n\rACC nicht abgeglichen!");
printf("\n\rACC not calibrated!");
}
 
//kurze Wartezeit (sonst reagiert die "Kompass kalibrieren?"-Abfrage nicht
//wait for a short time (otherwise the RC channel check wont work below)
timer = SetDelay(500);
while(!CheckDelay(timer));
 
155,7 → 155,7
 
if(ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)
{
printf("\n\rAbgleich Luftdrucksensor..");
printf("\n\rCalibrating air pressure sensor..");
timer = SetDelay(1000);
SearchAirPressureOffset();
while (!CheckDelay(timer));
170,7 → 170,7
ExternControl.Digital[0] = 0x55;
 
 
printf("\n\rSteuerung: ");
printf("\n\rControl: ");
if (ParamSet.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");
else printf("Neutral");
 
/branches/V0.68d Code Redesign killagreg/mm3.c
119,6 → 119,7
switch (MM3.STATE)
{
case MM3_STATE_RESET:
PORTC &= ~(1<<PORTC4); // select slave
PORTC |= (1<<PORTC5); // PC5 to High, MM3 Reset
MM3.STATE = MM3_STATE_START_TRANSFER;
return;
193,6 → 194,7
break;
}
}
PORTC |= (1<<PORTC4); // deselect slave
MM3.STATE = MM3_STATE_RESET;
}
}
325,9 → 327,10
// calculate Heading
heading = c_atan2(Hy_corr, Hx_corr);
 
// transform range from +-180° to 0°- 359°
heading += 360;
heading %= 360;
// atan returns angular range from -180 deg to 180 deg in counter clockwise notation
// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
if (heading < 0) heading = -heading;
else heading = 360 - heading;
 
return heading;
}