/branches/dongfang_FC_rewrite/gps.c |
---|
File deleted |
/branches/dongfang_FC_rewrite/gps.h |
---|
File deleted |
/branches/dongfang_FC_rewrite/GPSControl.h |
---|
File deleted |
/branches/dongfang_FC_rewrite/GPSControl.c |
---|
File deleted |
/branches/dongfang_FC_rewrite/dsl.c |
---|
File deleted |
/branches/dongfang_FC_rewrite/userparams.txt |
---|
File deleted |
\ No newline at end of file |
/branches/dongfang_FC_rewrite/dsl.h |
---|
File deleted |
/branches/dongfang_FC_rewrite/configuration.c |
---|
293,7 → 293,7 |
} |
staticParams.bitConfig = |
CFG_GYRO_SATURATION_PREVENTION | CFG_HEADING_HOLD; |
CFG_GYRO_SATURATION_PREVENTION | CFG_HEADING_HOLD | CFG_COMPASS_ACTIVE; |
memcpy(staticParams.name, "Default\0", 6); |
} |
/branches/dongfang_FC_rewrite/directGPSNaviControl.c |
---|
91,9 → 91,9 |
} |
// checks nick and roll sticks for manual control |
uint8_t GPS_isManuallyControlled(int16_t* naviSticks) { |
if (naviSticks[CONTROL_PITCH] < staticParams.naviStickThreshold |
&& naviSticks[CONTROL_ROLL] < staticParams.naviStickThreshold) |
uint8_t GPS_isManuallyControlled(int16_t* sticks) { |
if (sticks[CONTROL_PITCH] < staticParams.naviStickThreshold |
&& sticks[CONTROL_ROLL] < staticParams.naviStickThreshold) |
return 0; |
else |
return 1; |
101,9 → 101,8 |
// set given position to current gps position |
uint8_t GPS_setCurrPosition(GPS_Pos_t * pGPSPos) { |
uint8_t retval = 0; |
if (pGPSPos == NULL) |
return (retval); // bad pointer |
return 0; // bad pointer |
if (GPS_isSignalOK()) { // is GPS signal condition is fine |
pGPSPos->longitude = GPSInfo.longitude; |
110,27 → 109,24 |
pGPSPos->latitude = GPSInfo.latitude; |
pGPSPos->altitude = GPSInfo.altitude; |
pGPSPos->status = NEWDATA; |
retval = 1; |
return 1; |
} else { // bad GPS signal condition |
pGPSPos->status = INVALID; |
retval = 0; |
return 0; |
} |
return (retval); |
} |
// clear position |
uint8_t GPS_clearPosition(GPS_Pos_t * pGPSPos) { |
uint8_t retval = 0; |
if (pGPSPos == NULL) |
return retval; // bad pointer |
return 0; // bad pointer |
else { |
pGPSPos->longitude = 0; |
pGPSPos->latitude = 0; |
pGPSPos->altitude = 0; |
pGPSPos->status = INVALID; |
retval = 1; |
} |
return (retval); |
return 1; |
} |
// calculates the GPS control stick values from the deviation to target position |
140,8 → 136,7 |
static int32_t PID_Nick, 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; |
int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
int32_t PID_North = 0, PID_East = 0; |
static int32_t cos_target_latitude = 1; |
static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0; |
149,10 → 144,8 |
// if GPS data and Compass are ok |
if (GPS_isSignalOK() && (magneticHeading >= 0)) { |
if (pTargetPos != NULL) // if there is a target position |
{ |
if (pTargetPos->status != INVALID) // and the position data are valid |
{ |
if (pTargetPos != NULL) { // if there is a target position |
if (pTargetPos->status != INVALID) { // and the position data are valid |
// if the target data are updated or the target pointer has changed |
if ((pTargetPos->status != PROCESSED) |
|| (pTargetPos != pLastTargetPos)) { |
160,7 → 153,7 |
GPSPosDevIntegral_North = 0; |
GPSPosDevIntegral_East = 0; |
// recalculate latitude projection |
cos_target_latitude = int_cos((int16_t) (pTargetPos->latitude / 10000000L)); |
cos_target_latitude = cos_360((int16_t) (pTargetPos->latitude / 10000000L)); |
// remember last target pointer |
pLastTargetPos = pTargetPos; |
// mark data as processed |
236,8 → 229,9 |
// in the flight.c. Therefore a positive north deviation/velocity should result in a positive |
// GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll. |
coscompass = int_cos(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
sincompass = int_sin(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
coscompass = cos_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
sincompass = sin_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> MATH_UNIT_FACTOR_LOG; |
PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> MATH_UNIT_FACTOR_LOG; |
261,8 → 255,9 |
// store home position if start of flight flag is set |
if (MKFlags & MKFLAG_CALIBRATE) { |
MKFlags &= ~(MKFLAG_CALIBRATE); |
if (GPS_setCurrPosition(&homePosition)) |
beep(700); |
beep(500); |
} |
switch (GPSInfo.status) { |
360,5 → 355,8 |
GPSInfo.status = PROCESSED; |
break; |
} // eof GPSInfo.status |
debugOut.analog[11] = GPSInfo.satnum; |
} |
/branches/dongfang_FC_rewrite/dongfangMath.c |
---|
193,7 → 193,7 |
(int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (32767) }; |
int16_t int_sin(int32_t arg) { |
int16_t sin_mkdegrees(int32_t arg) { |
int8_t sign; |
int16_t result; |
int16_t argp = arg / MATH_DRG_FACTOR; |
211,13 → 211,13 |
return (sign == 1) ? result : -result; |
} |
int16_t int_cos(int32_t arg) { |
int16_t cos_mkdegrees(int32_t arg) { |
if (arg > 90L * MATH_DRG_FACTOR) |
return int_sin(arg + (90L - 360L) * MATH_DRG_FACTOR); |
return int_sin(arg + 90L * MATH_DRG_FACTOR); |
return sin_mkdegrees(arg + (90L - 360L) * MATH_DRG_FACTOR); |
return sin_mkdegrees(arg + 90L * MATH_DRG_FACTOR); |
} |
int16_t int_tan(int32_t arg) { |
int16_t tan_mkdegrees(int32_t arg) { |
int8_t sign = 1; |
int16_t result; |
int16_t argp = arg / MATH_DRG_FACTOR; |
233,3 → 233,26 |
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; |
} |
if (arg >= 90) { |
arg = 180 - arg; |
} |
result = pgm_read_word(&SIN_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,6 → 17,10 |
#define MATH_UNIT_FACTOR_LOG 12 |
#define MATH_UNIT_FACTOR (1L<<MATH_UNIT_FACTOR_LOG) |
int16_t int_sin(int32_t arg); |
int16_t int_cos(int32_t arg); |
int16_t int_tan(int32_t arg); |
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 |
---|
386,10 → 386,10 |
debugOut.analog[7] = filteredAcc[ROLL]; |
debugOut.analog[8] = filteredAcc[Z]; |
debugOut.analog[12] = term[PITCH]; |
debugOut.analog[13] = term[ROLL]; |
debugOut.analog[14] = yawTerm; |
debugOut.analog[15] = throttleTerm; |
debugOut.analog[13] = term[PITCH]; |
debugOut.analog[14] = term[ROLL]; |
debugOut.analog[15] = yawTerm; |
debugOut.analog[16] = throttleTerm; |
for (i = 0; i < MAX_MOTORS; i++) { |
int32_t tmp; |
/branches/dongfang_FC_rewrite/makefile |
---|
19,18 → 19,18 |
EXT = DIRECT_GPS |
#EXT = |
#GYRO=ENC-03_FC1.3 |
#GYRO_HW_NAME=ENC |
#GYRO_HW_FACTOR=1.304f |
GYRO=ENC-03_FC1.3 |
GYRO_HW_NAME=ENC |
GYRO_HW_FACTOR=1.304f |
GYRO_PITCHROLL_CORRECTION=1.0f |
GYRO_YAW_CORRECTION=1.0f |
#GYRO=ADXRS610_FC2.0 |
#GYRO_HW_NAME=ADXR |
#GYRO_HW_FACTOR=1.2288f |
#GYRO_PITCHROLL_CORRECTION=1.0f |
#GYRO_YAW_CORRECTION=1.0f |
GYRO=ADXRS610_FC2.0 |
GYRO_HW_NAME=ADXR |
GYRO_HW_FACTOR=1.2288f |
GYRO_PITCHROLL_CORRECTION=1.0f |
GYRO_YAW_CORRECTION=1.0f |
#GYRO=invenSense |
#GYRO_HW_NAME=Isense |
#GYRO_HW_FACTOR=0.6827f |
/branches/dongfang_FC_rewrite/mk3mag.c |
---|
130,9 → 130,11 |
if (!PWMTimeout) { |
if (checkDelay(beepDelay)) { |
if (!beepTime) |
beep(100); // make noise with 10Hz to signal the compass problem |
beep(50); // make noise with 10Hz to signal the compass problem |
beepDelay = setDelay(100); |
} |
} |
debugOut.analog[12] = magneticHeading; |
} |
/branches/dongfang_FC_rewrite/timer0.c |
---|
138,7 → 138,7 |
ISR(TIMER0_OVF_vect) |
{ // 9765.625 Hz |
static uint8_t cnt_1ms = 1, cnt = 0; |
uint8_t beeper_On = 0; |
uint8_t beeperOn = 0; |
#ifdef USE_NAVICTRL |
if(SendSPI) SendSPI--; // if SendSPI is 0, the transmit of a byte via SPI bus to and from The Navicontrol is done |
161,16 → 161,15 |
if (beepTime) { |
beepTime--; // decrement BeepTime |
if (beepTime & beepModulation) |
// beeper_On = 1; |
beeper_On = 1; // shut up in dev!!! |
beeperOn = 1; |
else |
beeper_On = 0; |
beeperOn = 0; |
} else { // beeper off if duration is over |
beeper_On = 0; |
beeperOn = 0; |
beepModulation = BEEP_MODULATION_NONE; |
} |
if (beeper_On) { |
if (beeperOn) { |
// set speaker port to high. |
if (boardRelease == 10) |
PORTD |= (1 << PORTD2); // Speaker at PD2 |
184,14 → 183,12 |
PORTC &= ~(1 << PORTC7);// Speaker at PC7 |
} |
#ifndef USE_NAVICTRL |
#ifdef USE_DIRECT_GPS |
// update compass value if this option is enabled in the settings |
if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
#ifdef USE_DIRECT_GPS |
MK3MAG_periodicTask(); // read out mk3mag pwm |
#endif |
} |
#endif |
} |
// ----------------------------------------------------------------------- |
/branches/dongfang_FC_rewrite/uart0.c |
---|
80,12 → 80,12 |
"AccZ ", |
"AccPitch (angle)", |
"AccRoll (angle) ", //10 |
"- ", |
"GPS sat ", |
"mag hdng ", |
"Pitch Term ", |
"Roll Term ", |
"Yaw Term ", |
"Throttle Term ", //15 |
"- ", |
"Yaw Term ", //15 |
"Throttle Term ", |
"ControlAct/10 ", |
"Acc. Vector ", |
"heightTrottleIn ", |
/branches/dongfang_FC_rewrite/ubx.c |
---|
26,34 → 26,34 |
typedef struct { |
uint32_t ITOW; // ms GPS Millisecond Time of Week |
int32_t Frac; // ns remainder of rounded ms above |
int32_t frac; // ns remainder of rounded ms above |
int16_t week; // GPS week |
uint8_t GPSfix; // GPSfix Type, range 0..6 |
uint8_t Flags; // Navigation Status Flags |
uint8_t flags; // Navigation Status Flags |
int32_t ECEF_X; // cm ECEF X coordinate |
int32_t ECEF_Y; // cm ECEF Y coordinate |
int32_t ECEF_Z; // cm ECEF Z coordinate |
uint32_t PAcc; // cm 3D Position Accuracy Estimate |
uint32_t pAcc; // cm 3D Position Accuracy Estimate |
int32_t ECEFVX; // cm/s ECEF X velocity |
int32_t ECEFVY; // cm/s ECEF Y velocity |
int32_t ECEFVZ; // cm/s ECEF Z velocity |
uint32_t SAcc; // cm/s Speed Accuracy Estimate |
uint32_t sAcc; // cm/s Speed Accuracy Estimate |
uint16_t PDOP; // 0.01 Position DOP |
uint8_t res1; // reserved |
uint8_t numSV; // Number of SVs used in navigation solution |
uint32_t res2; // reserved |
Status_t Status; |
Status_t status; |
} UBX_SOL_t; |
typedef struct { |
uint32_t ITOW; // ms GPS Millisecond Time of Week |
int32_t LON; // 1e-07 deg Longitude |
int32_t LAT; // 1e-07 deg Latitude |
int32_t HEIGHT; // mm Height above Ellipsoid |
int32_t lon; // 1e-07 deg Longitude |
int32_t lat; // 1e-07 deg Latitude |
int32_t height; // mm Height above Ellipsoid |
int32_t HMSL; // mm Height above mean sea level |
uint32_t Hacc; // mm Horizontal Accuracy Estimate |
uint32_t Vacc; // mm Vertical Accuracy Estimate |
Status_t Status; |
uint32_t hAcc; // mm Horizontal Accuracy Estimate |
uint32_t vAacc; // mm Vertical Accuracy Estimate |
Status_t status; |
} UBX_POSLLH_t; |
typedef struct { |
61,52 → 61,51 |
int32_t VEL_N; // cm/s NED north velocity |
int32_t VEL_E; // cm/s NED east velocity |
int32_t VEL_D; // cm/s NED down velocity |
uint32_t Speed; // cm/s Speed (3-D) |
uint32_t GSpeed; // cm/s Ground Speed (2-D) |
int32_t Heading; // 1e-05 deg Heading 2-D |
uint32_t SAcc; // cm/s Speed Accuracy Estimate |
uint32_t CAcc; // deg Course / Heading Accuracy Estimate |
Status_t Status; |
uint32_t speed; // cm/s Speed (3-D) |
uint32_t gSpeed; // cm/s Ground Speed (2-D) |
int32_t heading; // 1e-05 deg Heading 2-D |
uint32_t sAcc; // cm/s Speed Accuracy Estimate |
uint32_t cAcc; // deg Course / Heading Accuracy Estimate |
Status_t status; |
} UBX_VELNED_t; |
UBX_SOL_t UbxSol = |
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID }; |
UBX_POSLLH_t UbxPosLlh = { 0, 0, 0, 0, 0, 0, 0, INVALID }; |
UBX_VELNED_t UbxVelNed = { 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID }; |
UBX_SOL_t ubxSol = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID }; |
UBX_POSLLH_t ubxPosLlh = { 0, 0, 0, 0, 0, 0, 0, INVALID }; |
UBX_VELNED_t ubxVelNed = { 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID }; |
GPS_INFO_t GPSInfo = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, INVALID }; |
volatile uint8_t GPSTimeout = 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) { |
GPSInfo.status = INVALID; |
// NAV SOL |
GPSInfo.flags = UbxSol.Flags; |
GPSInfo.satfix = UbxSol.GPSfix; |
GPSInfo.satnum = UbxSol.numSV; |
GPSInfo.PAcc = UbxSol.PAcc; |
GPSInfo.VAcc = UbxSol.SAcc; |
GPSInfo.flags = ubxSol.flags; |
GPSInfo.satfix = ubxSol.GPSfix; |
GPSInfo.satnum = ubxSol.numSV; |
GPSInfo.PAcc = ubxSol.pAcc; |
GPSInfo.VAcc = ubxSol.sAcc; |
// NAV POSLLH |
GPSInfo.longitude = UbxPosLlh.LON; |
GPSInfo.latitude = UbxPosLlh.LAT; |
GPSInfo.altitude = UbxPosLlh.HEIGHT; |
GPSInfo.longitude = ubxPosLlh.lon; |
GPSInfo.latitude = ubxPosLlh.lat; |
GPSInfo.altitude = ubxPosLlh.height; |
GPSInfo.veleast = UbxVelNed.VEL_E; |
GPSInfo.velnorth = UbxVelNed.VEL_N; |
GPSInfo.veltop = -UbxVelNed.VEL_D; |
GPSInfo.velground = UbxVelNed.GSpeed; |
GPSInfo.veleast = ubxVelNed.VEL_E; |
GPSInfo.velnorth = ubxVelNed.VEL_N; |
GPSInfo.veltop = -ubxVelNed.VEL_D; |
GPSInfo.velground = ubxVelNed.gSpeed; |
GPSInfo.status = NEWDATA; |
} |
// set state to collect new data |
UbxSol.Status = PROCESSED; // never update old data |
UbxPosLlh.Status = PROCESSED; // never update old data |
UbxVelNed.Status = PROCESSED; // never update old data |
ubxSol.status = PROCESSED; // never update old data |
ubxPosLlh.status = PROCESSED; // never update old data |
ubxVelNed.status = PROCESSED; // never update old data |
} |
} |
142,21 → 141,21 |
case UBXSTATE_CLASS: // check message identifier |
switch (c) { |
case UBX_ID_POSLLH: // geodetic position |
ubxP = (int8_t *) &UbxPosLlh; // data start pointer |
ubxEp = (int8_t *) (&UbxPosLlh + 1); // data end pointer |
ubxSp = (int8_t *) &UbxPosLlh.Status; // status pointer |
ubxP = (int8_t *) &ubxPosLlh; // data start pointer |
ubxEp = (int8_t *) (&ubxPosLlh + 1); // data end pointer |
ubxSp = (int8_t *) &ubxPosLlh.status; // status pointer |
break; |
case UBX_ID_SOL: // navigation solution |
ubxP = (int8_t *) &UbxSol; // data start pointer |
ubxEp = (int8_t *) (&UbxSol + 1); // data end pointer |
ubxSp = (int8_t *) &UbxSol.Status; // status pointer |
ubxP = (int8_t *) &ubxSol; // data start pointer |
ubxEp = (int8_t *) (&ubxSol + 1); // data end pointer |
ubxSp = (int8_t *) &ubxSol.status; // status pointer |
break; |
case UBX_ID_VELNED: // velocity vector in tangent plane |
ubxP = (int8_t *) &UbxVelNed; // data start pointer |
ubxEp = (int8_t *) (&UbxVelNed + 1); // data end pointer |
ubxSp = (int8_t *) &UbxVelNed.Status; // status pointer |
ubxP = (int8_t *) &ubxVelNed; // data start pointer |
ubxEp = (int8_t *) (&ubxVelNed + 1); // data end pointer |
ubxSp = (int8_t *) &ubxVelNed.status; // status pointer |
break; |
default: // unsupported identifier |
227,4 → 226,3 |
break; |
} |
} |