Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2044 → Rev 2045

/branches/dongfang_FC_rewrite/attitude.c
224,16 → 224,18
* changed accordingly.
*/
void trigAxisCoupling(void) {
int16_t cospitch = int_cos(angle[PITCH]);
int16_t cosroll = int_cos(angle[ROLL]);
int16_t sinroll = int_sin(angle[ROLL]);
int16_t rollAngleInDegrees = angle[ROLL] / GYRO_DEG_FACTOR_PITCHROLL;
int16_t pitchAngleInDegrees = angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL;
 
int16_t cospitch = cos_360(pitchAngleInDegrees);
int16_t cosroll = cos_360(rollAngleInDegrees);
int16_t sinroll = sin_360(rollAngleInDegrees);
 
ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate
* sinroll) >> MATH_UNIT_FACTOR_LOG);
 
ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll
+ (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan(
angle[PITCH])) >> MATH_UNIT_FACTOR_LOG);
+ (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * tan_360(pitchAngleInDegrees)) >> MATH_UNIT_FACTOR_LOG);
 
ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch;
 
395,7 → 397,7
accVector += temp * temp;
temp = filteredAcc[2] >> 3;
accVector += temp * temp;
debugOut.analog[18] = accVector;
//debugOut.analog[18] = accVector;
}
 
/************************************************************************
/branches/dongfang_FC_rewrite/attitudeControl.c
18,7 → 18,10
uint8_t effect = dynamicParams.attitudeControl; // Userparam 3
int16_t deltaThrottle, y;
 
projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]);
int16_t rollAngleInDegrees = angle[ROLL] / GYRO_DEG_FACTOR_PITCHROLL;
int16_t pitchAngleInDegrees = angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL;
 
projection = (int32_t) cos_360(pitchAngleInDegrees) * (int32_t) cos_360(rollAngleInDegrees);
projection >>= 8;
 
if (projection < 0) {
/branches/dongfang_FC_rewrite/configuration.c
110,6 → 110,8
SET_POT(dynamicParams.levelCorrection[0], staticParams.levelCorrection[0]);
SET_POT(dynamicParams.levelCorrection[1], staticParams.levelCorrection[1]);
 
SET_POT(dynamicParams.naviMode, staticParams.naviMode);
 
for (i=0; i<sizeof(staticParams.userParams); i++) {
SET_POT(dynamicParams.userParams[i],staticParams.userParams[i]);
}
/branches/dongfang_FC_rewrite/configuration.h
67,7 → 67,7
uint8_t levelCorrection[2];
 
// Simple direct navigation
uint8_t directGPSMode;
uint8_t naviMode;
 
/* P */uint8_t userParams[8];
} dynamicParam_t;
184,13 → 184,8
uint8_t outputDebugMask;
uint8_t outputFlags;
 
// Simple direct navigation
uint8_t directNaviMode;
 
// NaviCtrl navigation
 
 
// Shared for both modes of navigation
uint8_t naviMode;
uint8_t naviStickThreshold;
uint8_t GPSMininumSatellites;
uint8_t naviP;
/branches/dongfang_FC_rewrite/controlMixer.c
185,6 → 185,9
int16_t* RC_PRTY = RC_getPRTY();
int16_t* EC_PRTY = EC_getPRTY();
debugOut.analog[29] = RC_PRTY[CONTROL_PITCH];
debugOut.analog[30] = RC_PRTY[CONTROL_ROLL];
 
int16_t naviSticks[] = {RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH], RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]};
 
navigation_periodicTask(&naviSticks);
/branches/dongfang_FC_rewrite/directGPSNaviControl.c
47,12 → 47,12
 
if (controlMixer_getSignalQuality() <= SIGNAL_BAD
|| MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
flightMode = GPS_FLIGHT_MODE_FREE;
flightMode = GPS_FLIGHT_MODE_HOME;
} else {
if (dynamicParams.directGPSMode < 50)
if (dynamicParams.naviMode < 50)
flightMode = GPS_FLIGHT_MODE_FREE;
else if (dynamicParams.naviMode < 180)
flightMode = GPS_FLIGHT_MODE_AID;
else if (dynamicParams.directGPSMode < 180)
flightMode = GPS_FLIGHT_MODE_FREE;
else
flightMode = GPS_FLIGHT_MODE_HOME;
}
61,6 → 61,8
beep(100);
flightModeOld = flightMode;
}
 
debugOut.analog[31] = flightMode;
}
 
// ---------------------------------------------------------------------------------
100,7 → 102,7
}
 
