| /branches/dongfang_FC_rewrite/attitude.c |
|---|
| 74,10 → 74,6 |
| // For Servo_On / Off |
| // #include "timer2.h" |
| #ifdef USE_MK3MAG |
| #include "mk3mag.h" |
| #include "gps.h" |
| #endif |
| #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
| /* |
| 113,13 → 109,22 |
| int readingHeight = 0; |
| // compass course |
| int16_t compassHeading = -1; // negative angle indicates invalid data. |
| // Yaw angle and compass stuff. |
| // This is updated/written from MM3. Negative angle indicates invalid data. |
| int16_t compassHeading = -1; |
| // This is NOT updated from MM3. Negative angle indicates invalid data. |
| int16_t compassCourse = -1; |
| int16_t compassOffCourse = 0; |
| uint16_t updateCompassCourse = 0; |
| // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
| // Not necessary. Never read anywhere. |
| // int16_t compassOffCourse = 0; |
| uint8_t updateCompassCourse = 0; |
| uint8_t compassCalState = 0; |
| uint16_t badCompassHeading = 500; |
| uint16_t ignoreCompassTimer = 500; |
| int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
| int16_t yawGyroDrift; |
| 202,8 → 207,10 |
| uint8_t axis; |
| for (axis=PITCH; axis <=ROLL; axis++) { |
| rate_PID[axis] = (gyro_PID[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
| rate_ATT[axis] = (gyro_ATT[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
| rate_PID[axis] = (gyro_PID[axis] + driftComp[axis]) |
| / HIRES_GYRO_INTEGRATION_FACTOR; |
| rate_ATT[axis] = (gyro_ATT[axis] + driftComp[axis]) |
| / HIRES_GYRO_INTEGRATION_FACTOR; |
| differential[axis] = gyroD[axis]; |
| averageAcc[axis] += acc[axis]; |
| } |
| 229,9 → 236,14 |
| int16_t sinroll = int_sin(angle[ROLL]); |
| int16_t tanpitch = int_tan(angle[PITCH]); |
| #define ANTIOVF 512 |
| ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
| ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t)rate_ATT[PITCH] * sinroll / ANTIOVF * tanpitch + (int32_t)yawRate * int_cos(angle[ROLL]) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
| ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
| ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
| * sinroll) / (int32_t) MATH_UNIT_FACTOR; |
| ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t) rate_ATT[PITCH] * sinroll |
| / ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL]) |
| / ANTIOVF * tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF |
| * MATH_UNIT_FACTOR)); |
| ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch |
| + ((int32_t) yawRate * cosroll) / cospitch; |
| } |
| // 480 usec with axis coupling - almost no time without. |
| 287,7 → 299,8 |
| // Well actually the Z axis acc. check is not so silly. |
| uint8_t axis; |
| int32_t correction; |
| if(!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] <= dynamicParams.UserParams[7]) { |
| if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
| <= dynamicParams.UserParams[7]) { |
| DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
| uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
| 314,11 → 327,13 |
| */ |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| accDerived = getAngleEstimateFromAcc(axis); |
| DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
| DebugOut.Analog[9 + axis] = (10 * accDerived) |
| / GYRO_DEG_FACTOR_PITCHROLL; |
| // 1000 * the correction amount that will be added to the gyro angle in next line. |
| correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
| angle[axis] = ((int32_t)(1000L - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived) / 1000L; |
| angle[axis] = ((int32_t) (1000L - permilleAcc) * angle[axis] |
| + (int32_t) permilleAcc * accDerived) / 1000L; |
| correctionSum[axis] += angle[axis] - correction; |
| DebugOut.Analog[16+axis] = angle[axis] - correction; |
| } |
| 355,13 → 370,16 |
| timer = DRIFTCORRECTION_TIME; |
| for (axis=PITCH; axis<=ROLL; axis++) { |
| // Take the sum of corrections applied, add it to delta |
| deltaCorrection = (correctionSum[axis] * HIRES_GYRO_INTEGRATION_FACTOR + DRIFTCORRECTION_TIME / 2) / DRIFTCORRECTION_TIME; |
| deltaCorrection = (correctionSum[axis] |
| * HIRES_GYRO_INTEGRATION_FACTOR + DRIFTCORRECTION_TIME / 2) |
| / DRIFTCORRECTION_TIME; |
| // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
| driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
| CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
| // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| DebugOut.Analog[18+axis] = deltaCorrection / staticParams.GyroAccTrim; |
| DebugOut.Analog[18 + axis] = deltaCorrection |
| / staticParams.GyroAccTrim; |
| DebugOut.Analog[28+axis] = driftComp[axis]; |
| correctionSum[axis] = 0; |
| 403,23 → 421,31 |
| if(compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) { |
| if (controlMixer_testCompassCalState()) { |
| compassCalState++; |
| if(compassCalState < 5) beepNumber(compassCalState); |
| else beep(1000); |
| if (compassCalState < 5) |
| beepNumber(compassCalState); |
| else |
| beep(1000); |
| } |
| } else { |
| // get maximum attitude angle |
| w = abs(angle[PITCH] / 512); |
| v = abs(angle[ROLL] / 512); |
| if(v > w) w = v; |
| if (v > w) |
| w = v; |
| correction = w / 8 + 1; |
| // calculate the deviation of the yaw gyro heading and the compass heading |
| if (compassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined |
| else error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180; |
| if (compassHeading < 0) |
| error = 0; // disable yaw drift compensation if compass heading is undefined |
| else |
| if(abs(yawRate) > 128) { // spinning fast |
| error = 0; |
| } else { |
| // compassHeading - yawGyroHeading, on a -180..179 deg interval. |
| error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180; |
| } |
| if(!badCompassHeading && w < 25) { |
| if (!ignoreCompassTimer && w < 25) { |
| yawGyroDrift += error; |
| // Basically this gets set if we are in "fix" mode, and when starting. |
| if(updateCompassCourse) { |
| beep(200); |
| yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW; |
| 428,26 → 454,34 |
| } |
| } |
| yawGyroHeading += (error * 8) / correction; |
| /* |
| w = (w * dynamicParams.CompassYawEffect) / 32; |
| w = dynamicParams.CompassYawEffect - w; |
| if(w >= 0) { |
| if(!badCompassHeading) { |
| */ |
| w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect) / 32; |
| // As readable formula: |
| // w = dynamicParams.CompassYawEffect * (1-w/32); |
| if (w >= 0) { // maxAttitudeAngle < 32 |
| if (!ignoreCompassTimer) { |
| v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8; |
| // calc course deviation |
| r = ((540 + (yawGyroHeading / GYRO_DEG_FACTOR_YAW) - compassCourse) % 360) - 180; |
| // yawGyroHeading - compassCourse on a -180..179 degree interval. |
| r = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) % 360) - 180; |
| v = (r * w) / v; // align to compass course |
| // limit yaw rate |
| w = 3 * dynamicParams.CompassYawEffect; |
| if (v > w) v = w; |
| else if (v < -w) v = -w; |
| if (v > w) |
| v = w; |
| else if (v < -w) |
| v = -w; |
| yawAngleDiff += v; |
| } else { // wait a while |
| ignoreCompassTimer--; |
| } |
| else |
| { // wait a while |
| badCompassHeading--; |
| } |
| } else { // ignore compass at extreme attitudes for a while |
| badCompassHeading = 500; |
| ignoreCompassTimer = 500; |
| } |
| } |
| } |
| /branches/dongfang_FC_rewrite/attitude.h |
|---|
| 96,12 → 96,12 |
| */ |
| extern int16_t compassHeading; |
| extern int16_t compassCourse; |
| extern int16_t compassOffCourse; |
| // extern int16_t compassOffCourse; |
| extern uint8_t compassCalState; |
| extern int32_t yawGyroHeading; |
| extern int16_t yawGyroHeadingInDeg; |
| extern uint16_t updateCompassCourse; |
| extern uint16_t badCompassHeading; |
| extern uint8_t updateCompassCourse; |
| extern uint16_t ignoreCompassTimer; |
| void updateCompass(void); |
| /branches/dongfang_FC_rewrite/configuration.c |
|---|
| 92,7 → 92,8 |
| SET_POT(dynamicParams.DynamicStability,staticParams.DynamicStability); |
| SET_POT_MM(dynamicParams.J16Timing,staticParams.J16Timing,1,255); |
| SET_POT_MM(dynamicParams.J17Timing,staticParams.J17Timing,1,255); |
| #if defined (USE_MK3MAG) |
| #if defined (USE_NAVICTRL) |
| SET_POT(dynamicParams.NaviGpsModeControl,staticParams.NaviGpsModeControl); |
| SET_POT(dynamicParams.NaviGpsGain,staticParams.NaviGpsGain); |
| SET_POT(dynamicParams.NaviGpsP,staticParams.NaviGpsP); |
| /branches/dongfang_FC_rewrite/flight.c |
|---|
| 265,7 → 265,7 |
| /* Yawing */ |
| /************************************************************************/ |
| if(abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
| badCompassHeading = 1000; |
| ignoreCompassTimer = 1000; |
| if(!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
| updateCompassCourse = 1; |
| } |
| 288,7 → 288,7 |
| updateCompass(); |
| } |
| #if defined (USE_MK3MAG) |
| #if defined (USE_NAVICTRL) |
| /************************************************************************/ |
| /* GPS is currently not supported. */ |
| /************************************************************************/ |
| 295,8 → 295,7 |
| if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) { |
| GPS_Main(); |
| MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
| } |
| else { |
| } else { |
| // GPSStickPitch = 0; |
| // GPSStickRoll = 0; |
| } |
| /branches/dongfang_FC_rewrite/gps.c |
|---|
| 56,8 → 56,7 |
| #include "rc.h" |
| #include "eeprom.h" |
| typedef enum |
| { |
| typedef enum { |
| GPS_FLIGHT_MODE_UNDEF, |
| GPS_FLIGHT_MODE_FREE, |
| GPS_FLIGHT_MODE_AID, |
| 68,9 → 67,7 |
| #define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes |
| #define GPS_P_LIMIT 25 |
| typedef struct |
| { |
| typedef struct { |
| int32_t Longitude; |
| int32_t Latitude; |
| int32_t Altitude; |
| 84,7 → 81,6 |
| // the current flight mode |
| FlightMode_t FlightMode = GPS_FLIGHT_MODE_UNDEF; |
| // --------------------------------------------------------------------------------- |
| void GPS_UpdateParameter(void) { |
| static FlightMode_t FlightModeOld = GPS_FLIGHT_MODE_UNDEF; |
| 92,9 → 88,12 |
| if((RC_Quality < 100) || (MKFlags & MKFLAG_EMERGENCY_LANDING)) { |
| FlightMode = GPS_FLIGHT_MODE_FREE; |
| } else { |
| if (dynamicParams.NaviGpsModeControl < 50) FlightMode = GPS_FLIGHT_MODE_AID; |
| else if(dynamicParams.NaviGpsModeControl < 180) FlightMode = GPS_FLIGHT_MODE_FREE; |
| else FlightMode = GPS_FLIGHT_MODE_HOME; |
| if (dynamicParams.NaviGpsModeControl < 50) |
| FlightMode = GPS_FLIGHT_MODE_AID; |
| else if (dynamicParams.NaviGpsModeControl < 180) |
| FlightMode = GPS_FLIGHT_MODE_FREE; |
| else |
| FlightMode = GPS_FLIGHT_MODE_HOME; |
| } |
| if (FlightMode != FlightModeOld) { |
| 107,12 → 106,14 |
| // This function defines a good GPS signal condition |
| uint8_t GPS_IsSignalOK(void) { |
| static uint8_t GPSFix = 0; |
| if( (GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D) && (GPSInfo.flags & FLAG_GPSFIXOK) && ((GPSInfo.satnum >= staticParams.NaviGpsMinSat) || GPSFix)) { |
| if ((GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D) |
| && (GPSInfo.flags & FLAG_GPSFIXOK) && ((GPSInfo.satnum |
| >= staticParams.NaviGpsMinSat) || GPSFix)) { |
| GPSFix = 1; |
| return 1; |
| } else |
| return (0); |
| } |
| else return (0); |
| } |
| // --------------------------------------------------------------------------------- |
| // rescale xy-vector length to limit |
| 131,14 → 132,20 |
| // checks nick and roll sticks for manual control |
| uint8_t GPS_IsManualControlled(void) { |
| if ((abs(PPM_in[staticParams.ChannelAssignment[CH_NICK]]) < staticParams.NaviStickThreshold) && (abs(PPM_in[staticParams.ChannelAssignment[CH_ROLL]]) < staticParams.NaviStickThreshold)) return 0; |
| else return 1; |
| if ((abs(PPM_in[staticParams.ChannelAssignment[CH_NICK]]) |
| < staticParams.NaviStickThreshold) |
| && (abs(PPM_in[staticParams.ChannelAssignment[CH_ROLL]]) |
| < staticParams.NaviStickThreshold)) |
| return 0; |
| else |
| return 1; |
| } |
| // 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 |
| if (pGPSPos == NULL) |
| return (retval); // bad pointer |
| if(GPS_IsSignalOK()) { // is GPS signal condition is fine |
| pGPSPos->Longitude = GPSInfo.longitude; |
| 181,7 → 188,8 |
| 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; |
| 194,13 → 202,14 |
| 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) ) |
| { |
| if ((pTargetPos->Status != PROCESSED) || (pTargetPos |
| != pLastTargetPos)) { |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| GPSPosDevIntegral_East = 0; |
| // recalculate latitude projection |
| cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L)); |
| cos_target_latitude = (int32_t) c_cos_8192( |
| (int16_t) (pTargetPos->Latitude / 10000000L)); |
| // remember last target pointer |
| pLastTargetPos = pTargetPos; |
| // mark data as processed |
| 212,9 → 221,7 |
| // calculate latitude projection |
| GPSPosDev_East *= cos_target_latitude; |
| GPSPosDev_East /= 8192; |
| } |
| else // no valid target position available |
| { |
| } else {// no valid target position available |
| // reset error |
| GPSPosDev_North = 0; |
| GPSPosDev_East = 0; |
| 222,9 → 229,7 |
| GPSPosDevIntegral_North = 0; |
| GPSPosDevIntegral_East = 0; |
| } |
| } |
| else // no target position available |
| { |
| } else { // no target position available |
| // reset error |
| GPSPosDev_North = 0; |
| GPSPosDev_East = 0; |
| 244,17 → 249,19 |
| P_East = ((int32_t)dynamicParams.NaviGpsP * GPSPosDev_East)/2048; |
| // I-Part |
| I_North = ((int32_t)dynamicParams.NaviGpsI * GPSPosDevIntegral_North)/8192; |
| I_East = ((int32_t)dynamicParams.NaviGpsI * GPSPosDevIntegral_East)/8192; |
| I_North = ((int32_t) dynamicParams.NaviGpsI * GPSPosDevIntegral_North) |
| / 8192; |
| I_East = ((int32_t) dynamicParams.NaviGpsI * GPSPosDevIntegral_East) |
| / 8192; |
| // combine P & I |
| PID_North = P_North + I_North; |
| PID_East = P_East + I_East; |
| if(!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT)) |
| { |
| if (!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT)) { |
| GPSPosDevIntegral_North += GPSPosDev_North/16; |
| GPSPosDevIntegral_East += GPSPosDev_East/16; |
| GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, GPS_POSINTEGRAL_LIMIT); |
| GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East, |
| GPS_POSINTEGRAL_LIMIT); |
| } |
| // combine PI- and D-Part |
| 288,9 → 295,7 |
| GPSStickNick = (int16_t)PID_Nick; |
| GPSStickRoll = (int16_t)PID_Roll; |
| } |
| else // invalid GPS data or bad compass reading |
| { |
| } else { // invalid GPS data or bad compass reading |
| GPS_Neutral(); // do nothing |
| // reset error integral |
| GPSPosDevIntegral_North = 0; |
| 306,7 → 311,8 |
| // store home position if start of flight flag is set |
| if(MKFlags & MKFLAG_CALIBRATE) { |
| if(GPS_SetCurrPosition(&HomePosition)) BeepTime = 700; |
| if (GPS_SetCurrPosition(&HomePosition)) |
| BeepTime = 700; |
| } |
| switch(GPSInfo.status) { |
| 318,11 → 324,11 |
| break; |
| case PROCESSED: // if gps data are already processed do nothing |
| // downcount timeout |
| if(GPSTimeout) GPSTimeout--; |
| if (GPSTimeout) |
| GPSTimeout--; |
| // if no new data arrived within timeout set current data invalid |
| // and therefore disable GPS |
| else |
| { |
| else { |
| GPS_Neutral(); |
| GPSInfo.status = INVALID; |
| } |
| 330,11 → 336,8 |
| case NEWDATA: // new valid data from gps device |
| // if the gps data quality is good |
| beep_rythm++; |
| if (GPS_IsSignalOK()) |
| { |
| switch(FlightMode) // check what's to do |
| { |
| if (GPS_IsSignalOK()) { |
| 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 |
| 343,28 → 346,23 |
| break; |
| case GPS_FLIGHT_MODE_AID: |
| if(HoldPosition.Status != INVALID) |
| { |
| if( GPS_IsManualControlled() ) // MK controlled by user |
| { |
| if (HoldPosition.Status != INVALID) { |
| if (GPS_IsManualControlled()) { // MK controlled by user |
| // update hold point to current gps position |
| GPS_SetCurrPosition(&HoldPosition); |
| // disable gps control |
| GPS_Neutral(); |
| GPS_P_Delay = 0; |
| } |
| else // GPS control active |
| { |
| if(GPS_P_Delay < 7) |
| { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
| } else { // GPS control active |
| 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_PIDController(NULL); // activates only the D-Part |
| } else |
| GPS_PIDController(&HoldPosition);// activates the P&D-Part |
| } |
| else GPS_PIDController(&HoldPosition);// activates the P&D-Part |
| } |
| } |
| else // invalid Hold Position |
| } else // invalid Hold Position |
| { // try to catch a valid hold position from gps data input |
| GPS_SetCurrPosition(&HoldPosition); |
| GPS_Neutral(); |
| 372,8 → 370,7 |
| break; |
| case GPS_FLIGHT_MODE_HOME: |
| if(HomePosition.Status != INVALID) |
| { |
| if (HomePosition.Status != INVALID) { |
| // update hold point to current gps position |
| // to avoid a flight back if home comming is deactivated |
| GPS_SetCurrPosition(&HoldPosition); |
| 380,30 → 377,24 |
| if( GPS_IsManualControlled() ) // MK controlled by user |
| { |
| GPS_Neutral(); |
| } |
| else // GPS control active |
| } else // GPS control active |
| { |
| GPS_PIDController(&HomePosition); |
| } |
| } |
| else // bad home position |
| } else // bad home position |
| { |
| BeepTime = 50; // signal invalid home position |
| // try to hold at least the position as a fallback option |
| if (HoldPosition.Status != INVALID) |
| { |
| if (HoldPosition.Status != INVALID) { |
| if( GPS_IsManualControlled() ) // MK controlled by user |
| { |
| GPS_Neutral(); |
| } |
| else // GPS control active |
| } else // GPS control active |
| { |
| GPS_PIDController(&HoldPosition); |
| } |
| } |
| else |
| { // try to catch a valid hold position |
| } else { // try to catch a valid hold position |
| GPS_SetCurrPosition(&HoldPosition); |
| GPS_Neutral(); |
| } |
| 417,11 → 408,13 |
| else // gps data quality is bad |
| { // disable gps control |
| GPS_Neutral(); |
| if(FlightMode != GPS_FLIGHT_MODE_FREE) |
| { |
| if (FlightMode != GPS_FLIGHT_MODE_FREE) { |
| // beep if signal is not sufficient |
| if(!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
| else if (GPSInfo.satnum < staticParams.NaviGpsMinSat && !(beep_rythm % 5)) BeepTime = 10; |
| if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) |
| BeepTime = 100; |
| else if (GPSInfo.satnum < staticParams.NaviGpsMinSat |
| && !(beep_rythm % 5)) |
| BeepTime = 10; |
| } |
| } |
| // set current data as processed to avoid further calculations on the same gps data |
| /branches/dongfang_FC_rewrite/main.c |
|---|
| 136,14 → 136,14 |
| // Instead, while away the time by flashing the 2 outputs: |
| // First J16, then J17. Makes it easier to see which is which. |
| timer = SetDelay(200); |
| OUTPUT_ON(0); |
| OUTPUT_SET(0,1); |
| GRN_OFF; |
| RED_ON; |
| while(!CheckDelay(timer)); |
| timer = SetDelay(200); |
| OUTPUT_OFF(0); |
| OUTPUT_ON(1); |
| OUTPUT_SET(0,0); |
| OUTPUT_SET(1,1); |
| RED_OFF; |
| GRN_ON; |
| while(!CheckDelay(timer)); |
| 150,7 → 150,7 |
| timer = SetDelay(200); |
| while(!CheckDelay(timer)); |
| OUTPUT_OFF(1); |
| OUTPUT_SET(1,0); |
| twi_diagnostics(); |
| /branches/dongfang_FC_rewrite/makefile |
|---|
| 143,7 → 143,7 |
| SRC += eeprom.c uart1.c heightControl.c configuration.c attitudeControl.c commands.c $(GYRO).c |
| ifeq ($(EXT), MK3MAG) |
| SRC += mk3mag.c gps.c ubx.c |
| SRC += mk3mag.c |
| #mymath.c |
| endif |
| ifeq ($(EXT), NAVICTRL) |
| /branches/dongfang_FC_rewrite/menu.c |
|---|
| 60,10 → 60,11 |
| #include "twimaster.h" |
| #include "attitude.h" |
| #if (!defined (USE_MK3MAG)) |
| #if (!defined (USE_NAVICTRL)) |
| uint8_t MaxMenuItem = 13; |
| #else |
| #ifdef USE_MK3MAG |
| #ifdef USE_NAVICTRL |
| #include "gps.c" |
| uint8_t MaxMenuItem = 14; |
| #endif |
| #endif |
| 84,8 → 85,7 |
| /************************************/ |
| /* Clear LCD Buffer */ |
| /************************************/ |
| void LCD_Clear(void) |
| { |
| void LCD_Clear(void) { |
| uint8_t i; |
| for( i = 0; i < DISPLAYBUFFSIZE; i++) DisplayBuff[i] = ' '; |
| } |
| 202,7 → 202,7 |
| LCD_printfxy(0,0,"Compass "); |
| LCD_printfxy(0,1,"Course: %5i", compassCourse); |
| LCD_printfxy(0,2,"Heading: %5i", compassHeading); |
| LCD_printfxy(0,3,"OffCourse: %5i", compassOffCourse); |
| LCD_printfxy(0,3,"OffCourse: %5i", ((540 + compassHeading - compassCourse) % 360) - 180); |
| break; |
| case 9:// Poti Menu Item |
| LCD_printfxy(0,0,"Po1: %3i Po5: %3i" ,variables[0], variables[4]); //PPM24-Extesion |
| 242,7 → 242,7 |
| if(motor[11].Present) LCD_printfxy(12,3,"12"); |
| break; |
| #if (defined (USE_MK3MAG)) |
| #if (defined (USE_NAVICTRL)) |
| case 14://GPS Lat/Lon coords |
| if (GPSInfo.status == INVALID) { |
| LCD_printfxy(0,0,"No GPS data!"); |
| /branches/dongfang_FC_rewrite/mk3mag.c |
|---|
| 87,9 → 87,13 |
| // in other words 100us/° with a +1ms offset. |
| // The signal goes low for 65ms between pulses, |
| // so the cycle time is 65mS + the pulse width. |
| // pwm is high |
| if (debugCounter++ == 5000) { |
| DebugOut.Digital[0] ^= DEBUG_MK3MAG; |
| debugCounter = 0; |
| } |
| if (PINC & (1 << PINC4)) { |
| // If PWM signal is high increment PWM high counter |
| // This counter is incremented by a periode of 102.4us, |
| 107,22 → 111,30 |
| if ((PWMCount) && (PWMCount < 362)) { // 362 * 102.4us = 37.0688 ms |
| if (PWMCount < 10) |
| compassHeading = 0; |
| else |
| else { |
| compassHeading = ((uint32_t) (PWMCount - 10) * 1049L) / 1024; // correct timebase and offset |
| compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
| DebugOut.Digital[1] ^= DEBUG_MK3MAG; // correct signal recd. |
| } |
| /* |
| compassHeading - compassCourse on a -180..179 range. |
| compassHeading 20 compassCourse 30 --> ((540 - 10)%360) - 180 = -10 |
| compassHeading 30 compassCourse 20 --> ((540 + 10)%360) - 180 = 10 |
| compassHeading 350 compassCourse 10 --> ((540 + 340)%360) - 180 = -20 |
| compassHeading 10 compassCourse 350 --> ((540 - 340)%360) - 180 = 20 |
| */ |
| //compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
| PWMTimeout = 12; // if 12 periodes long no valid PWM was detected the data are invalid |
| // 12 * 362 counts * 102.4 us |
| } |
| PWMCount = 0; // reset pwm counter |
| DebugOut.Digital[0] ^= DEBUG_MK3MAG; |
| DebugOut.Digital[1] &= ~DEBUG_MK3MAG; |
| } if (!PWMTimeout) { |
| } |
| if (!PWMTimeout) { |
| if (CheckDelay(BeepDelay)) { |
| if (!BeepTime) |
| BeepTime = 100; // make noise with 10Hz to signal the compass problem |
| BeepDelay = SetDelay(100); |
| DebugOut.Digital[1] |= DEBUG_MK3MAG; |
| } |
| } |
| } |
| /branches/dongfang_FC_rewrite/output.c |
|---|
| 59,7 → 59,7 |
| void output_init(void) { |
| // set PC2 & PC3 as output (control of J16 & J17) |
| DDRC |= (1<<DDC2)|(1<<DDC3); |
| OUTPUT_OFF(0); OUTPUT_OFF(1); |
| OUTPUT_SET(0,0); OUTPUT_SET(1,0); |
| flashCnt[0] = flashCnt[1] = 0; |
| flashMask[0] = flashMask[1] = 128; |
| } |
| 94,25 → 94,12 |
| */ |
| #define DIGITAL_DEBUG_MASK DEBUG_MK3MAG |
| // invert means: An "1" bit in digital debug data will feed NO base current to output transistor. |
| #define DIGITAL_DEBUG_INVERT 0 |
| void output_update(void) { |
| uint8_t output0, output1; |
| if (!DIGITAL_DEBUG_MASK) |
| flashingLights(); |
| else { |
| if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST) { |
| // Show the state for a SET bit. If inverse, then invert. |
| output0 = output1 = ~DIGITAL_DEBUG_INVERT; |
| } else if (DIGITAL_DEBUG_INVERT) { |
| output0 = (~DebugOut.Digital[0]) & DIGITAL_DEBUG_MASK; |
| output1 = (~DebugOut.Digital[1]) & DIGITAL_DEBUG_MASK; |
| } else { |
| output0 = DebugOut.Digital[0] & DIGITAL_DEBUG_MASK; |
| output1 = DebugOut.Digital[1] & DIGITAL_DEBUG_MASK; |
| OUTPUT_SET(0, DebugOut.Digital[0] & DIGITAL_DEBUG_MASK); |
| OUTPUT_SET(1, DebugOut.Digital[1] & DIGITAL_DEBUG_MASK); |
| } |
| OUTPUT_SET(0, output0); |
| OUTPUT_SET(1, output1); |
| } |
| } |
| /branches/dongfang_FC_rewrite/output.h |
|---|
| 7,9 → 7,12 |
| // PORTbit = 0 --> LED on. |
| // To use the normal transistor set-up where 1 --> transistor conductive, reverse the |
| // ON and OFF statements. |
| #define OUTPUT_ON(num) {PORTC |= (4 << (num));} |
| #define OUTPUT_OFF(num) {PORTC &= ~(4 << (num));} |
| #define OUTPUT_SET(num, state) {if ((state)) OUTPUT_ON(num) else OUTPUT_OFF(num)} |
| // invert means: An "1" bit in digital debug data make a LOW on the output. |
| #define DIGITAL_DEBUG_INVERT 1 |
| #define OUTPUT_HIGH(num) {PORTC |= (4 << (num));} |
| #define OUTPUT_LOW(num) {PORTC &= ~(4 << (num));} |
| #define OUTPUT_SET(num, state) {if (DIGITAL_DEBUG_INVERT){if(state) OUTPUT_LOW(num) else OUTPUT_HIGH(num)} else {if(state) OUTPUT_HIGH(num) else OUTPUT_LOW(num)}} |
| #define OUTPUT_TOGGLE(num) ( {PORTC ^= (4 << (num));} |
| #define DEBUG_LEDTEST 256 |
| /branches/dongfang_FC_rewrite/spi.c |
|---|
| 292,8 → 292,8 |
| if(fromNaviCtrl.CompassHeading <= 360) { |
| compassHeading = fromNaviCtrl.CompassHeading; |
| } |
| if(compassHeading < 0) compassOffCourse = 0; |
| else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
| //if(compassHeading < 0) compassOffCourse = 0; |
| //else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
| // NaviCtrl wants to beep? |
| if (fromNaviCtrl.BeepTime > BeepTime && !compassCalState) BeepTime = fromNaviCtrl.BeepTime; |
| /branches/dongfang_FC_rewrite/timer0.c |
|---|
| 54,7 → 54,7 |
| #include "eeprom.h" |
| #include "analog.h" |
| // for degugging! |
| // for debugging! |
| #include "rc.h" |
| #ifdef USE_MK3MAG |