public double getSpeedParameter() {
Parameters params = getAttribute(AttributeType.PARAMETERS);
if (params != null) {
Parameter speedParam = params.getParameter("WPNAV_SPEED");
if (speedParam != null) return speedParam.getValue();
}
return 0;
}
public static void writeParameters(MavLinkDrone drone, Parameters parameters) {
if (drone == null || parameters == null) return;
List<Parameter> parametersList = parameters.getParameters();
if (parametersList.isEmpty()) return;
org.droidplanner.services.android.core.drone.profiles.Parameters droneParams =
drone.getParameters();
for (Parameter proxyParam : parametersList) {
droneParams.sendParameter(
new org.droidplanner.services.android.core.parameters.Parameter(
proxyParam.getName(), proxyParam.getValue(), proxyParam.getType()));
}
}
public static Parameters getParameters(
MavLinkDrone drone, Context context, Map<String, Parameter> cachedParameters) {
if (drone == null) return new Parameters();
final Map<String, Parameter> incompleteParams = new HashMap<>();
final List<Parameter> parametersList = new ArrayList<>();
Map<String, org.droidplanner.services.android.core.parameters.Parameter> droneParameters =
drone.getParameters().getParameters();
if (!droneParameters.isEmpty()) {
for (org.droidplanner.services.android.core.parameters.Parameter param :
droneParameters.values()) {
if (param.name != null) {
Parameter cachedParam = cachedParameters.get(param.name);
if (cachedParam == null) {
cachedParam = new Parameter(param.name, param.value, param.type);
incompleteParams.put(param.name, cachedParam);
cachedParameters.put(param.name, cachedParam);
} else {
cachedParam.setValue(param.value);
cachedParam.setType(param.type);
}
parametersList.add(cachedParam);
}
}
final VehicleProfile profile = drone.getVehicleProfile();
if (!incompleteParams.isEmpty() && profile != null) {
try {
String metadataType = profile.getParameterMetadataType();
if (metadataType != null) {
ParameterMetadataLoader.load(context, metadataType, incompleteParams);
}
} catch (IOException | XmlPullParserException e) {
Timber.e(e, e.getMessage());
}
}
}
return new Parameters(new ArrayList<>(parametersList));
}
@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;
}