// set given position to current gps position
uint8_t GPS_setCurrPosition(GPS_Pos_t * pGPSPos) {
uint8_t GPS_writeCurrPositionTo(GPS_Pos_t * pGPSPos) {
if (pGPSPos == NULL)
return 0; // bad pointer
 
153,7 → 155,7
GPSPosDevIntegral_North = 0;
GPSPosDevIntegral_East = 0;
// recalculate latitude projection
cos_target_latitude = cos_360((int16_t) (pTargetPos->latitude / 10000000L));
cos_target_latitude = cos_360(pTargetPos->latitude / 10000000L);
// remember last target pointer
pLastTargetPos = pTargetPos;
// mark data as processed
238,8 → 240,12
// limit resulting GPS control vector
GPS_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
 
sticks[CONTROL_PITCH] += (int16_t) PID_Nick;
sticks[CONTROL_ROLL] += (int16_t) PID_Roll;
debugOut.analog[27] = PID_Nick;
debugOut.analog[28] = PID_Roll;
 
sticks[CONTROL_PITCH] += PID_Nick;
sticks[CONTROL_ROLL] += PID_Roll;
 
} else { // invalid GPS data or bad compass reading
// reset error integral
GPSPosDevIntegral_North = 0;
256,9 → 262,11
// store home position if start of flight flag is set
if (MKFlags & MKFLAG_CALIBRATE) {
MKFlags &= ~(MKFLAG_CALIBRATE);
if (GPS_setCurrPosition(&homePosition))
beep(500);
}
if (GPS_writeCurrPositionTo(&homePosition)) {
homePosition.latitude += 9000L;
beep(500);
}
}
 
switch (GPSInfo.status) {
case INVALID: // invalid gps data
283,7 → 291,7
switch (flightMode) { // check what's to do
case GPS_FLIGHT_MODE_FREE:
// update hold position to current gps position
GPS_setCurrPosition(&holdPosition); // can get invalid if gps signal is bad
GPS_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad
// disable gps control
break;
 
291,7 → 299,7
if (holdPosition.status != INVALID) {
if (GPS_isManuallyControlled(sticks)) { // MK controlled by user
// update hold point to current gps position
GPS_setCurrPosition(&holdPosition);
GPS_writeCurrPositionTo(&holdPosition);
// disable gps control
GPS_P_Delay = 0;
} else { // GPS control active
298,7 → 306,7
if (GPS_P_Delay < 7) {
// delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
GPS_P_Delay++;
GPS_setCurrPosition(&holdPosition); // update hold point to current gps position
GPS_writeCurrPositionTo(&holdPosition); // update hold point to current gps position
GPS_PIDController(NULL, sticks); // activates only the D-Part
} else
GPS_PIDController(&holdPosition, sticks); // activates the P&D-Part
305,7 → 313,7
}
} else // invalid Hold Position
{ // try to catch a valid hold position from gps data input
GPS_setCurrPosition(&holdPosition);
GPS_writeCurrPositionTo(&holdPosition);
}
break;
 
313,7 → 321,7
if (homePosition.status != INVALID) {
// update hold point to current gps position
// to avoid a flight back if home comming is deactivated
GPS_setCurrPosition(&holdPosition);
GPS_writeCurrPositionTo(&holdPosition);
if (GPS_isManuallyControlled(sticks)) // MK controlled by user
{
} else {// GPS control active
332,7 → 340,7
GPS_PIDController(&holdPosition, sticks);
}
} else { // try to catch a valid hold position
GPS_setCurrPosition(&holdPosition);
GPS_writeCurrPositionTo(&holdPosition);
}
}
break; // eof TSK_HOME
/branches/dongfang_FC_rewrite/dongfangMath.c
97,8 → 97,8
(int16_t) (0.9975640502598242 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9986295347545738 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9993908270190958 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5), (int16_t) (1.0
* MATH_UNIT_FACTOR) };
(int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (MATH_UNIT_FACTOR) + 0.5 };
 
const int16_t TAN_TABLE[] PROGMEM
= { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5),
193,66 → 193,42
(int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (32767) };
 
int16_t sin_mkdegrees(int32_t arg) {
int16_t sin_360(int16_t arg) {
int8_t sign;
int16_t result;
int16_t argp = arg / MATH_DRG_FACTOR;
argp %= 360;
if (argp < 0) {
argp = -argp;
arg %= 360;
if (arg < 0) {
arg = -arg;
sign = -1;
} else {
sign = 1;
}
if (argp >= 90) {
argp = 180 - argp;
if (arg > 180) {
arg = 360 - arg;
sign = -sign;
}
result = pgm_read_word(&SIN_TABLE[(uint8_t) argp]);
if (arg > 90) {
arg = 180 - arg;
}
result = pgm_read_word(&SIN_TABLE[(uint8_t) arg]);
return (sign == 1) ? result : -result;
}
 
int16_t cos_mkdegrees(int32_t arg) {
if (arg > 90L * MATH_DRG_FACTOR)
return sin_mkdegrees(arg + (90L - 360L) * MATH_DRG_FACTOR);
return sin_mkdegrees(arg + 90L * MATH_DRG_FACTOR);
int16_t cos_360(int16_t arg) {
return sin_360(arg + 90);
}
 
int16_t tan_mkdegrees(int32_t arg) {
int16_t tan_360(int16_t arg) {
int8_t sign = 1;
int16_t result;
int16_t argp = arg / MATH_DRG_FACTOR;
if (argp >= 90) {
argp = 180 - argp;
sign = -1;
} else if (argp < -90) {
argp += 180;
} else if (argp < 0) {
argp = -argp;
sign = -1;
}
result = pgm_read_word(&TAN_TABLE[(uint8_t) argp]);
return (sign == 1) ? result : -result;
}
 
int16_t sin_360(int16_t arg) {
int8_t sign;
int16_t result;
arg %= 360;
if (arg < 0) {
arg = -arg;
sign = -1;
} else {
sign = 1;
arg = -arg;
sign = -1;
}
if (arg >= 90) {
arg = 180 - arg;
sign = -sign;
}
result = pgm_read_word(&SIN_TABLE[(uint8_t) arg]);
result = pgm_read_word(&TAN_TABLE[(uint8_t) arg]);
return (sign == 1) ? result : -result;
}
 
int16_t cos_360(int16_t arg) {
if (arg > 90L)
return sin_360(arg + 90- 360);
return sin_360(arg + 90);
}
/branches/dongfang_FC_rewrite/dongfangMath.h
17,10 → 17,6
#define MATH_UNIT_FACTOR_LOG 12
#define MATH_UNIT_FACTOR (1L<<MATH_UNIT_FACTOR_LOG)
 
int16_t sin_mkdegrees(int32_t arg);
int16_t cos_mkdegrees(int32_t arg);
int16_t tan_mkdegrees(int32_t arg);
 
int16_t sin_360(int16_t arg);
int16_t cos_360(int16_t arg);
int16_t tan_360(int16_t arg);
/branches/dongfang_FC_rewrite/flight.c
432,8 → 432,8
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (!(--debugDataTimer)) {
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
debugOut.analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
debugOut.analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
debugOut.analog[0] = angle[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg
debugOut.analog[1] = angle[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg
debugOut.analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
}
}
/branches/dongfang_FC_rewrite/heightControl.c
77,7 → 77,7
}
 
// height, in meters (so the division factor is: 100)
debugOut.analog[30] = (117100 - filteredAirPressure) / 100;
// debugOut.analog[31] = (117100 - filteredAirPressure) / 100;
// Calculated 0 alt number: 108205
// Experimental 0 alt number: 117100
}
126,9 → 126,9
debugOut.analog[25] = dThrottleI;
debugOut.analog[26] = dThrottleD;
 
debugOut.analog[27] = dynamicParams.heightP;
debugOut.analog[28] = dynamicParams.heightI;
debugOut.analog[29] = dynamicParams.heightD;
//debugOut.analog[27] = dynamicParams.heightP;
//debugOut.analog[28] = dynamicParams.heightI;
//debugOut.analog[29] = dynamicParams.heightD;
 
int16_t dThrottle = dThrottleI + dThrottleP + dThrottleD;
 
/branches/dongfang_FC_rewrite/makefile
257,6 → 257,9
AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex $(FUSE_SETTINGS)
#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep
 
# Make a dummy read in order to cycle the reset pin on the MCU.
AVRDUDE_RESET = -u -U lfuse:r:m
 
#avrdude -c avrispv2 -P usb -p m32 -U flash:w:blink.hex
AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) -B 1
 
391,6 → 394,9
program: $(TARGET).hex $(TARGET).eep
$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM)
 
reset:
$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_RESET)
 
# Create final output files (.hex, .eep) from ELF output file.
%.hex: %.elf
@echo
/branches/dongfang_FC_rewrite/rc.c
271,9 → 271,9
commandTimer = 0;
}
} else { // Bad signal
RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE]
= RC_PRTY[CONTROL_YAW] = 0;
RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0;
}
debugOut.analog[18] = RCQuality;
}
 
