package org.droidplanner.services.android.impl.core.drone.autopilot.apm.solo.sololink;
import android.content.Context;
import android.os.Handler;
import android.text.TextUtils;
import com.o3dr.android.client.BuildConfig;
import com.o3dr.android.client.utils.connection.TcpConnection;
import com.o3dr.android.client.utils.connection.UdpConnection;
import com.o3dr.services.android.lib.drone.companion.solo.button.ButtonTypes;
import com.o3dr.services.android.lib.drone.companion.solo.tlv.SoloButtonSetting;
import com.o3dr.services.android.lib.drone.companion.solo.tlv.SoloButtonSettingGetter;
import com.o3dr.services.android.lib.drone.companion.solo.tlv.SoloButtonSettingSetter;
import com.o3dr.services.android.lib.drone.companion.solo.tlv.SoloGoproRequestState;
import com.o3dr.services.android.lib.drone.companion.solo.tlv.SoloMessageShotManagerError;
import com.o3dr.services.android.lib.drone.companion.solo.tlv.TLVMessageParser;
import com.o3dr.services.android.lib.drone.companion.solo.tlv.TLVMessageTypes;
import com.o3dr.services.android.lib.drone.companion.solo.tlv.TLVPacket;
import com.o3dr.services.android.lib.model.ICommandListener;
import com.o3dr.services.android.lib.model.SimpleCommandListener;
import org.droidplanner.services.android.impl.communication.model.DataLink;
import org.droidplanner.services.android.impl.core.drone.autopilot.apm.solo.AbstractLinkManager;
import org.droidplanner.services.android.impl.core.drone.autopilot.apm.solo.SoloComp;
import org.droidplanner.services.android.impl.core.drone.autopilot.apm.solo.controller.ControllerLinkManager;
import org.droidplanner.services.android.impl.utils.connection.SshConnection;
import java.io.IOException;
import java.net.UnknownHostException;
import java.nio.ByteBuffer;
import java.util.List;
import java.util.Locale;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.atomic.AtomicReference;
import timber.log.Timber;
/**
* Handles solo link related logic.
*/
public class SoloLinkManager extends AbstractLinkManager<SoloLinkListener> {
public static final String SOLO_LINK_IP = "10.1.1.10";
public static final int SOLO_LINK_TCP_PORT = 5507;
private static final int SHOT_FOLLOW_UDP_PORT = 14558;
private static final String SOLO_VERSION_FILENAME = "/VERSION";
private static final String PIXHAWK_VERSION_FILENAME = "/PIX_VERSION";
private static final String GIMBAL_VERSION_FILENAME = "/AXON_VERSION";
private final UdpConnection followDataConn;
private final SshConnection sshLink;
private final SoloButtonSettingGetter presetButtonAGetter = new SoloButtonSettingGetter(ButtonTypes.BUTTON_A,
ButtonTypes.BUTTON_EVENT_PRESS);
private final SoloButtonSettingGetter presetButtonBGetter = new SoloButtonSettingGetter(ButtonTypes.BUTTON_B,
ButtonTypes.BUTTON_EVENT_PRESS);
private final SoloGoproRequestState goproStateGetter = new SoloGoproRequestState();
private final AtomicReference<SoloButtonSetting> loadedPresetButtonA = new AtomicReference<>();
private final AtomicReference<SoloButtonSetting> loadedPresetButtonB = new AtomicReference<>();
private final AtomicReference<String> vehicleVersion = new AtomicReference<>("");
private final AtomicReference<String> pixhawkVersion = new AtomicReference<>("");
private final AtomicReference<String> gimbalVersion = new AtomicReference<>("");
private final Runnable soloLinkVersionRetriever = new Runnable() {
@Override
public void run() {
final String version = retrieveVersion(SOLO_VERSION_FILENAME);
if (version != null) {
vehicleVersion.set(version);
}
if(linkListener != null && areVersionsSet())
linkListener.onVersionsUpdated();
}
};
private final Runnable pixhawkVersionRetriever = new Runnable() {
@Override
public void run() {
final String version = retrieveVersion(PIXHAWK_VERSION_FILENAME);
if (version != null) {
pixhawkVersion.set(version);
}
if(linkListener != null && areVersionsSet())
linkListener.onVersionsUpdated();
}
};
private final Runnable gimbalVersionRetriever = new Runnable() {
@Override
public void run() {
final String version = retrieveVersion(GIMBAL_VERSION_FILENAME);
if(version != null)
gimbalVersion.set(version);
if(linkListener != null && areVersionsSet())
linkListener.onVersionsUpdated();
}
};
private SoloLinkListener linkListener;
public SoloLinkManager(Context context, Handler handler, ExecutorService asyncExecutor,
DataLink.DataLinkProvider mavClient) {
super(context, new TcpConnection(handler, getSoloLinkIp(), SOLO_LINK_TCP_PORT), handler,
asyncExecutor,
mavClient);
sshLink = new SshConnection(getSoloLinkIp(), SoloComp.SSH_USERNAME, SoloComp.SSH_PASSWORD, mavClient);
UdpConnection dataConn = null;
try {
dataConn = new UdpConnection(handler, getSoloLinkIp(), SHOT_FOLLOW_UDP_PORT, 14557);
} catch (UnknownHostException e) {
Timber.e(e, "Error while creating follow udp connection.");
}
followDataConn = dataConn;
}
public static String getSoloLinkIp() {
if (!BuildConfig.SITL_DEBUG) {
return SOLO_LINK_IP;
} else {
return BuildConfig.SOLO_LINK_IP;
}
}
public String getVehicleVersion() {
return vehicleVersion.get();
}
public String getPixhawkVersion() {
return pixhawkVersion.get();
}
public String getGimbalVersion(){
return gimbalVersion.get();
}
public boolean areVersionsSet(){
return !TextUtils.isEmpty(vehicleVersion.get()) && !TextUtils.isEmpty(pixhawkVersion.get());
}
@Override
public void start(SoloLinkListener listener) {
if(!isStarted()) {
Timber.i("Starting solo link manager");
}
super.start(listener);
this.linkListener = listener;
}
@Override
public void stop() {
if(isStarted()) {
Timber.i("Stopping solo link manager");
}
super.stop();
}
@Override
public void refreshState(){
Timber.d("Connected to sololink.");
//Load the mac address for the vehicle.
loadMacAddress();
loadPresetButtonSettings();
loadGoproState();
refreshSoloLinkVersions();
}
@Override
protected SshConnection getSshLink() {
return sshLink;
}
@Override
public void onIpDisconnected() {
Timber.d("Disconnected from sololink.");
super.onIpDisconnected();
}
@Override
public void onPacketReceived(ByteBuffer packetBuffer) {
final List<TLVPacket> tlvMsgs = TLVMessageParser.parseTLVPacket(packetBuffer);
if (tlvMsgs.isEmpty()) {
return;
}
for(TLVPacket tlvMsg : tlvMsgs) {
final int messageType = tlvMsg.getMessageType();
Timber.d("Received tlv message: " + messageType);
//Have shot manager examine the received message first.
switch (messageType) {
case TLVMessageTypes.TYPE_SOLO_MESSAGE_SHOT_MANAGER_ERROR:
Timber.w(((SoloMessageShotManagerError) tlvMsg).getExceptionInfo());
break;
case TLVMessageTypes.TYPE_SOLO_GET_BUTTON_SETTING:
final SoloButtonSettingGetter receivedPresetButton = (SoloButtonSettingGetter) tlvMsg;
handleReceivedPresetButton(receivedPresetButton);
break;
}
if (linkListener != null) {
linkListener.onTlvPacketReceived(tlvMsg);
}
}
}
private void sendPacket(byte[] payload, int payloadSize, ICommandListener listener) {
linkConn.sendPacket(payload, payloadSize, listener);
}
private void sendFollowPacket(byte[] payload, int payloadSize, ICommandListener listener) {
if (followDataConn == null) {
throw new IllegalStateException("Unable to send follow data.");
}
followDataConn.sendPacket(payload, payloadSize, listener);
}
public void sendTLVPacket(TLVPacket packet, ICommandListener listener) {
sendTLVPacket(packet, false, listener);
}
public void sendTLVPacket(TLVPacket packet, boolean useFollowLink, ICommandListener listener) {
if (packet == null) {
return;
}
final byte[] messagePayload = packet.toBytes();
if (useFollowLink) {
sendFollowPacket(messagePayload, messagePayload.length, listener);
} else {
sendPacket(messagePayload, messagePayload.length, listener);
}
}
public void loadPresetButtonSettings() {
sendTLVPacket(presetButtonAGetter, new SimpleCommandListener() {
@Override
public void onSuccess() {
sendTLVPacket(presetButtonBGetter, null);
}
});
}
private void loadGoproState() {
sendTLVPacket(goproStateGetter, null);
}
private void handleReceivedPresetButton(SoloButtonSetting presetButton) {
final int buttonType = presetButton.getButton();
switch (buttonType) {
case ButtonTypes.BUTTON_A:
loadedPresetButtonA.set(presetButton);
if (linkListener != null) {
linkListener.onPresetButtonLoaded(buttonType, presetButton);
}
break;
case ButtonTypes.BUTTON_B:
loadedPresetButtonB.set(presetButton);
if (linkListener != null) {
linkListener.onPresetButtonLoaded(buttonType, presetButton);
}
break;
}
}
public SoloButtonSetting getLoadedPresetButton(int buttonType) {
switch (buttonType) {
case ButtonTypes.BUTTON_A:
return loadedPresetButtonA.get();
case ButtonTypes.BUTTON_B:
return loadedPresetButtonB.get();
default:
return null;
}
}
/**
* Update the vehicle preset button settings
*/
public void pushPresetButtonSettings(final SoloButtonSettingSetter buttonSetter, final ICommandListener listener) {
if (!isLinkConnected() || buttonSetter == null) {
return;
}
sendTLVPacket(buttonSetter, new SimpleCommandListener() {
@Override
public void onSuccess() {
postSuccessEvent(listener);
handleReceivedPresetButton(buttonSetter);
}
@Override
public void onTimeout() {
postTimeoutEvent(listener);
}
});
}
public void disableFollowDataConnection() {
if (followDataConn != null) {
followDataConn.disconnect();
}
}
public void enableFollowDataConnection() {
if (followDataConn != null) {
followDataConn.connect(linkProvider.getConnectionExtras());
}
}
public boolean updateSololinkWifi(CharSequence wifiSsid, CharSequence password) {
Timber.d(String.format(Locale.US, "Updating solo wifi ssid to %s with password %s", wifiSsid, password));
try {
String ssidUpdateResult = sshLink.execute(ControllerLinkManager.SOLOLINK_SSID_CONFIG_PATH + " --set-wifi-ssid " +
wifiSsid);
String passwordUpdateResult = sshLink.execute(ControllerLinkManager.SOLOLINK_SSID_CONFIG_PATH + " --set-wifi-password " +
password);
String restartResult = sshLink.execute(ControllerLinkManager.SOLOLINK_SSID_CONFIG_PATH + " --reboot");
return true;
} catch (IOException e) {
Timber.e(e, "Error occurred while updating the sololink wifi ssid.");
return false;
}
}
private void updateSoloLinkVersion() {
postAsyncTask(soloLinkVersionRetriever);
}
private void updatePixhawkVersion() {
postAsyncTask(pixhawkVersionRetriever);
}
private void updateGimbalVersion(){
postAsyncTask(gimbalVersionRetriever);
}
private String retrieveVersion(String versionFile) {
try {
String version = sshLink.execute("cat " + versionFile);
if (TextUtils.isEmpty(version)) {
Timber.d("No version file was found");
return "";
} else {
return version.split("\n")[0];
}
} catch (IOException e) {
Timber.e("Unable to retrieve the current version.", e);
}
return null;
}
/**
* Refresh the vehicle's components versions
*/
public void refreshSoloLinkVersions() {
updateSoloLinkVersion();
updatePixhawkVersion();
updateGimbalVersion();
}
}