Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 00:05:28 +03:00
Code Issues Wiki Activity

do fast operations

Browse source
This commit is contained in:
JuliooCesarMDM 2021-06-05 22:24:49 -03:00
parent 2fcb1cb83d
commit 79db68dc50
2 changed files with 6 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/main/navigation/navigation.c
View file

@ -1041,7 +1041,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime; timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime;
if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference. if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate)); float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
float centidegsPerIteration = rateTarget * timeDifference / 1000.0f; float centidegsPerIteration = rateTarget * timeDifference * 0.001f;
posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration); posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw)); DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
posControl.cruise.lastYawAdjustmentTime = currentTimeMs; posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
@ -2144,8 +2144,8 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp
static void updateHomePositionCompatibility(void) static void updateHomePositionCompatibility(void)
{ {
geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos); geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos);
GPS_distanceToHome = posControl.homeDistance / 100; GPS_distanceToHome = posControl.homeDistance * 0.01f;
GPS_directionToHome = posControl.homeDirection / 100; GPS_directionToHome = posControl.homeDirection * 0.01f;
} }
// Backdoor for RTH estimator // Backdoor for RTH estimator

6
src/main/navigation/navigation_fixedwing.c
View file

@ -132,10 +132,10 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
// velocity error. We use PID controller on altitude error and calculate desired pitch angle // velocity error. We use PID controller on altitude error and calculate desired pitch angle
// Update energies // Update energies
const float demSPE = (posControl.desiredState.pos.z / 100.0f) * GRAVITY_MSS; const float demSPE = (posControl.desiredState.pos.z * 0.01f) * GRAVITY_MSS;
const float demSKE = 0.0f; const float demSKE = 0.0f;
const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z / 100.0f) * GRAVITY_MSS; const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z * 0.01f) * GRAVITY_MSS;
const float estSKE = 0.0f; const float estSKE = 0.0f;
// speedWeight controls balance between potential and kinetic energy used for pitch controller // speedWeight controls balance between potential and kinetic energy used for pitch controller
@ -316,7 +316,7 @@ float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingErr
-limit, -limit,
limit, limit,
yawPidFlags yawPidFlags
) / 100.0f; ) * 0.01f;
DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional); DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral); DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);