@Override protected boolean setVelocity(Bundle data, ICommandListener listener) { // Retrieve the normalized values float normalizedXVel = data.getFloat(ControlActions.EXTRA_VELOCITY_X); float normalizedYVel = data.getFloat(ControlActions.EXTRA_VELOCITY_Y); float normalizedZVel = data.getFloat(ControlActions.EXTRA_VELOCITY_Z); double attitudeInRad = Math.toRadians(attitude.getYaw()); final double cosAttitude = Math.cos(attitudeInRad); final double sinAttitude = Math.sin(attitudeInRad); float projectedX = (float) (normalizedXVel * cosAttitude) - (float) (normalizedYVel * sinAttitude); float projectedY = (float) (normalizedXVel * sinAttitude) + (float) (normalizedYVel * cosAttitude); // Retrieve the speed parameters. float defaultSpeed = 5; // m/s ParameterManager parameterManager = getParameterManager(); // Retrieve the horizontal speed value Parameter horizSpeedParam = parameterManager.getParameter("WPNAV_SPEED"); double horizontalSpeed = horizSpeedParam == null ? defaultSpeed : horizSpeedParam.getValue() / 100; // Retrieve the vertical speed value. String vertSpeedParamName = normalizedZVel >= 0 ? "WPNAV_SPEED_UP" : "WPNAV_SPEED_DN"; Parameter vertSpeedParam = parameterManager.getParameter(vertSpeedParamName); double verticalSpeed = vertSpeedParam == null ? defaultSpeed : vertSpeedParam.getValue() / 100; MavLinkCommands.setVelocityInLocalFrame( this, (float) (projectedX * horizontalSpeed), (float) (projectedY * horizontalSpeed), (float) (normalizedZVel * verticalSpeed), listener); return true; }