Skip to content

Commit

Permalink
Merge pull request #1247 from alextel69/feature-mission-item-do-set-s…
Browse files Browse the repository at this point in the history
…ervo

Add DO_SET_SERVO mission item
  • Loading branch information
arthurbenemann committed Nov 3, 2014
2 parents f4d1f07 + 05e7691 commit 3067b14
Show file tree
Hide file tree
Showing 7 changed files with 289 additions and 1 deletion.
117 changes: 117 additions & 0 deletions Android/res/layout/fragment_editor_detail_set_servo.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,117 @@
<?xml version="1.0" encoding="utf-8"?>
<RelativeLayout xmlns:android="http://schemas.android.com/apk/res/android"
xmlns:tools="http://schemas.android.com/tools"
style="@style/missionItemDetailLayout" >

<org.droidplanner.android.widgets.spinners.SpinnerSelfSelect
android:id="@+id/spinnerWaypointType"
android:layout_width="match_parent"
android:layout_height="wrap_content"
android:layout_alignParentTop="true"
android:layout_margin="5dp"
android:entries="@array/ExampleWaypointType" />

<RelativeLayout
android:id="@+id/title_rect"
android:layout_width="match_parent"
android:layout_height="64dp"
android:layout_alignParentTop="true"
android:background="@drawable/wp_title_rectangle" >

<TextView
android:id="@+id/WaypointIndex"
style="@style/largeMissionDetailText"
android:layout_width="64dp"
android:layout_height="wrap_content"
android:layout_centerVertical="true"
tools:text="22" />

<View
android:id="@+id/title_div"
android:layout_width="1dp"
android:layout_height="54dp"
android:layout_alignParentTop="true"
android:layout_marginTop="5dp"
android:layout_toRightOf="@id/WaypointIndex"
android:background="@drawable/wp_title_div" />

<RelativeLayout
android:id="@+id/title_content"
android:layout_width="match_parent"
android:layout_height="wrap_content"
android:layout_centerVertical="true"
android:layout_toRightOf="@id/title_div"
android:orientation="vertical" >

<TextView
android:id="@+id/WaypointType"
style="@style/missionHeaderTitle"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_marginLeft="12dp"
android:text="@string/waypointType_Set_Servo" />

<TextView
android:id="@+id/DistanceLabel"
style="@style/missionHeaderlabel"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_alignLeft="@id/WaypointType"
android:layout_below="@id/WaypointType" />
</RelativeLayout>
</RelativeLayout>

<ImageView
android:id="@+id/menuHint"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_alignBottom="@id/title_rect"
android:layout_alignParentRight="true"
android:layout_margin="10dp"
android:src="@drawable/ic_menu_hint"
tools:ignore="ContentDescription" />

<LinearLayout
android:layout_width="match_parent"
android:layout_height="wrap_content"
android:layout_below="@id/title_rect"
android:orientation="vertical"
android:paddingBottom="5dp" >

<TextView
style="@style/ModeDetailText"
android:layout_width="match_parent"
android:layout_height="wrap_content"
android:background="@drawable/mode_desc_rectangle"
android:padding="12dp"
android:text="@string/waypointInfo_SetServo" />

<org.droidplanner.android.widgets.spinnerWheel.CardWheelHorizontalView
android:id="@+id/picker1"
style="@style/missionItemDetailCard"
android:layout_width="match_parent"
android:layout_height="wrap_content"
android:orientation="vertical"
android:text="@string/channel_label" />

<TextView
style="@style/ModeDetailText"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_gravity="left"
android:text="@string/pwm"
android:textAppearance="?android:attr/textAppearanceMedium" />

<EditText
android:id="@+id/PwmEditText"
android:layout_width="match_parent"
android:layout_height="wrap_content"
android:inputType="number"
android:imeOptions="actionDone"
android:paddingLeft="16dp"
android:paddingRight="16dp"
/>

</LinearLayout>

