Fix follow roi target

ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/ArduPilot.java

@@ -82,7 +82,6 @@ public abstract class ArduPilot extends GenericMavLinkDrone {
 
     private final org.droidplanner.services.android.core.drone.variables.RC rc;
     private final Mission mission;
-    private final MissionStats missionStats;
     private final GuidedPoint guidedPoint;
     private final AccelCalibration accelCalibrationSetup;
     private final WaypointManager waypointManager;
@@ -101,7 +100,6 @@ public ArduPilot(Context context, MAVLinkStreams.MAVLinkOutputStream mavClient,
 
         rc = new RC(this);
         this.mission = new Mission(this);
-        this.missionStats = new MissionStats(this);
         this.guidedPoint = new GuidedPoint(this, handler);
         this.accelCalibrationSetup = new AccelCalibration(this, handler);
         this.magCalibration = new MagnetometerCalibrationImpl(this);
@@ -129,13 +127,6 @@ protected void setAltitudeGroundAndAirSpeeds(double altitude, double groundSpeed
         }
     }
 
-    protected void setDisttowpAndSpeedAltErrors(double disttowp, double alt_error, double aspd_error) {
-        missionStats.setDistanceToWp(disttowp);
-
-        this.altitude.setTargetAltitude(this.altitude.getAltitude() + alt_error);
-        notifyDroneEvent(DroneInterfaces.DroneEventsType.ORIENTATION);
-    }
-
     @Override
     public WaypointManager getWaypointManager() {
         return waypointManager;
@@ -146,11 +137,6 @@ public Mission getMission() {
         return mission;
     }
 
-    @Override
-    public MissionStats getMissionStats() {
-        return missionStats;
-    }
-
     @Override
     public GuidedPoint getGuidedPoint() {
         return guidedPoint;
@@ -178,9 +164,6 @@ public DroneAttribute getAttribute(String attributeType) {
                 case AttributeType.MISSION:
                     return CommonApiUtils.getMission(this);
 
-                case AttributeType.TYPE:
-                    return CommonApiUtils.getType(this);
-
                 case AttributeType.GUIDED_STATE:
                     return CommonApiUtils.getGuidedState(this);
 
@@ -439,19 +422,6 @@ public void onMavLinkMessageReceived(MAVLinkMessage message) {
                     processVfrHud((msg_vfr_hud) message);
                     break;
 
-                case msg_mission_current.MAVLINK_MSG_ID_MISSION_CURRENT:
-                    getMissionStats().setWpno(((msg_mission_current) message).seq);
-                    break;
-
-                case msg_mission_item_reached.MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
-                    getMissionStats().setLastReachedWaypointNumber(((msg_mission_item_reached) message).seq);
-                    break;
-
-                case msg_nav_controller_output.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
-                    msg_nav_controller_output m_nav = (msg_nav_controller_output) message;
-                    setDisttowpAndSpeedAltErrors(m_nav.wp_dist, m_nav.alt_error, m_nav.aspd_error);
-                    break;
-
                 case msg_raw_imu.MAVLINK_MSG_ID_RAW_IMU:
                     msg_raw_imu msg_imu = (msg_raw_imu) message;
                     mag.newData(msg_imu);

ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/solo/SoloComp.java

@@ -22,6 +22,7 @@
 import com.o3dr.services.android.lib.drone.companion.solo.tlv.SoloMessageLocation;
 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.AbstractCommandListener;
 import com.o3dr.services.android.lib.model.ICommandListener;
 
 import org.droidplanner.services.android.BuildConfig;

ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/generic/GenericMavLinkDrone.java

@@ -12,7 +12,10 @@
 import com.MAVLink.common.msg_global_position_int;
 import com.MAVLink.common.msg_gps_raw_int;
 import com.MAVLink.common.msg_heartbeat;
+import com.MAVLink.common.msg_mission_current;
 import com.MAVLink.common.msg_mission_item;
+import com.MAVLink.common.msg_mission_item_reached;
+import com.MAVLink.common.msg_nav_controller_output;
 import com.MAVLink.common.msg_radio_status;
 import com.MAVLink.common.msg_sys_status;
 import com.MAVLink.common.msg_vibration;
@@ -85,6 +88,7 @@ public class GenericMavLinkDrone implements MavLinkDrone {
     private final StreamRates streamRates;
     private final ParameterManager parameterManager;
     private final LogMessageListener logListener;
+    private final MissionStats missionStats;
 
     private final DroneInterfaces.AttributeEventListener attributeListener;
 
@@ -110,6 +114,7 @@ public GenericMavLinkDrone(Context context, Handler handler, MAVLinkStreams.MAVL
         events = new DroneEvents(this, handler);
         heartbeat = initHeartBeat(handler);
         this.type = new Type(this);
+        this.missionStats = new MissionStats(this);
         this.streamRates = new StreamRates(this);
         this.state = new State(this, handler, warningParser);
         parameterManager = new ParameterManager(this, context, handler);
@@ -121,8 +126,7 @@ public GenericMavLinkDrone(Context context, Handler handler, MAVLinkStreams.MAVL
 
     @Override
     public MissionStats getMissionStats() {
-        //TODO: complete implementation
-        return null;
+        return missionStats;
     }
 
     @Override
@@ -382,6 +386,9 @@ public DroneAttribute getAttribute(String attributeType) {
                 }
 
                 return parameters;
+
+            case AttributeType.TYPE:
+                return CommonApiUtils.getType(this);
         }
 
         return null;
@@ -441,9 +448,29 @@ public void onMavLinkMessageReceived(MAVLinkMessage message) {
             case msg_mission_item.MAVLINK_MSG_ID_MISSION_ITEM:
                 processHomeUpdate((msg_mission_item) message);
                 break;
+
+            case msg_mission_current.MAVLINK_MSG_ID_MISSION_CURRENT:
+                missionStats.setWpno(((msg_mission_current) message).seq);
+                break;
+
+            case msg_mission_item_reached.MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
+                missionStats.setLastReachedWaypointNumber(((msg_mission_item_reached) message).seq);
+                break;
+
+            case msg_nav_controller_output.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
+                msg_nav_controller_output m_nav = (msg_nav_controller_output) message;
+                setDisttowpAndSpeedAltErrors(m_nav.wp_dist, m_nav.alt_error, m_nav.aspd_error);
+                break;
         }
     }
 
+    private void setDisttowpAndSpeedAltErrors(double disttowp, double alt_error, double aspd_error) {
+        missionStats.setDistanceToWp(disttowp);
+
+        this.altitude.setTargetAltitude(this.altitude.getAltitude() + alt_error);
+        notifyDroneEvent(DroneInterfaces.DroneEventsType.ORIENTATION);
+    }
+
     private void checkControlSensorsHealth(msg_sys_status sysStatus) {
         boolean isRCFailsafe = (sysStatus.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR
                 .MAV_SYS_STATUS_SENSOR_RC_RECEIVER) == 0;

ServiceApp/src/org/droidplanner/services/android/core/gcs/follow/FollowSoloShot.java

@@ -2,6 +2,7 @@
 
 import android.os.Handler;
 
+import com.o3dr.services.android.lib.coordinate.LatLong;
 import com.o3dr.services.android.lib.coordinate.LatLongAlt;
 import com.o3dr.services.android.lib.drone.companion.solo.tlv.SoloMessageLocation;
 
@@ -24,7 +25,7 @@ public class FollowSoloShot extends FollowAlgorithm {
 
     public FollowSoloShot(DroneManager droneMgr, Handler handler) {
         super(droneMgr, handler);
-        final ArduSolo drone = (ArduSolo) droneMgr.getDrone();
+        ArduSolo drone = (ArduSolo) droneMgr.getDrone();
         this.soloComp = drone.getSoloComp();
     }
 
@@ -43,12 +44,9 @@ public void disableFollow() {
     @Override
     protected void processNewLocation(Location location) {
         if (location != null) {
-            final LatLongAlt receivedCoord = location.getCoord();
-
-            locationCoord.setAltitude(receivedCoord.getAltitude());
-            locationCoord.setLatitude(receivedCoord.getLatitude());
-            locationCoord.setLongitude(receivedCoord.getLongitude());
+            LatLongAlt receivedCoord = location.getCoord();
 
+            locationCoord.set((LatLong)receivedCoord);
             locationSetter.setCoordinate(locationCoord);
 
             soloComp.updateFollowCenter(locationSetter);
@@ -62,6 +60,43 @@ public FollowModes getType() {
 
     @Override
     protected ROIEstimator initROIEstimator(MavLinkDrone drone, Handler handler) {
-        return null;
+        return new SoloROIEstimator(drone, handler, ((ArduSolo)drone).getSoloComp());
+    }
+
+    protected static class SoloROIEstimator extends ROIEstimator {
+
+        private final LatLongAlt locationCoord = new LatLongAlt(0, 0, 0);
+        private final SoloMessageLocation locationSetter = new SoloMessageLocation(locationCoord);
+        private final SoloComp soloComp;
+
+        public SoloROIEstimator(MavLinkDrone drone, Handler handler, SoloComp soloComp) {
+            super(drone, handler);
+            this.soloComp = soloComp;
+        }
+
+        @Override
+        public void enableFollow(){
+            isFollowEnabled.set(true);
+        }
+
+        @Override
+        public void disableFollow(){
+            if(isFollowEnabled.compareAndSet(true, false)){
+                realLocation = null;
+                disableWatchdog();
+            }
+        }
+
+        @Override
+        protected long getUpdatePeriod(){
+            return 40l;
+        }
+
+        @Override
+        protected void sendUpdateROI(LatLong goCoord){
+            locationCoord.set(goCoord);
+            locationSetter.setCoordinate(locationCoord);
+            soloComp.updateFollowCenter(locationSetter);
+        }
     }
 }

ServiceApp/src/org/droidplanner/services/android/core/gcs/roi/ROIEstimator.java

@@ -86,13 +86,21 @@ protected void updateROI() {
                 * (System.currentTimeMillis() - timeOfLastLocation) / 1000f;
         LatLong goCoord = GeoTools.newCoordFromBearingAndDistance(gcsCoord, bearing, distanceTraveledSinceLastPoint);
 
-        MavLinkDoCmds.setROI(drone, new LatLongAlt(goCoord.getLatitude(), goCoord.getLongitude(), (0.0)), null);
+        sendUpdateROI(goCoord);
 
         if (realLocation.getSpeed() > 0)
-            watchdog.postDelayed(watchdogCallback, TIMEOUT);
+            watchdog.postDelayed(watchdogCallback, getUpdatePeriod());
+    }
+
+    protected void sendUpdateROI(LatLong goCoord) {
+        MavLinkDoCmds.setROI(drone, new LatLongAlt(goCoord.getLatitude(), goCoord.getLongitude(), (0.0)), null);
     }
 
     public boolean isFollowEnabled() {
         return isFollowEnabled.get();
     }
+
+    protected long getUpdatePeriod(){
+        return TIMEOUT;
+    }
 }