飞控问题
已完成
通过DjiFlightController_ExecuteJoystickAction控制无人机飞行，比如向东北方向移动50m，为什么飞机会先向东飞一段距离，再向北飞，而不是直接飞过去？
-
int controlFreqInHz = 50; // Hz
int cycleTimeInMs = 1000 / controlFreqInHz;
int outOfControlBoundsTimeLimit = 10 * cycleTimeInMs; // 10 cycles
int withinControlBoundsTimeReqmt = 100 * cycleTimeInMs; // 100 cycles
int elapsedTimeInMs = 0;
int withinBoundsCounter = 0;
int outOfBounds = 0;
int brakeCounter = 0;
T_DjiFcSubscriptionPositionFused originGPSPosition;
T_DjiFcSubscriptionQuaternion originalQuaternion;
dji_f32_t originHeightBaseHomePoint;
float originalyawInDeg = 0.0;
T_DjiFcSubscriptionPositionFused currentGPSPosition;
T_DjiFcSubscriptionQuaternion currentQuaternion;
dji_f32_t currentHeight;
T_DjiFlightControllerJoystickMode joystickMode = {
DJI_FLIGHT_CONTROLLER_HORIZONTAL_POSITION_CONTROL_MODE,
DJI_FLIGHT_CONTROLLER_VERTICAL_POSITION_CONTROL_MODE,
DJI_FLIGHT_CONTROLLER_YAW_ANGLE_CONTROL_MODE,
DJI_FLIGHT_CONTROLLER_HORIZONTAL_GROUND_COORDINATE,
DJI_FLIGHT_CONTROLLER_STABLE_CONTROL_MODE_ENABLE,
};
returnCode = DjiFlightController_ObtainJoystickCtrlAuthority();
//! get origin position and relative height(from home point)of aircraft.
originGPSPosition = DjiTest_FlightControlGetValueOfPositionFused();
originHeightBaseHomePoint = DjiTest_FlightControlGetValueOfRelativeHeight();
originalQuaternion = DjiTest_FlightControlGetValueOfQuaternion();
originalyawInDeg = DjiTest_FlightControlQuaternionToEulerAngle(originalQuaternion).z / s_degToRad;
offsetDesired.x = droneOffset.x * cos(originalyawInDeg * s_degToRad) -
droneOffset.y * sin(originalyawInDeg * s_degToRad);
offsetDesired.y = droneOffset.y * cos(originalyawInDeg * s_degToRad) +
droneOffset.x * sin(originalyawInDeg * s_degToRad);
offsetDesired.z = droneOffset.z;
if (keepYaw) {
yawDesiredInDeg = originalyawInDeg;
}
if(originHeightBaseHomePoint==-1)
{
return;
}
DjiFlightController_SetJoystickMode(joystickMode);
while (elapsedTimeInMs < timeoutInMilSec) {
currentGPSPosition = DjiTest_FlightControlGetValueOfPositionFused();
currentQuaternion = DjiTest_FlightControlGetValueOfQuaternion();
currentHeight = DjiTest_FlightControlGetValueOfRelativeHeight();
if(originHeightBaseHomePoint==-1)
{
return;
}
float yawInRad = DjiTest_FlightControlQuaternionToEulerAngle(currentQuaternion).z;
//! get the vector between aircraft and origin point.
T_DjiTestFlightControlVector3f localOffset = DjiTest_FlightControlLocalOffsetFromGpsAndFusedHeightOffset(
currentGPSPosition,
originGPSPosition,
currentHeight,
originHeightBaseHomePoint);
//! get the vector between aircraft and target point.
T_DjiTestFlightControlVector3f offsetRemaining = DjiTest_FlightControlVector3FSub(offsetDesired, localOffset);
T_DjiTestFlightControlVector3f positionCommand = offsetRemaining;
DjiTest_FlightControlHorizCommandLimit(speedFactor, &positionCommand.x, &positionCommand.y);
T_DjiFlightControllerJoystickCommand joystickCommand = {positionCommand.x, positionCommand.y,
offsetDesired.z + originHeightBaseHomePoint,
yawDesiredInDeg};
DjiFlightController_ExecuteJoystickAction(joystickCommand);
if (DjiTest_FlightControlVectorNorm(offsetRemaining) < posThresholdInM &&
fabs(yawInRad / s_degToRad - yawDesiredInDeg) < yawThresholdInDeg) {
//! 1. We are within bounds; start incrementing our in-bound counter
withinBoundsCounter += cycleTimeInMs;
} else {
if (withinBoundsCounter != 0) {
//! 2. Start incrementing an out-of-bounds counter
outOfBounds += cycleTimeInMs;
}
}
//! 3. Reset withinBoundsCounter if necessary
if (outOfBounds > outOfControlBoundsTimeLimit) {
withinBoundsCounter = 0;
outOfBounds = 0;
}
//! 4. If within bounds, set flag and break
if (withinBoundsCounter >= withinControlBoundsTimeReqmt) {
break;
}
osalHandler->TaskSleepMs(cycleTimeInMs);
elapsedTimeInMs += cycleTimeInMs;
}
while (brakeCounter < withinControlBoundsTimeReqmt) {
osalHandler->TaskSleepMs(cycleTimeInMs);
brakeCounter += cycleTimeInMs;
}
-
感谢，我看不到你的邮箱，可以发我邮箱嘛，xmyyzy123@qq.com
请先登录再写评论。
评论
9 条评论