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

Merge pull request #7082 from JulioCesarMatias/STM32FastProcessingOperations

Browse source 
Change to STM32 faster processing operations
This commit is contained in:
Paweł Spychalski 2021-06-15 20:03:09 +02:00 • committed by GitHub
commit 7c96829445
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
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

@ -1035,7 +1035,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime;
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 centidegsPerIteration = rateTarget * timeDifference / 1000.0f;
float centidegsPerIteration = rateTarget * timeDifference * 0.001f;
posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
@ -2138,8 +2138,8 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp
static void updateHomePositionCompatibility(void)
{
geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos);
GPS_distanceToHome = posControl.homeDistance / 100;
GPS_directionToHome = posControl.homeDirection / 100;
GPS_distanceToHome = posControl.homeDistance * 0.01f;
GPS_directionToHome = posControl.homeDirection * 0.01f;
}
// Backdoor for RTH estimator

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

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