diff --git a/Android/.classpath b/Android/.classpath index 695fcafa91..a36b91f54c 100644 --- a/Android/.classpath +++ b/Android/.classpath @@ -1,5 +1,7 @@ + + diff --git a/Android/AndroidManifest.xml b/Android/AndroidManifest.xml index 2afa8591d0..22fbf676af 100644 --- a/Android/AndroidManifest.xml +++ b/Android/AndroidManifest.xml @@ -2,7 +2,7 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Android/res/layout/fragment_editor_detail_takeoff.xml b/Android/res/layout/fragment_editor_detail_takeoff.xml index 42fc1d161b..b53dc65831 100644 --- a/Android/res/layout/fragment_editor_detail_takeoff.xml +++ b/Android/res/layout/fragment_editor_detail_takeoff.xml @@ -52,7 +52,7 @@ android:layout_marginLeft="12dp" android:layout_width="wrap_content" android:layout_height="wrap_content" - android:text="@string/mission_item_title_takeoff"/> + android:text="@string/waypointType_TakeOff"/> pref_warn_on_arm pref_auto_insert_mission_takeoff_rtl_land pref_warn_on_takeoff_in_auto + pref_warn_on_dronie_creation pref_enable_tts tts_periodic diff --git a/Android/res/values/strings.xml b/Android/res/values/strings.xml index ad308a0c6c..4e38e383c1 100644 --- a/Android/res/values/strings.xml +++ b/Android/res/values/strings.xml @@ -177,14 +177,15 @@ Waypoint Spline Waypoint - Take-Off + Takeoff Return To Launch Land + Change Speed Circle Loiter Region of Interest Survey Polygon - Landmark Mapper + Structure Scan Land vehicle at the current location. You must manually exit Auto mode to disarm motors. @@ -199,7 +200,7 @@ Set condition to reach a certain target angle. Delay mission state machine until within desired distance of next NAV point Changes the home location either to the current location or a specified location. - Change speed and/or throttle set points.Set -1 for both Speed and Throttle if no changes is required + Change vehicle speed until the end of this mission. Sets a location and altitude to point copter towards and to aim an optional camera. Repeat waypoint @@ -337,6 +338,7 @@ Altitude m m/s + Speed Ground Speed Air Speed --° @@ -351,6 +353,7 @@ Auto Follow Dronie + Change Speed Telemetry Flight modes Info Bar @@ -476,7 +479,7 @@ How to plan and fly a mission - TAKEOFF + Advanced Number of Steps Altitude Step @@ -495,7 +498,7 @@ Locator Open Log File - Waiting for GPS... + Waiting for GPS… Miscellaneous @@ -509,5 +512,8 @@ User Interface -> Preference Dialogs]]> Take off in Auto? Warning! The drone will now take off and start the mission. + Create dronie? + Please stand back 8m before starting the + dronie. diff --git a/Android/res/xml/preferences.xml b/Android/res/xml/preferences.xml index 2e7973d48a..e29987d007 100644 --- a/Android/res/xml/preferences.xml +++ b/Android/res/xml/preferences.xml @@ -168,6 +168,14 @@ android:defaultValue="@string/pref_dialog_entry_ask" android:entryValues="@array/preference_dialog_entry"/> + + diff --git a/Android/src/org/droidplanner/android/activities/FlightActivity.java b/Android/src/org/droidplanner/android/activities/FlightActivity.java index 0393973f99..d625497767 100644 --- a/Android/src/org/droidplanner/android/activities/FlightActivity.java +++ b/Android/src/org/droidplanner/android/activities/FlightActivity.java @@ -119,9 +119,7 @@ public void onDrawerOpened() { resetMapBearing.setOnClickListener(new View.OnClickListener() { @Override public void onClick(View v) { - if(mapFragment != null) { - mapFragment.updateMapBearing(0); - } + updateMapBearing(0); } }); @@ -237,6 +235,11 @@ private void updateMapLocationButtons(AutoPanMode mode) { } } + public void updateMapBearing(float bearing){ + if(mapFragment != null) + mapFragment.updateMapBearing(bearing); + } + /** * Ensures that the device has the correct version of the Google Play * Services. diff --git a/Android/src/org/droidplanner/android/fragments/CopterFlightActionsFragment.java b/Android/src/org/droidplanner/android/fragments/CopterFlightActionsFragment.java index 7b85d6548f..fb2f3ee4dc 100644 --- a/Android/src/org/droidplanner/android/fragments/CopterFlightActionsFragment.java +++ b/Android/src/org/droidplanner/android/fragments/CopterFlightActionsFragment.java @@ -1,5 +1,6 @@ package org.droidplanner.android.fragments; +import android.app.Activity; import android.graphics.Color; import android.os.Bundle; import android.support.v4.app.Fragment; @@ -14,9 +15,11 @@ import org.droidplanner.R; import org.droidplanner.android.DroidPlannerApp; +import org.droidplanner.android.activities.FlightActivity; import org.droidplanner.android.activities.helpers.SuperUI; import org.droidplanner.android.dialogs.YesNoDialog; import org.droidplanner.android.dialogs.YesNoWithPrefsDialog; +import org.droidplanner.android.proxy.mission.MissionProxy; import org.droidplanner.android.utils.analytics.GAUtils; import org.droidplanner.core.MAVLink.MavLinkArm; import org.droidplanner.core.drone.DroneInterfaces; @@ -35,7 +38,7 @@ public class CopterFlightActionsFragment extends Fragment implements View.OnClic private static final double TAKEOFF_ALTITUDE = 10.0; private Drone drone; - + private MissionProxy missionProxy; private Follow followMe; private View mDisconnectedButtons; @@ -49,6 +52,15 @@ public class CopterFlightActionsFragment extends Fragment implements View.OnClic private Button pauseBtn; private Button autoBtn; + @Override + public void onAttach(Activity activity){ + super.onAttach(activity); + if(!(activity instanceof FlightActivity)){ + throw new IllegalStateException("Parent activity must be an instance of " + + FlightActivity.class.getName()); + } + } + @Override public View onCreateView(LayoutInflater inflater, ViewGroup container, Bundle savedInstanceState) { View view = inflater.inflate(R.layout.fragment_copter_mission_control, container, false); @@ -56,6 +68,7 @@ public View onCreateView(LayoutInflater inflater, ViewGroup container, Bundle sa DroidPlannerApp droidPlannerApp = (DroidPlannerApp) getActivity().getApplication(); drone = droidPlannerApp.getDrone(); followMe = droidPlannerApp.getFollowMe(); + missionProxy = droidPlannerApp.getMissionProxy(); return view; } @@ -208,7 +221,7 @@ public void onClick(View v) { break; case R.id.mc_dronieBtn: - drone.getMission().makeAndUploadDronie(); + getDronieConfirmation(); eventBuilder.setAction(ACTION_FLIGHT_ACTION_BUTTON).setLabel("Dronie uploaded"); break; @@ -223,6 +236,31 @@ public void onClick(View v) { } + private void getDronieConfirmation() { + YesNoWithPrefsDialog ynd = YesNoWithPrefsDialog.newInstance(getActivity() + .getApplicationContext(), getString(R.string.pref_dronie_creation_title), + getString(R.string.pref_dronie_creation_message), new YesNoDialog.Listener() { + @Override + public void onYes() { + final float bearing = missionProxy.makeAndUploadDronie(); + if(bearing >= 0){ + final FlightActivity flightActivity = (FlightActivity) getActivity(); + if(flightActivity != null){ + flightActivity.updateMapBearing(bearing); + } + } + } + + @Override + public void onNo() { + } + }, getString(R.string.pref_warn_on_dronie_creation_key)); + + if(ynd != null){ + ynd.show(getChildFragmentManager(), "Confirm dronie creation"); + } + } + private void getTakeOffInAutoConfirmation() { YesNoWithPrefsDialog ynd = YesNoWithPrefsDialog.newInstance(getActivity() .getApplicationContext(), getString(R.string.dialog_confirm_take_off_in_auto_title), diff --git a/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java b/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java index e87b48e343..b835fb106b 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java +++ b/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java @@ -25,6 +25,7 @@ import org.droidplanner.core.mission.waypoints.SpatialCoordItem; import org.droidplanner.core.mission.waypoints.SplineWaypoint; import org.droidplanner.core.mission.waypoints.Waypoint; +import org.droidplanner.core.model.Drone; import org.droidplanner.core.util.Pair; /** @@ -223,16 +224,6 @@ public void addTakeoff() { mMission.addMissionItem(takeoff); } - public void addDronie(Coord2D start, Coord2D end) { - clear(); - - List dronieItems = Mission.createDronie(mMission,start, end); - - addMissionItems(dronieItems); - } - - - public void addTakeOffAndRTL(){ if(!mMission.isFirstItemTakeoff()){ final Takeoff takeOff = new Takeoff(mMission, new Altitude(Takeoff.DEFAULT_TAKEOFF_ALTITUDE)); @@ -569,4 +560,10 @@ public Length getMissionLength() { return new Length(0); } } + + public float makeAndUploadDronie() { + final double bearing = mMission.makeAndUploadDronie(); + refresh(); + return (float) bearing; + } } diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionChangeSpeedFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionChangeSpeedFragment.java new file mode 100644 index 0000000000..bf868c1c81 --- /dev/null +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionChangeSpeedFragment.java @@ -0,0 +1,50 @@ +package org.droidplanner.android.proxy.mission.item.fragments; + +import org.droidplanner.R; +import org.droidplanner.android.widgets.spinnerWheel.CardWheelHorizontalView; +import org.droidplanner.android.widgets.spinnerWheel.adapters.NumericWheelAdapter; +import org.droidplanner.core.helpers.units.Speed; +import org.droidplanner.core.mission.MissionItem; +import org.droidplanner.core.mission.MissionItemType; +import org.droidplanner.core.mission.commands.ChangeSpeed; + +import android.os.Bundle; +import android.view.View; + +public class MissionChangeSpeedFragment extends MissionDetailFragment implements + CardWheelHorizontalView.OnCardWheelChangedListener { + + @Override + protected int getResource() { + return R.layout.fragment_editor_detail_change_speed; + } + + @Override + public void onViewCreated(View view, Bundle savedInstanceState) { + super.onViewCreated(view, savedInstanceState); + typeSpinner.setSelection(commandAdapter.getPosition(MissionItemType.CHANGE_SPEED)); + + ChangeSpeed item = (ChangeSpeed) getMissionItems().get(0); + + final NumericWheelAdapter adapter = new NumericWheelAdapter(getActivity() + .getApplicationContext(), R.layout.wheel_text_centered, 1, + 20, "%d m/s"); + final CardWheelHorizontalView cardAltitudePicker = (CardWheelHorizontalView) view + .findViewById(R.id.picker1); + cardAltitudePicker.setViewAdapter(adapter); + cardAltitudePicker.addChangingListener(this); + cardAltitudePicker.setCurrentValue((int) item.getSpeed().valueInMetersPerSecond()); + } + + @Override + public void onChanged(CardWheelHorizontalView wheel, int oldValue, int newValue) { + switch (wheel.getId()) { + case R.id.picker1: + for(MissionItem missionItem : getMissionItems()) { + ChangeSpeed item = (ChangeSpeed) missionItem; + item.setSpeed(new Speed(newValue)); + } + break; + } + } +} diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java index 35129383ee..d979c4859a 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java @@ -77,6 +77,9 @@ public static MissionDetailFragment newInstance(MissionItemType itemType) { case CIRCLE: fragment = new MissionCircleFragment(); break; + case CHANGE_SPEED: + fragment = new MissionChangeSpeedFragment(); + break; case ROI: fragment = new MissionRegionOfInterestFragment(); break; diff --git a/ChangeLog.md b/ChangeLog.md index 506d053d90..375f4fc035 100644 --- a/ChangeLog.md +++ b/ChangeLog.md @@ -9,6 +9,11 @@ All the changes are logged below (preferable with the pull request numbers in pa # Releases +## Droidplanner v2.8.1 +* Added Change Speed mission item (#1202) +* UI changes to scan option (#1204) +* Dronie update (#1203) + ## Droidplanner v2.8.0 * Tablet only flight's * Initial ArduPlane support (#1181, #1193, #1080, #1194) diff --git a/Core/src/org/droidplanner/core/mission/Mission.java b/Core/src/org/droidplanner/core/mission/Mission.java index 320466fd97..1f91569659 100644 --- a/Core/src/org/droidplanner/core/mission/Mission.java +++ b/Core/src/org/droidplanner/core/mission/Mission.java @@ -1,8 +1,8 @@ package org.droidplanner.core.mission; -import java.util.ArrayList; -import java.util.Collections; -import java.util.List; +import com.MAVLink.Messages.ardupilotmega.msg_mission_ack; +import com.MAVLink.Messages.ardupilotmega.msg_mission_item; +import com.MAVLink.Messages.enums.MAV_CMD; import org.droidplanner.core.drone.DroneInterfaces.DroneEventsType; import org.droidplanner.core.drone.DroneVariable; @@ -11,6 +11,8 @@ import org.droidplanner.core.helpers.geoTools.GeoTools; import org.droidplanner.core.helpers.units.Altitude; import org.droidplanner.core.helpers.units.Length; +import org.droidplanner.core.helpers.units.Speed; +import org.droidplanner.core.mission.commands.ChangeSpeed; import org.droidplanner.core.mission.commands.ReturnToHome; import org.droidplanner.core.mission.commands.Takeoff; import org.droidplanner.core.mission.waypoints.Circle; @@ -22,9 +24,9 @@ import org.droidplanner.core.model.Drone; import org.droidplanner.core.util.Pair; -import com.MAVLink.Messages.ardupilotmega.msg_mission_ack; -import com.MAVLink.Messages.ardupilotmega.msg_mission_item; -import com.MAVLink.Messages.enums.MAV_CMD; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; /** * This implements a mavlink mission. A mavlink mission is a set of @@ -104,10 +106,10 @@ public void addMissionItem(MissionItem missionItem) { notifyMissionUpdate(); } - public void addMissionItem(int index, MissionItem missionItem){ - items.add(index, missionItem); - notifyMissionUpdate(); - } + public void addMissionItem(int index, MissionItem missionItem) { + items.add(index, missionItem); + notifyMissionUpdate(); + } /** * Signals that this mission object was updated. //TODO: maybe move outside @@ -141,39 +143,39 @@ public Altitude getLastAltitude() { */ public void replace(MissionItem oldItem, MissionItem newItem) { final int index = items.indexOf(oldItem); - if(index == -1){ - return; - } + if (index == -1) { + return; + } items.remove(index); items.add(index, newItem); notifyMissionUpdate(); } - public void replaceAll(List> updatesList){ - if(updatesList == null || updatesList.isEmpty()){ - return; - } + public void replaceAll(List> updatesList) { + if (updatesList == null || updatesList.isEmpty()) { + return; + } - boolean wasUpdated = false; - for(Pair updatePair : updatesList){ - final MissionItem oldItem = updatePair.first; - final int index = items.indexOf(oldItem); - if(index == -1){ - continue; - } + boolean wasUpdated = false; + for (Pair updatePair : updatesList) { + final MissionItem oldItem = updatePair.first; + final int index = items.indexOf(oldItem); + if (index == -1) { + continue; + } - final MissionItem newItem = updatePair.second; - items.remove(index); - items.add(index, newItem); + final MissionItem newItem = updatePair.second; + items.remove(index); + items.add(index, newItem); - wasUpdated = true; - } + wasUpdated = true; + } - if(wasUpdated) { - notifyMissionUpdate(); - } - } + if (wasUpdated) { + notifyMissionUpdate(); + } + } /** * Reverse the order of the mission items. @@ -267,6 +269,9 @@ private List processMavLinkMessages(List msgs) { case MAV_CMD.MAV_CMD_NAV_TAKEOFF: received.add(new Takeoff(msg, this)); break; + case MAV_CMD.MAV_CMD_DO_CHANGE_SPEED: + received.add(new ChangeSpeed(msg, this)); + break; case MAV_CMD.MAV_CMD_DO_SET_ROI: received.add(new RegionOfInterest(msg, this)); break; @@ -298,28 +303,50 @@ public List getMsgMissionItems() { return data; } - public void makeAndUploadDronie() { + /** + * Create and upload a dronie mission to the drone + * + * @return the bearing in degrees the drone trajectory will take. + */ + public double makeAndUploadDronie() { Coord2D currentPosition = myDrone.getGps().getPosition(); - if(currentPosition == null || myDrone.getGps().getSatCount()<=5){ + if (currentPosition == null || myDrone.getGps().getSatCount() <= 5) { myDrone.notifyDroneEvent(DroneEventsType.WARNING_NO_GPS); - return; + return -1; } + + final double bearing = 180 + myDrone.getOrientation().getYaw(); items.clear(); - items.addAll(createDronie(this, currentPosition, GeoTools.newCoordFromBearingAndDistance( - currentPosition, 180 + myDrone.getOrientation().getYaw(), 50.0))); + items.addAll(createDronie(currentPosition, + GeoTools.newCoordFromBearingAndDistance(currentPosition, bearing, 50.0))); sendMissionToAPM(); notifyMissionUpdate(); + + return bearing; } - public static List createDronie(Mission mMission,Coord2D start, Coord2D end) { + public List createDronie(Coord2D start, Coord2D end) { final int startAltitude = 4; - + final int roiDistance = -8; + Coord2D slowDownPoint = GeoTools.pointAlongTheLine(start, end, 5); + + Speed defaultSpeed = myDrone.getSpeed().getSpeedParameter(); + if (defaultSpeed == null) { + defaultSpeed = new Speed(5); + } + List dronieItems = new ArrayList(); - dronieItems.add(new Takeoff(mMission, new Altitude(startAltitude))); - dronieItems.add(new RegionOfInterest(mMission,new Coord3D(GeoTools.pointAlongTheLine(start, end, -8), new Altitude(1.0)))); - dronieItems.add(new Waypoint(mMission, new Coord3D(end, new Altitude(startAltitude+GeoTools.getDistance(start, end).valueInMeters()/2.0)))); - dronieItems.add(new Waypoint(mMission, new Coord3D(start, new Altitude(startAltitude)))); - dronieItems.add(new Land(mMission,start)); + dronieItems.add(new Takeoff(this, new Altitude(startAltitude))); + dronieItems.add(new RegionOfInterest(this, new Coord3D(GeoTools.pointAlongTheLine(start, + end, roiDistance), new Altitude(1.0)))); + dronieItems.add(new Waypoint(this, new Coord3D(end, new Altitude(startAltitude + + GeoTools.getDistance(start, end).valueInMeters() / 2.0)))); + dronieItems.add(new Waypoint(this, new Coord3D(slowDownPoint, new Altitude(startAltitude + + GeoTools.getDistance(start, slowDownPoint).valueInMeters() / 2.0)))); + dronieItems.add(new ChangeSpeed(this, new Speed(1.0))); + dronieItems.add(new Waypoint(this, new Coord3D(start, new Altitude(startAltitude)))); + dronieItems.add(new ChangeSpeed(this, defaultSpeed)); + dronieItems.add(new Land(this, start)); return dronieItems; } @@ -337,7 +364,7 @@ public boolean isFirstItemTakeoff() { } public boolean isLastItemLandOrRTL() { - MissionItem last = items.get(items.size()-1); + MissionItem last = items.get(items.size() - 1); return (last instanceof ReturnToHome) || (last instanceof Land); } } diff --git a/Core/src/org/droidplanner/core/mission/MissionItemType.java b/Core/src/org/droidplanner/core/mission/MissionItemType.java index c756f4fffe..c54f3ece13 100644 --- a/Core/src/org/droidplanner/core/mission/MissionItemType.java +++ b/Core/src/org/droidplanner/core/mission/MissionItemType.java @@ -3,6 +3,7 @@ import java.util.Collections; import org.droidplanner.core.helpers.coordinates.Coord2D; +import org.droidplanner.core.mission.commands.ChangeSpeed; import org.droidplanner.core.mission.commands.ReturnToHome; import org.droidplanner.core.mission.commands.Takeoff; import org.droidplanner.core.mission.survey.CylindricalSurvey; @@ -16,7 +17,7 @@ public enum MissionItemType { WAYPOINT("Waypoint"), SPLINE_WAYPOINT("Spline Waypoint"), TAKEOFF("Takeoff"), RTL( "Return to Launch"), LAND("Land"), CIRCLE("Circle"), ROI("Region of Interest"), SURVEY( - "Survey"), CYLINDRICAL_SURVEY("Landmark Mapper"); + "Survey"), CYLINDRICAL_SURVEY("Structure Scan"), CHANGE_SPEED("Change Speed"); private final String name; @@ -36,6 +37,8 @@ public MissionItem getNewItem(MissionItem referenceItem) throws IllegalArgumentE return new SplineWaypoint(referenceItem); case TAKEOFF: return new Takeoff(referenceItem); + case CHANGE_SPEED: + return new ChangeSpeed(referenceItem); case RTL: return new ReturnToHome(referenceItem); case LAND: @@ -53,4 +56,4 @@ public MissionItem getNewItem(MissionItem referenceItem) throws IllegalArgumentE + "."); } } -} \ No newline at end of file +} diff --git a/Core/src/org/droidplanner/core/mission/commands/ChangeSpeed.java b/Core/src/org/droidplanner/core/mission/commands/ChangeSpeed.java new file mode 100644 index 0000000000..2f599421a6 --- /dev/null +++ b/Core/src/org/droidplanner/core/mission/commands/ChangeSpeed.java @@ -0,0 +1,58 @@ +package org.droidplanner.core.mission.commands; + +import java.util.List; + +import org.droidplanner.core.helpers.units.Speed; +import org.droidplanner.core.mission.Mission; +import org.droidplanner.core.mission.MissionItem; +import org.droidplanner.core.mission.MissionItemType; + +import com.MAVLink.Messages.ardupilotmega.msg_mission_item; +import com.MAVLink.Messages.enums.MAV_CMD; +import com.MAVLink.Messages.enums.MAV_FRAME; + +public class ChangeSpeed extends MissionCMD { + private Speed speed = new Speed(5); + + public ChangeSpeed(MissionItem item) { + super(item); + } + + public ChangeSpeed(msg_mission_item msg, Mission mission) { + super(mission); + unpackMAVMessage(msg); + } + + public ChangeSpeed(Mission mission, Speed speed) { + super(mission); + this.speed = speed; + } + + @Override + public List packMissionItem() { + List list = super.packMissionItem(); + msg_mission_item mavMsg = list.get(0); + mavMsg.command = MAV_CMD.MAV_CMD_DO_CHANGE_SPEED; + mavMsg.frame = MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT; + mavMsg.param2 = (float) speed.valueInMetersPerSecond(); + return list; + } + + @Override + public void unpackMAVMessage(msg_mission_item mavMsg) { + speed = new Speed(mavMsg.param2); + } + + @Override + public MissionItemType getType() { + return MissionItemType.CHANGE_SPEED; + } + + public Speed getSpeed() { + return speed; + } + + public void setSpeed(Speed speed) { + this.speed = speed; + } +} \ No newline at end of file diff --git a/Core/src/org/droidplanner/core/mission/survey/CylindricalSurvey.java b/Core/src/org/droidplanner/core/mission/survey/CylindricalSurvey.java index 233dd353f2..0f0ce9190a 100644 --- a/Core/src/org/droidplanner/core/mission/survey/CylindricalSurvey.java +++ b/Core/src/org/droidplanner/core/mission/survey/CylindricalSurvey.java @@ -35,7 +35,7 @@ private CylindricalSurvey(Mission mission) { startHeight = new Altitude(10); heightStep = new Altitude(5); numberOfSteps = 2; - crossHatch = true; + crossHatch = false; } public CylindricalSurvey(Mission mission, Coord2D center) { @@ -180,4 +180,4 @@ public void setNumberOfSteps(int newValue) { numberOfSteps = newValue; } -} \ No newline at end of file +} diff --git a/Core/test/org/droidplanner/core/mission/waypoints/ChangeSpeedTest.java b/Core/test/org/droidplanner/core/mission/waypoints/ChangeSpeedTest.java new file mode 100644 index 0000000000..3441d1e0a8 --- /dev/null +++ b/Core/test/org/droidplanner/core/mission/waypoints/ChangeSpeedTest.java @@ -0,0 +1,35 @@ +package org.droidplanner.core.mission.waypoints; + +import java.util.List; + +import junit.framework.TestCase; + +import org.droidplanner.core.helpers.units.Speed; +import org.droidplanner.core.mission.Mission; +import org.droidplanner.core.mission.commands.ChangeSpeed; + +import com.MAVLink.Messages.ardupilotmega.msg_mission_item; +import com.MAVLink.Messages.enums.MAV_CMD; + +public class ChangeSpeedTest extends TestCase { + + public void testPackMissionItem() { + Mission mission = new Mission(null); + ChangeSpeed item = new ChangeSpeed(mission, new Speed(12.0)); + + List listOfMsg = item.packMissionItem(); + assertEquals(1, listOfMsg.size()); + + msg_mission_item msg = listOfMsg.get(0); + + assertEquals(MAV_CMD.MAV_CMD_DO_CHANGE_SPEED, msg.command); + assertEquals(0.0f, msg.x); + assertEquals(0.0f, msg.y); + assertEquals(0.0f, msg.z); + assertEquals(0.0f, msg.param1); + assertEquals(12.0f, msg.param2); + assertEquals(0.0f, msg.param3); + assertEquals(0.0f, msg.param3); + } + +}