| /branches/dongfang_FC_rewrite/attitude.c |
|---|
| 232,10 → 232,10 |
| int16_t sinroll = sin_360(rollAngleInDegrees); |
| ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate |
| * sinroll) >> MATH_UNIT_FACTOR_LOG); |
| * sinroll) >> LOG_MATH_UNIT_FACTOR); |
| ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
| + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * tan_360(pitchAngleInDegrees)) >> MATH_UNIT_FACTOR_LOG); |
| + (int32_t)yawRate * cosroll) >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees)) >> LOG_MATH_UNIT_FACTOR); |
| ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
| /branches/dongfang_FC_rewrite/attitudeControl.c |
|---|
| 9,7 → 9,7 |
| // = cos^2(45 degs). |
| // const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2); |
| const int32_t MINPROJECTION = 1L << (MATH_UNIT_FACTOR_LOG * 2 - 9); |
| const int32_t MINPROJECTION = 1L << (LOG_MATH_UNIT_FACTOR * 2 - 9); |
| // Takes 380 - 400 usec. Way too slow. |
| // With static MINPROJECTION: 220 usec. |
| 34,7 → 34,7 |
| projection = -MINPROJECTION; |
| } else { |
| } |
| y = ((int32_t) throttle << (MATH_UNIT_FACTOR_LOG * 2 - 8)) / projection - throttle; |
| y = ((int32_t) throttle << (LOG_MATH_UNIT_FACTOR * 2 - 8)) / projection - throttle; |
| } |
| deltaThrottle = ((int32_t)y * effect) >> 6; |
| // debugOut.analog[8] = deltaThrottle; |
| /branches/dongfang_FC_rewrite/configuration.h |
|---|
| 187,6 → 187,7 |
| // Shared for both modes of navigation |
| uint8_t naviMode; |
| uint8_t naviStickThreshold; |
| uint8_t naviStickLimit; |
| uint8_t GPSMininumSatellites; |
| uint8_t naviP; |
| uint8_t naviI; |
| /branches/dongfang_FC_rewrite/directGPSNaviControl.c |
|---|
| 24,7 → 24,7 |
| } FlightMode_t; |
| #define GPS_POSINTEGRAL_LIMIT 32000 |
| #define GPS_STICK_LIMIT 400 // limit of gps stick control to avoid critical flight attitudes |
| #define LOG_NAVI_STICK_GAIN 2 |
| #define GPS_P_LIMIT 100 |
| typedef struct { |
| 166,7 → 166,7 |
| GPSPosDev_East = (GPSInfo.longitude - pTargetPos->longitude); // to calculate real cm we would need *111/100 additionally |
| // calculate latitude projection |
| GPSPosDev_East *= cos_target_latitude; |
| GPSPosDev_East >>= MATH_UNIT_FACTOR_LOG; |
| GPSPosDev_East >>= LOG_MATH_UNIT_FACTOR; |
| } else { // no valid target position available |
| // reset error |
| GPSPosDev_North = 0; |
| 233,11 → 233,11 |
| 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-2); |
| PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (MATH_UNIT_FACTOR_LOG-2); |
| PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
| PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN); |
| // limit resulting GPS control vector |
| navi_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT); |
| navi_limitXY(&PID_Nick, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN); |
| debugOut.analog[27] = -PID_Nick; |
| debugOut.analog[28] = -PID_Roll; |
| /branches/dongfang_FC_rewrite/dongfangMath.h |
|---|
| 14,8 → 14,8 |
| //#define MATH_UNIT_FACTOR 8192 |
| // Changed: We want to be able to multiply 2 sines/cosines and still stay comfortably (factor 100) within 31 bits. |
| // 4096 = 12 bits, square = 24 bits, 7 bits to spare. |
| #define MATH_UNIT_FACTOR_LOG 12 |
| #define MATH_UNIT_FACTOR (1L<<MATH_UNIT_FACTOR_LOG) |
| #define LOG_MATH_UNIT_FACTOR 12 |
| #define MATH_UNIT_FACTOR (1L<<LOG_MATH_UNIT_FACTOR) |
| int16_t sin_360(int16_t arg); |
| int16_t cos_360(int16_t arg); |