/*
/branches/dongfang_FC_rewrite/uart0.c
87,7 → 87,7
"Yaw Term ", //15
"Throttle Term ",
"ControlAct/10 ",
"Acc. Vector ",
"RC Qual ",
"heightTrottleIn ",
"HeightdThrottle ", //20
"Height ",
96,11 → 96,12
"HeightPdThrottle",
"HeightIdThrottle", //25
"HeightDdThrottle",
"HeightPFactor ",
"HeightIFactor ",
"HeightDFactor ",
"Altitude ", //30
"AirpressADC " };
"NaviStickPitch ",
"NaviStickRoll ",
"StickPitch ",
"StickRoll ", //30
"navi mode "
};
 
/****************************************************************/
/* Initialization of the USART0 */
653,11 → 654,9
if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D)
&& txd_complete) {
sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D));
data3D.anglePitch = (int16_t) ((10 * angle[PITCH])
/ GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
data3D.angleRoll = (int16_t) ((10 * angle[ROLL])
/ GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
data3D.heading = (int16_t) ((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1°
data3D.anglePitch = (int16_t) (angle[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1°
data3D.angleRoll = (int16_t) (angle[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1°
data3D.heading = (int16_t) (yawGyroHeading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1°
data3D_timer = setDelay(data3D_interval);
request_data3D = FALSE;
}