</RelativeLayout>
6 changes: 6 additions & 0 deletions Android/res/values/strings.xml
Original file line number Diff line number Diff line change
Expand Up @@ -182,6 +182,7 @@
<string name="waypointType_Land">Land</string>
<string name="waypointType_Change_Speed">Change Speed</string>
<string name="waypointType_Camera_Trigger">Camera Trigger</string>
<string name="waypointType_Set_Servo">Set Servo</string>
<string name="waypointType_EPM">EPM</string>
<string name="waypointType_Circle">Circle</string>
<string name="waypointType_Loiter">Loiter</string>
Expand All @@ -204,9 +205,13 @@
<string name="waypointInfo_SetHome">Changes the home location either to the current location or a specified location.</string>
<string name="waypointInfo_SetSpeed">Change vehicle speed until the end of this mission.</string>
<string name="waypointInfo_CameraTrigger">Trigger the camera shutter at regular distance intervals.</string>
<string name="waypointInfo_SetServo">Move a servo to a particular PWM value.</string>
<string name="waypointInfo_EpmGrabber">Release the payload of the EPM grabber</string>
<string name="waypointInfo_ROI">Sets a location and altitude to point copter towards and to aim an optional camera.</string>
<string name="waypointInfo_SetJump">Repeat waypoint</string>

<!-- Custom waypoint info -->
<string name="pwm">PWM:</string>

