public static void sendMavlinkMessage(MavLinkDrone drone, MavlinkMessageWrapper messageWrapper) {
if (drone == null || messageWrapper == null) return;
MAVLinkMessage message = messageWrapper.getMavLinkMessage();
if (message == null) return;
message.compid = drone.getCompid();
message.sysid = drone.getSysid();
// Set the target system and target component for MAVLink messages that support those
// attributes.
try {
Class<?> tempMessage = message.getClass();
Field target_system = tempMessage.getDeclaredField("target_system");
Field target_component = tempMessage.getDeclaredField("target_component");
target_system.setByte(message, (byte) message.sysid);
target_component.setByte(message, (byte) message.compid);
} catch (NoSuchFieldException
| SecurityException
| IllegalAccessException
| IllegalArgumentException
| ExceptionInInitializerError e) {
Timber.e(e, e.getMessage());
}
drone.getMavClient().sendMavMessage(message, null);
}
public static CameraProxy getCameraProxy(MavLinkDrone drone, List<CameraDetail> cameraDetails) {
final CameraDetail camDetail;
final FootPrint currentFieldOfView;
final List<FootPrint> proxyPrints = new ArrayList<>();
if (drone == null) {
camDetail = new CameraDetail();
currentFieldOfView = new FootPrint();
} else {
Camera droneCamera = drone.getCamera();
camDetail = ProxyUtils.getCameraDetail(droneCamera.getCamera());
List<Footprint> footprints = droneCamera.getFootprints();
for (Footprint footprint : footprints) {
proxyPrints.add(CommonApiUtils.getProxyCameraFootPrint(footprint));
}
Gps droneGps = (Gps) drone.getAttribute(AttributeType.GPS);
currentFieldOfView =
droneGps != null && droneGps.isValid()
? CommonApiUtils.getProxyCameraFootPrint(droneCamera.getCurrentFieldOfView())
: new FootPrint();
}
return new CameraProxy(camDetail, currentFieldOfView, proxyPrints, cameraDetails);
}
public static void changeToGuidedMode(MavLinkDrone drone, ICommandListener listener) {
final State droneState = drone.getState();
final int droneType = drone.getType();
if (Type.isCopter(droneType)) {
droneState.changeFlightMode(ApmModes.ROTOR_GUIDED, listener);
} else if (Type.isPlane(droneType)) {
// You have to send a guided point to the plane in order to trigger guided mode.
forceSendGuidedPoint(drone, getGpsPosition(drone), getDroneAltConstrained(drone));
} else if (Type.isRover(droneType)) {
droneState.changeFlightMode(ApmModes.ROVER_GUIDED, listener);
}
}
public static FollowAlgorithm.FollowModes followTypeToMode(
MavLinkDrone drone, FollowType followType) {
final FollowAlgorithm.FollowModes followMode;
switch (followType) {
case ABOVE:
followMode =
(drone.getFirmwareType() == FirmwareType.ARDU_SOLO)
? FollowAlgorithm.FollowModes.SPLINE_ABOVE
: FollowAlgorithm.FollowModes.ABOVE;
break;
case LEAD:
followMode = FollowAlgorithm.FollowModes.LEAD;
break;
default:
case LEASH:
followMode =
(drone.getFirmwareType() == FirmwareType.ARDU_SOLO)
? FollowAlgorithm.FollowModes.SPLINE_LEASH
: FollowAlgorithm.FollowModes.LEASH;
break;
case CIRCLE:
followMode = FollowAlgorithm.FollowModes.CIRCLE;
break;
case LEFT:
followMode = FollowAlgorithm.FollowModes.LEFT;
break;
case RIGHT:
followMode = FollowAlgorithm.FollowModes.RIGHT;
break;
case GUIDED_SCAN:
followMode = FollowAlgorithm.FollowModes.GUIDED_SCAN;
break;
case LOOK_AT_ME:
followMode = FollowAlgorithm.FollowModes.LOOK_AT_ME;
break;
case SOLO_SHOT:
followMode = FollowAlgorithm.FollowModes.SOLO_SHOT;
break;
}
return followMode;
}
public static GuidedState getGuidedState(MavLinkDrone drone) {
if (drone == null) return new GuidedState();
final GuidedPoint guidedPoint = drone.getGuidedPoint();
int guidedState;
switch (guidedPoint.getState()) {
default:
case UNINITIALIZED:
guidedState = GuidedState.STATE_UNINITIALIZED;
break;
case ACTIVE:
guidedState = GuidedState.STATE_ACTIVE;
break;
case IDLE:
guidedState = GuidedState.STATE_IDLE;
break;
}
LatLong guidedCoord = guidedPoint.getCoord();
if (guidedCoord == null) {
guidedCoord = new LatLong(0, 0);
}
double guidedAlt = guidedPoint.getAltitude();
return new GuidedState(guidedState, new LatLongAlt(guidedCoord, guidedAlt));
}
public static Survey buildSurvey(MavLinkDrone drone, Survey survey) {
org.droidplanner.services.android.core.mission.Mission droneMission =
drone == null ? null : drone.getMission();
SurveyImpl updatedSurveyImpl = (SurveyImpl) ProxyUtils.getMissionItemImpl(droneMission, survey);
return (Survey) ProxyUtils.getProxyMissionItem(updatedSurveyImpl);
}
/**
* Check if the kill switch feature is supported on the given drone
*
* @param drone
* @return true if it's supported, false otherwise.
*/
public static boolean isKillSwitchSupported(MavLinkDrone drone) {
if (drone == null) return false;
if (!org.droidplanner.services.android.core.drone.variables.Type.isCopter(drone.getType()))
return false;
final String firmwareVersion = drone.getFirmwareVersion();
if (TextUtils.isEmpty(firmwareVersion)) return false;
if (!firmwareVersion.startsWith("APM:Copter V3.3")
&& !firmwareVersion.startsWith("APM:Copter V3.4")
&& !firmwareVersion.startsWith("Solo")) {
return false;
}
return true;
}
public static void startMagnetometerCalibration(
MavLinkDrone drone, boolean retryOnFailure, boolean saveAutomatically, int startDelay) {
if (drone == null) return;
drone
.getMagnetometerCalibration()
.startCalibration(retryOnFailure, saveAutomatically, startDelay);
}
public static Mission getMission(MavLinkDrone drone) {
Mission proxyMission = new Mission();
if (drone == null) return proxyMission;
org.droidplanner.services.android.core.mission.Mission droneMission = drone.getMission();
List<org.droidplanner.services.android.core.mission.MissionItem> droneMissionItems =
droneMission.getComponentItems();
proxyMission.setCurrentMissionItem((short) drone.getMissionStats().getCurrentWP());
if (!droneMissionItems.isEmpty()) {
for (org.droidplanner.services.android.core.mission.MissionItem item : droneMissionItems) {
proxyMission.addMissionItem(ProxyUtils.getProxyMissionItem(item));
}
}
return proxyMission;
}
public static void forceSendGuidedPoint(
MavLinkDrone drone, LatLong coord, double altitudeInMeters) {
drone.notifyDroneEvent(DroneEventsType.GUIDEDPOINT);
if (coord != null) {
MavLinkCommands.setGuidedMode(
drone, coord.getLatitude(), coord.getLongitude(), altitudeInMeters);
}
}
public static StructureScanner buildStructureScanner(MavLinkDrone drone, StructureScanner item) {
org.droidplanner.services.android.core.mission.Mission droneMission =
drone == null ? null : drone.getMission();
StructureScannerImpl updatedScan =
(StructureScannerImpl) ProxyUtils.getMissionItemImpl(droneMission, item);
StructureScanner proxyScanner = (StructureScanner) ProxyUtils.getProxyMissionItem(updatedScan);
return proxyScanner;
}
public static float getDefaultMinAltitude(MavLinkDrone drone) {
final int droneType = drone.getType();
if (Type.isCopter(droneType)) {
return 2f;
} else if (Type.isPlane(droneType)) {
return 15f;
} else {
return 0f;
}
}
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));
}
public static boolean isGuidedMode(MavLinkDrone drone) {
if (drone == null) return false;
final int droneType = drone.getType();
final ApmModes droneMode = drone.getState().getMode();
if (Type.isCopter(droneType)) {
return droneMode == ApmModes.ROTOR_GUIDED;
}
if (Type.isPlane(droneType)) {
return droneMode == ApmModes.FIXED_WING_GUIDED;
}
if (Type.isRover(droneType)) {
return droneMode == ApmModes.ROVER_GUIDED || droneMode == ApmModes.ROVER_HOLD;
}
return false;
}
public static void forceSendGuidedPointAndVelocity(
MavLinkDrone drone,
LatLong coord,
double altitudeInMeters,
double xVel,
double yVel,
double zVel) {
drone.notifyDroneEvent(DroneEventsType.GUIDEDPOINT);
if (coord != null) {
MavLinkCommands.sendGuidedPositionAndVelocity(
drone, coord.getLatitude(), coord.getLongitude(), altitudeInMeters, xVel, yVel, zVel);
}
}
public static void setMission(MavLinkDrone drone, Mission mission, boolean pushToDrone) {
if (drone == null) return;
org.droidplanner.services.android.core.mission.Mission droneMission = drone.getMission();
droneMission.clearMissionItems();
List<MissionItem> itemsList = mission.getMissionItems();
for (MissionItem item : itemsList) {
droneMission.addMissionItem(ProxyUtils.getMissionItemImpl(droneMission, item));
}
if (pushToDrone) droneMission.sendMissionToAPM();
}
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 void arm(
final MavLinkDrone drone,
final boolean arm,
final boolean emergencyDisarm,
final ICommandListener listener) {
if (drone == null) return;
if (!arm && emergencyDisarm) {
if (org.droidplanner.services.android.core.drone.variables.Type.isCopter(drone.getType())
&& !isKillSwitchSupported(drone)) {
changeVehicleMode(
drone,
VehicleMode.COPTER_STABILIZE,
new AbstractCommandListener() {
@Override
public void onSuccess() {
MavLinkArm.sendArmMessage(drone, arm, emergencyDisarm, listener);
}
@Override
public void onError(int executionError) {
if (listener != null) {
try {
listener.onError(executionError);
} catch (RemoteException e) {
Timber.e(e, e.getMessage());
}
}
}
@Override
public void onTimeout() {
if (listener != null) {
try {
listener.onTimeout();
} catch (RemoteException e) {
Timber.e(e, e.getMessage());
}
}
}
});
return;
}
}
MavLinkArm.sendArmMessage(drone, arm, emergencyDisarm, listener);
}
public static void sendGuidedPoint(
MavLinkDrone drone, LatLong point, boolean force, ICommandListener listener) {
if (drone == null) return;
GuidedPoint guidedPoint = drone.getGuidedPoint();
if (guidedPoint.isInitialized()) {
guidedPoint.newGuidedCoord(point);
} else if (force) {
try {
guidedPoint.forcedGuidedCoordinate(point, listener);
} catch (Exception e) {
Timber.e(e, e.getMessage());
}
}
}
public static State getState(MavLinkDrone drone, boolean isConnected, Vibration vibration) {
if (drone == null) return new State();
org.droidplanner.services.android.core.drone.variables.State droneState = drone.getState();
ApmModes droneMode = droneState.getMode();
AccelCalibration accelCalibration = drone.getCalibrationSetup();
String calibrationMessage =
accelCalibration != null && accelCalibration.isCalibrating()
? accelCalibration.getMessage()
: null;
final msg_ekf_status_report ekfStatus = droneState.getEkfStatus();
final EkfStatus proxyEkfStatus =
ekfStatus == null
? new EkfStatus()
: new EkfStatus(
ekfStatus.flags,
ekfStatus.compass_variance,
ekfStatus.pos_horiz_variance,
ekfStatus.terrain_alt_variance,
ekfStatus.velocity_variance,
ekfStatus.pos_vert_variance);
return new State(
isConnected,
CommonApiUtils.getVehicleMode(droneMode),
droneState.isArmed(),
droneState.isFlying(),
droneState.getErrorId(),
drone.getMavlinkVersion(),
calibrationMessage,
droneState.getFlightStartTime(),
proxyEkfStatus,
isConnected && drone.isConnectionAlive(),
vibration);
}
public static MagnetometerCalibrationStatus getMagnetometerCalibrationStatus(MavLinkDrone drone) {
final MagnetometerCalibrationStatus calStatus = new MagnetometerCalibrationStatus();
if (drone != null) {
final MagnetometerCalibrationImpl magCalImpl = drone.getMagnetometerCalibration();
calStatus.setCalibrationCancelled(magCalImpl.isCancelled());
Collection<MagnetometerCalibrationImpl.Info> calibrationInfo =
magCalImpl.getMagCalibrationTracker().values();
for (MagnetometerCalibrationImpl.Info info : calibrationInfo) {
calStatus.addCalibrationProgress(getMagnetometerCalibrationProgress(info.getCalProgress()));
calStatus.addCalibrationResult(getMagnetometerCalibrationResult(info.getCalReport()));
}
}
return calStatus;
}
public static void changeVehicleMode(
MavLinkDrone drone, VehicleMode newMode, ICommandListener listener) {
if (drone == null) return;
int mavType;
switch (newMode.getDroneType()) {
default:
case Type.TYPE_COPTER:
mavType = MAV_TYPE.MAV_TYPE_QUADROTOR;
break;
case Type.TYPE_PLANE:
mavType = MAV_TYPE.MAV_TYPE_FIXED_WING;
break;
case Type.TYPE_ROVER:
mavType = MAV_TYPE.MAV_TYPE_GROUND_ROVER;
break;
}
drone.getState().changeFlightMode(ApmModes.getMode(newMode.getMode(), mavType), listener);
}
public Type(MavLinkDrone myDrone) {
super(myDrone);
myDrone.addDroneListener(this);
}
public static void setGuidedAltitude(MavLinkDrone drone, double altitude) {
if (drone == null) return;
drone.getGuidedPoint().changeGuidedAltitude(altitude);
}
public static void doGuidedTakeoff(
MavLinkDrone drone, double altitude, ICommandListener listener) {
if (drone == null) return;
drone.getGuidedPoint().doGuidedTakeoff(altitude, listener);
}
public static void sendIMUCalibrationAck(MavLinkDrone drone, int step) {
if (drone == null) return;
drone.getCalibrationSetup().sendAck(step);
}
public static void startIMUCalibration(MavLinkDrone drone, ICommandListener listener) {
if (drone != null) drone.getCalibrationSetup().startCalibration(listener);
}
public static void acceptMagnetometerCalibration(MavLinkDrone drone) {
if (drone == null) return;
drone.getMagnetometerCalibration().acceptCalibration();
}
public static void cancelMagnetometerCalibration(MavLinkDrone drone) {
if (drone == null) return;
drone.getMagnetometerCalibration().cancelCalibration();
}
public static float generateDronie(MavLinkDrone drone) {
if (drone == null) return -1;
return (float) drone.getMission().makeAndUploadDronie();
}
