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