<!-- Waypoint editor -->
<string name="editor_err_land_rtl_added">One cannot add new waypoints after LAND or RTL</string>
Expand Down Expand Up @@ -346,6 +351,7 @@
<string name="speed_unit">m/s</string>
<string name="speed_label">Speed</string>
<string name="length_label">Length</string>
<string name="channel_label">Channel</string>
<string name="ground_speed_label">Ground Speed</string>
<string name="air_speed_label">Air Speed</string>
<string name="default_angle_value">--°</string>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,9 @@ public static MissionDetailFragment newInstance(MissionItemType itemType) {
case EPM_GRIPPER:
fragment = new MissionEpmGrabberFragment();
break;
case SET_SERVO:
fragment = new SetServoFragment();
break;
default:
fragment = null;
break;
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
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.mission.MissionItem;
import org.droidplanner.core.mission.MissionItemType;
import org.droidplanner.core.mission.commands.SetServo;

import android.os.Bundle;
import android.text.Editable;
import android.text.TextWatcher;
import android.view.View;
import android.widget.EditText;

public class SetServoFragment extends MissionDetailFragment implements
CardWheelHorizontalView.OnCardWheelChangedListener, TextWatcher {

@Override
protected int getResource() {
return R.layout.fragment_editor_detail_set_servo;
}

@Override
public void onViewCreated(View view, Bundle savedInstanceState) {
super.onViewCreated(view, savedInstanceState);
typeSpinner.setSelection(commandAdapter
.getPosition(MissionItemType.SET_SERVO));

SetServo item = (SetServo) getMissionItems().get(0);

final NumericWheelAdapter adapter = new NumericWheelAdapter(
getActivity().getApplicationContext(),
R.layout.wheel_text_centered, 1, 8, "%d");
final CardWheelHorizontalView cardChannelPicker = (CardWheelHorizontalView) view
.findViewById(R.id.picker1);
final EditText pwmEditText = (EditText) view
.findViewById(R.id.PwmEditText);

cardChannelPicker.setViewAdapter(adapter);
cardChannelPicker.addChangingListener(this);
cardChannelPicker.setCurrentValue((int) item.getChannel());

pwmEditText.setText(Integer.toString(item.getPwm()));
pwmEditText.addTextChangedListener(this);

}

@Override
public void onChanged(CardWheelHorizontalView wheel, int oldValue,
int newValue) {
switch (wheel.getId()) {
case R.id.picker1:
for (MissionItem missionItem : getMissionItems()) {
SetServo item = (SetServo) missionItem;
item.setChannel(newValue);
}
break;
}
}

@Override
public void afterTextChanged(Editable editable) {

}

@Override
public void beforeTextChanged(CharSequence arg0, int arg1, int arg2,
int arg3) {

}

@Override
public void onTextChanged(CharSequence s, int arg1, int arg2, int arg3) {
if (!s.toString().isEmpty()) {
for (MissionItem missionItem : getMissionItems()) {
SetServo item = (SetServo) missionItem;
item.setPwm(Integer.valueOf(s.toString()));
}
}

}
}
4 changes: 4 additions & 0 deletions Core/src/org/droidplanner/core/mission/Mission.java
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
import org.droidplanner.core.mission.commands.ChangeSpeed;
import org.droidplanner.core.mission.commands.EpmGripper;
import org.droidplanner.core.mission.commands.ReturnToHome;
import org.droidplanner.core.mission.commands.SetServo;
import org.droidplanner.core.mission.commands.Takeoff;
import org.droidplanner.core.mission.waypoints.Circle;
import org.droidplanner.core.mission.waypoints.Land;
Expand Down Expand Up @@ -257,6 +258,9 @@ private List<MissionItem> processMavLinkMessages(List<msg_mission_item> msgs) {

for (msg_mission_item msg : msgs) {
switch (msg.command) {
case MAV_CMD.MAV_CMD_DO_SET_SERVO:
received.add(new SetServo(msg, this));
break;
case MAV_CMD.MAV_CMD_NAV_WAYPOINT:
received.add(new Waypoint(msg, this));
break;
Expand Down
5 changes: 4 additions & 1 deletion Core/src/org/droidplanner/core/mission/MissionItemType.java
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
import org.droidplanner.core.mission.commands.ChangeSpeed;
import org.droidplanner.core.mission.commands.EpmGripper;
import org.droidplanner.core.mission.commands.ReturnToHome;
import org.droidplanner.core.mission.commands.SetServo;
import org.droidplanner.core.mission.commands.Takeoff;
import org.droidplanner.core.mission.survey.Survey;
import org.droidplanner.core.mission.waypoints.Circle;
Expand All @@ -19,7 +20,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("Structure Scan"), CHANGE_SPEED("Change Speed"), CAMERA_TRIGGER("Camera Trigger"), EPM_GRIPPER("EPM");
"Survey"), CYLINDRICAL_SURVEY("Structure Scan"), CHANGE_SPEED("Change Speed"), CAMERA_TRIGGER("Camera Trigger"), EPM_GRIPPER("EPM"), SET_SERVO("Set Servo");

private final String name;

Expand Down Expand Up @@ -57,6 +58,8 @@ public MissionItem getNewItem(MissionItem referenceItem) throws IllegalArgumentE
return new Survey(referenceItem.getMission(), Collections.<Coord2D> emptyList());
case CYLINDRICAL_SURVEY:
return new StructureScanner(referenceItem);
case SET_SERVO:
return new SetServo(referenceItem);
default:
throw new IllegalArgumentException("Unrecognized mission item type (" + name + ")"
+ ".");
Expand Down
72 changes: 72 additions & 0 deletions Core/src/org/droidplanner/core/mission/commands/SetServo.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
package org.droidplanner.core.mission.commands;

import java.util.List;

import org.droidplanner.core.helpers.units.Altitude;
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;

public class SetServo extends MissionCMD {

private int pwm;
private int channel;

public SetServo(MissionItem item) {
super(item);
}

public SetServo(msg_mission_item msg, Mission mission) {
super(mission);
unpackMAVMessage(msg);
}

public SetServo(Mission mission, int channel, int pwm) {
super(mission);
this.channel = channel;
this.pwm = pwm;
}


@Override
public void unpackMAVMessage(msg_mission_item mavMsg) {
channel = (int) mavMsg.param1;
pwm = (int) mavMsg.param2;

}

@Override
public MissionItemType getType() {
return MissionItemType.SET_SERVO;
}

@Override
public List<msg_mission_item> packMissionItem() {
List<msg_mission_item> list = super.packMissionItem();
msg_mission_item mavMsg = list.get(0);
mavMsg.command = MAV_CMD.MAV_CMD_DO_SET_SERVO;
mavMsg.param1 = channel;
mavMsg.param2 = pwm;
return list;
}

public int getPwm() {
return pwm;
}

public int getChannel() {
return channel;
}

public void setPwm(int pwm) {
this.pwm = pwm;
}

public void setChannel(int channel) {
this.channel = channel;
}

}

0 comments on commit 3067b14

Please sign in to comment.