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