Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2057 → Rev 2058

/branches/dongfang_FC_rewrite/attitude.c
183,7 → 183,7
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
uint8_t axis;
 
if (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE) {
if (staticParams.bitConfig & CFG_AXIS_COUPLING_ENABLED) {
trigAxisCoupling();
} else {
ACRate[PITCH] = rate_ATT[PITCH];
198,7 → 198,6
*/
heading += ACYawRate;
intervalWrap(&heading, YAWOVER360);
 
headingError += ACYawRate;
 
/*
326,7 → 325,6
accVector += temp * temp;
temp = filteredAcc[2] >> 3;
accVector += temp * temp;
//debugOut.analog[18] = accVector;
}
 
#ifdef USE_MK3MAG
353,7 → 351,7
int32_t error;
 
if (commands_isCalibratingCompass()) {
//debugOut.analog[29] = 1;
debugOut.analog[30] = -1;
return;
}
 
364,13 → 362,13
 
// Compass is invalid, skip.
if (magneticHeading < 0) {
//debugOut.analog[29] = 2;
debugOut.analog[30] = -2;
return;
}
 
// Spinning fast, skip
if (abs(yawRate) > 128) {
//debugOut.analog[29] = 3;
debugOut.analog[30] = -3;
return;
}
 
377,10 → 375,12
// Otherwise invalidated, skip
if (ignoreCompassTimer) {
ignoreCompassTimer--;
//debugOut.analog[29] = 4;
debugOut.analog[30] = -4;
return;
}
 
debugOut.analog[30] = magneticHeading;
 
// TODO: Find computational cost of this.
error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading);
if (error <= -YAWOVER180) error += YAWOVER360;
/branches/dongfang_FC_rewrite/configuration.c
278,7 → 278,7
staticParams.outputDebugMask = 8;
staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | OUTPUTFLAGS_USE_ONBOARD_LEDS;
 
staticParams.naviMode = 200; // free.
staticParams.naviMode = 0; // free.
}
 
/***************************************************/
/branches/dongfang_FC_rewrite/configuration.h
228,7 → 228,7
#define CFG_COMPASS_ENABLED (1<<3)
#define CFG_UNUSED (1<<4)
#define CFG_GPS_ENABLED (1<<5)
#define CFG_AXIS_COUPLING_ACTIVE (1<<6)
#define CFG_AXIS_COUPLING_ENABLED (1<<6)
#define CFG_GYRO_SATURATION_PREVENTION (1<<7)
 
#define IMU_REVERSE_GYRO_PR (1<<0)
/branches/dongfang_FC_rewrite/controlMixer.c
50,6 → 50,9
EC_setNeutral();
HC_setGround();
FC_setNeutral(); // FC is FailsafeControl, not FlightCtrl.
 
// This is to set the home pos in navi.
// MKFlags |= MKFLAG_CALIBRATE;
}
 
/*
146,20 → 149,27
// This will init the values (not just add to them).
RC_periodicTaskAndPRTY(tempPRTY);
 
debugOut.analog[16] = tempPRTY[CONTROL_PITCH];
debugOut.analog[17] = tempPRTY[CONTROL_ROLL];
 
// Add external control to RC
EC_periodicTaskAndPRTY(tempPRTY);
 
#ifdef USE_DIRECT_GPS
// Add navigations control to RC and external control.
navigation_periodicTaskAndPRTY(tempPRTY);
if (staticParams.bitConfig & (CFG_COMPASS_ENABLED))
navigation_periodicTaskAndPRTY(tempPRTY);
#endif
 
debugOut.analog[18] = tempPRTY[CONTROL_PITCH];
debugOut.analog[19] = tempPRTY[CONTROL_ROLL];
 
// Add compass control (could also have been before navi, they are independent)
CC_periodicTaskAndPRTY(tempPRTY);
 
FC_periodicTaskAndPRTY(tempPRTY);
 
if (!MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
// This is temporary. There might be some emergency height control also.
if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {
// Add height control (could also have been before navi and/or compass, they are independent)
HC_periodicTaskAndPRTY(tempPRTY);
 
167,6 → 177,9
AC_getPRTY(tempPRTY);
}
 
debugOut.analog[20] = tempPRTY[CONTROL_PITCH];
debugOut.analog[21] = tempPRTY[CONTROL_ROLL];
 
// Commit results to global variable and also measure control activity.
controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE];
updateControlAndMeasureControlActivity(CONTROL_PITCH, tempPRTY[CONTROL_PITCH]);
/branches/dongfang_FC_rewrite/directGPSNaviControl.c
20,7 → 20,7
} FlightMode_t;
 
#define GPS_POSINTEGRAL_LIMIT 32000
#define LOG_NAVI_STICK_GAIN 2
#define LOG_NAVI_STICK_GAIN 3
#define GPS_P_LIMIT 100
 
typedef struct {
36,13 → 36,14
GPS_Pos_t homePosition = { 0, 0, 0, INVALID };
// the current flight mode
FlightMode_t flightMode = GPS_FLIGHT_MODE_UNDEF;
int16_t naviSticks[2] = {0,0};
 
// ---------------------------------------------------------------------------------
void navi_updateFlightMode(void) {
static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF;
 
if (controlMixer_getSignalQuality() <= SIGNAL_BAD || (MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {
flightMode = GPS_FLIGHT_MODE_HOME;
if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
flightMode = GPS_FLIGHT_MODE_FREE;
} else {
if (dynamicParams.naviMode < 50)
flightMode = GPS_FLIGHT_MODE_FREE;
124,11 → 125,15
return 1;
}
 
void navi_setNeutral(void) {
naviSticks[CONTROL_PITCH] = naviSticks[CONTROL_ROLL] = 0;
}
 
// calculates the GPS control stick values from the deviation to target position
// if the pointer to the target positin is NULL or is the target position invalid
// then the P part of the controller is deactivated.
void navi_PIDController(GPS_Pos_t *pTargetPos, int16_t *PRTY) {
static int32_t PID_Nick, PID_Roll;
void navi_PIDController(GPS_Pos_t *pTargetPos) {
static int32_t PID_Pitch, PID_Roll;
int32_t coscompass, sincompass;
int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
178,7 → 183,6
}
 
//Calculate PID-components of the controller
 
// D-Part
D_North = ((int32_t) staticParams.naviD * GPSInfo.velnorth) >> 9;
D_East = ((int32_t) staticParams.naviD * GPSInfo.veleast) >> 9;
194,6 → 198,7
// combine P & I
PID_North = P_North + I_North;
PID_East = P_East + I_East;
 
if (!navi_limitXY(&PID_North, &PID_East, GPS_P_LIMIT)) {
// within limit
GPSPosDevIntegral_North += GPSPosDev_North >> 4;
226,17 → 231,20
coscompass = -cos_360(heading / GYRO_DEG_FACTOR_YAW);
sincompass = -sin_360(heading / GYRO_DEG_FACTOR_YAW);
 
PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);
PID_Pitch = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);
PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);
 
// limit resulting GPS control vector
navi_limitXY(&PID_Nick, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN);
navi_limitXY(&PID_Pitch, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN);
 
PRTY[CONTROL_PITCH] += PID_Nick;
PRTY[CONTROL_ROLL] += PID_Roll;
debugOut.analog[26] = PID_Pitch;
debugOut.analog[27] = PID_Roll;
 
naviSticks[CONTROL_PITCH] = PID_Pitch;
naviSticks[CONTROL_ROLL] = PID_Roll;
} else { // invalid GPS data or bad compass reading
// reset error integral
navi_setNeutral();
GPSPosDevIntegral_North = 0;
GPSPosDevIntegral_East = 0;
}
252,7 → 260,8
if (MKFlags & MKFLAG_CALIBRATE) {
MKFlags &= ~(MKFLAG_CALIBRATE);
if (navi_writeCurrPositionTo(&homePosition)) {
homePosition.latitude += 10000L;
// shift north to simulate an offset.
// homePosition.latitude += 10000L;
beep(500);
}
}
259,6 → 268,7
 
switch (GPSInfo.status) {
case INVALID: // invalid gps data
navi_setNeutral();
if (flightMode != GPS_FLIGHT_MODE_FREE) {
beep(100); // beep if signal is neccesary
}
270,6 → 280,7
// if no new data arrived within timeout set current data invalid
// and therefore disable GPS
else {
navi_setNeutral();
GPSInfo.status = INVALID;
}
break;
282,6 → 293,7
// update hold position to current gps position
navi_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad
// disable gps control
navi_setNeutral();
break;
 
case GPS_FLIGHT_MODE_AID:
290,6 → 302,7
// update hold point to current gps position
navi_writeCurrPositionTo(&holdPosition);
// disable gps control
navi_setNeutral();
GPS_P_Delay = 0;
} else { // GPS control active
if (GPS_P_Delay < 7) {
296,13 → 309,14
// delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
GPS_P_Delay++;
navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position
navi_PIDController(NULL, PRTY); // activates only the D-Part
navi_PIDController(NULL); // activates only the D-Part
} else
navi_PIDController(&holdPosition, PRTY); // activates the P&D-Part
navi_PIDController(&holdPosition); // activates the P&D-Part
}
} else // invalid Hold Position
{ // try to catch a valid hold position from gps data input
} else { // invalid Hold Position
// try to catch a valid hold position from gps data input
navi_writeCurrPositionTo(&holdPosition);
navi_setNeutral();
}
break;
 
311,10 → 325,10
// update hold point to current gps position
// to avoid a flight back if home comming is deactivated
navi_writeCurrPositionTo(&holdPosition);
if (navi_isManuallyControlled(PRTY)) // MK controlled by user
{
if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
navi_setNeutral();
} else {// GPS control active
navi_PIDController(&homePosition, PRTY);
navi_PIDController(&homePosition);
}
} else {
// bad home position
324,21 → 338,25
if (holdPosition.status != INVALID) {
if (navi_isManuallyControlled(PRTY)) {
// MK controlled by user
navi_setNeutral();
} else {
// GPS control active
navi_PIDController(&holdPosition, PRTY);
navi_PIDController(&holdPosition);
}
} else { // try to catch a valid hold position
navi_writeCurrPositionTo(&holdPosition);
navi_setNeutral();
}
}
break; // eof TSK_HOME
default: // unhandled task
navi_setNeutral();
break; // eof default
} // eof switch GPS_Task
} // eof gps data quality is good
else // gps data quality is bad
{ // disable gps control
else { // gps data quality is bad
// disable gps control
navi_setNeutral();
if (flightMode != GPS_FLIGHT_MODE_FREE) {
// beep if signal is not sufficient
if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5))
352,4 → 370,7
GPSInfo.status = PROCESSED;
break;
} // eof GPSInfo.status
 
PRTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH];
PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];
}
/branches/dongfang_FC_rewrite/failsafeControl.c
46,8 → 46,9
}
}
 
// In either case, use e. throttle.
// In either case, use e. throttle and neutral controls. TODO: If there is supposed to be a navi come-home, this should affect RC control only and not navi.
PRTY[CONTROL_THROTTLE] = staticParams.emergencyThrottle; // Set emergency throttle
PRTY[CONTROL_PITCH] = PRTY[CONTROL_ROLL] = PRTY[CONTROL_YAW] = 0;
} else {
// Signal is OK.
if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
/branches/dongfang_FC_rewrite/flight.c
69,6 → 69,10
}
 
void flight_takeOff() {
// This is for GPS module to mark home position.
// TODO: What a disgrace, change it.
MKFlags |= MKFLAG_CALIBRATE;
 
HC_setGround();
#ifdef USE_MK3MAG
attitude_resetHeadingToMagnetic();
119,7 → 123,6
 
// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
throttleTerm *= CONTROL_SCALING;
// TODO: We dont need to repeat this for every iteration!
 
// end part 1: 750-800 usec.
// start part 3: 350 - 400 usec.
126,11 → 129,14
#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
// This is where control affects the target heading. It also (later) directly controls yaw.
headingError -= controls[CONTROL_YAW];
 
if (headingError < -YAW_I_LIMIT)
headingError = -YAW_I_LIMIT;
if (headingError > YAW_I_LIMIT)
else if (headingError > YAW_I_LIMIT)
headingError = YAW_I_LIMIT;
 
debugOut.analog[29] = headingError / 100;
 
PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
PDPart += (int32_t) (yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);
189,11 → 195,11
// The P-part is the P of the PID controller. That's the angle integrals (not rates).
for (axis = PITCH; axis <= ROLL; axis++) {
PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
// In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
// In HH mode, the I part is summed from P and D of gyros minus stick.
if (gyroIFactor) {
int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 3);
int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2);
if (axis == 0) debugOut.analog[28] = iDiff;
PDPart += iDiff;
IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
} else {
209,6 → 215,8
debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
}
 
PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
 
term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * invKi) >> 14);
term[axis] += (dynamicParams.levelCorrection[axis] - 128);
 
247,7 → 255,7
debugOut.analog[8] = yawTerm;
debugOut.analog[9] = throttleTerm;
 
debugOut.analog[16] = gyroActivity;
//debugOut.analog[16] = gyroActivity;
 
for (i = 0; i < MAX_MOTORS; i++) {
int32_t tmp;
/branches/dongfang_FC_rewrite/heightControl.c
77,7 → 77,7
}
 
// height, in meters (so the division factor is: 100)
// debugOut.analog[31] = (117100 - filteredAirPressure) / 100;
debugOut.analog[24] = (117100 - filteredAirPressure) / 100;
// Calculated 0 alt number: 108205
// Experimental 0 alt number: 117100
}
91,8 → 91,8
int32_t heightError = rampedTargetHeight - height;
int16_t dHeight = analog_getDHeight();
debugOut.analog[14] = height/100L;
debugOut.analog[15] = dHeight;
debugOut.analog[22] = height/10L;
debugOut.analog[23] = dHeight;
 
if (heightError > 0) {
debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
/branches/dongfang_FC_rewrite/rc.c
207,7 → 207,6
commandTimer = 0;
}
} // if RCQuality is no good, we just do nothing.
// debugOut.analog[18] = RCQuality;
}
 
/*
/branches/dongfang_FC_rewrite/uart0.c
104,22 → 104,22
"M4 ",
"pdpart ",
"control ", //15
"ipart ",
"ControlAct/10 ",
"RC Qual ",
"heightTrottleIn ",
"HeightdThrottle ", //20
"Height ",
"TargetHeight ",
"heightError ",
"HeightPdThrottle",
"HeightIdThrottle", //25
"HeightDdThrottle",
"Yaw Limit / 100 ",
"HeadingError/100",
"PD Yaw ",
"2 ", //30
"3 "
"RC Stick P ",
"RC Stick R ",
"Stick P w GPS ",
"Stick R w GPS ",
"Stick P w Emerg.", //20
"Stick R w Emerg.",
"Height[dm] ",
"dHeight ",
"Altitude ",
" ", //25
"naviPitch ",
"naviRoll ",
"i part contrib ",
"HeadingError ",
"Magnetic Heading", //30
"GPS Datasets "
};
 
/****************************************************************/
/branches/dongfang_FC_rewrite/ubx.c
1,7 → 1,7
#include <inttypes.h>
#include "ubx.h"
//#include "main.h"
#include <avr/io.h>
#include "output.h"
 
// ubx protocol parser state machine
#define UBXSTATE_IDLE 0
75,13 → 75,16
GPS_INFO_t GPSInfo = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID };
 
volatile uint8_t GPSTimeout = 0;
volatile uint16_t GPSDatasetCounter = 0;
 
void updateGPSInfo(void) {
if ((ubxSol.status == NEWDATA) && (ubxPosLlh.status == NEWDATA)
&& (ubxVelNed.status == NEWDATA)) {
if ((ubxSol.status == NEWDATA) && (ubxPosLlh.status == NEWDATA) && (ubxVelNed.status == NEWDATA)) {
//RED_FLASH;
// DebugOut.Digital ....blah...
if (GPSInfo.status != NEWDATA) {
GPSDatasetCounter++;
debugOut.analog[31] = GPSDatasetCounter;
 
GPSInfo.status = INVALID;
// NAV SOL
GPSInfo.flags = ubxSol.flags;
185,8 → 188,7
if (*ubxSp == NEWDATA) {
updateGPSInfo(); //update GPS info respectively
ubxstate = UBXSTATE_IDLE;
} else // data invalid or allready processd
{
} else {// data invalid or already processd
*ubxSp = INVALID;
ubxstate = UBXSTATE_DATA;
}