Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2043 → Rev 2044

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