Skip to content

Commit fc75dbe

Browse files
m4gr3dm4gr3d
committed
Added extras attributes to the drone Attitude property.
1 parent 05f6e56 commit fc75dbe

File tree

7 files changed

+113
-76
lines changed

7 files changed

+113
-76
lines changed

‎ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/Attitude.java

Lines changed: 80 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -8,16 +8,45 @@
88
*/
99
public class Attitude implements DroneAttribute {
1010

11-
private double roll;
11+
/**
12+
* Roll angle (deg, -180..+180)
13+
*/
14+
private double roll;
15+
16+
/**
17+
* Roll angular speed (deg/s)
18+
*/
19+
private float rollSpeed;
20+
21+
/**
22+
* Pitch angle (deg, -180 to 180)
23+
*/
1224
private double pitch;
25+
26+
/**
27+
* Pitch angular speed (deg / s)
28+
*/
29+
private float pitchSpeed;
30+
31+
/**
32+
* Yaw angle (deg, -180 to 180)
33+
*/
1334
private double yaw;
1435

36+
/**
37+
* Yaw angular speed (deg/ s)
38+
*/
39+
private float yawSpeed;
40+
1541
public Attitude(){}
1642

17-
public Attitude(double roll, double pitch, double yaw) {
43+
public Attitude(double roll, double pitch, double yaw, float rollSpeed, float pitchSpeed, float yawSpeed) {
1844
this.roll = roll;
1945
this.pitch = pitch;
2046
this.yaw = yaw;
47+
this.rollSpeed = rollSpeed;
48+
this.pitchSpeed = pitchSpeed;
49+
this.yawSpeed = yawSpeed;
2150
}
2251

2352
public void setRoll(double roll) {
@@ -44,18 +73,56 @@ public double getYaw() {
4473
return yaw;
4574
}
4675

76+
public float getPitchSpeed() {
77+
return pitchSpeed;
78+
}
79+
80+
public void setPitchSpeed(float pitchSpeed) {
81+
this.pitchSpeed = pitchSpeed;
82+
}
83+
84+
public float getRollSpeed() {
85+
return rollSpeed;
86+
}
87+
88+
public void setRollSpeed(float rollSpeed) {
89+
this.rollSpeed = rollSpeed;
90+
}
91+
92+
public float getYawSpeed() {
93+
return yawSpeed;
94+
}
95+
96+
public void setYawSpeed(float yawSpeed) {
97+
this.yawSpeed = yawSpeed;
98+
}
99+
100+
@Override
101+
public String toString() {
102+
return "Attitude{" +
103+
"pitch=" + pitch +
104+
", roll=" + roll +
105+
", rollSpeed=" + rollSpeed +
106+
", pitchSpeed=" + pitchSpeed +
107+
", yaw=" + yaw +
108+
", yawSpeed=" + yawSpeed +
109+
'}';
110+
}
111+
47112
@Override
48113
public boolean equals(Object o) {
49114
if (this == o) return true;
50115
if (!(o instanceof Attitude)) return false;
51116

52117
Attitude attitude = (Attitude) o;
53118

54-
if (Double.compare(attitude.pitch, pitch) != 0) return false;
55119
if (Double.compare(attitude.roll, roll) != 0) return false;
120+
if (Float.compare(attitude.rollSpeed, rollSpeed) != 0) return false;
121+
if (Double.compare(attitude.pitch, pitch) != 0) return false;
122+
if (Float.compare(attitude.pitchSpeed, pitchSpeed) != 0) return false;
56123
if (Double.compare(attitude.yaw, yaw) != 0) return false;
124+
return Float.compare(attitude.yawSpeed, yawSpeed) == 0;
57125

58-
return true;
59126
}
60127

61128
@Override
@@ -64,22 +131,16 @@ public int hashCode() {
64131
long temp;
65132
temp = Double.doubleToLongBits(roll);
66133
result = (int) (temp ^ (temp >>> 32));
134+
result = 31 * result + (rollSpeed != +0.0f ? Float.floatToIntBits(rollSpeed) : 0);
67135
temp = Double.doubleToLongBits(pitch);
68136
result = 31 * result + (int) (temp ^ (temp >>> 32));
137+
result = 31 * result + (pitchSpeed != +0.0f ? Float.floatToIntBits(pitchSpeed) : 0);
69138
temp = Double.doubleToLongBits(yaw);
70139
result = 31 * result + (int) (temp ^ (temp >>> 32));
140+
result = 31 * result + (yawSpeed != +0.0f ? Float.floatToIntBits(yawSpeed) : 0);
71141
return result;
72142
}
73143

74-
@Override
75-
public String toString() {
76-
return "Attitude{" +
77-
"roll=" + roll +
78-
", pitch=" + pitch +
79-
", yaw=" + yaw +
80-
'}';
81-
}
82-
83144

84145
@Override
85146
public int describeContents() {
@@ -91,12 +152,18 @@ public void writeToParcel(Parcel dest, int flags) {
91152
dest.writeDouble(this.roll);
92153
dest.writeDouble(this.pitch);
93154
dest.writeDouble(this.yaw);
155+
dest.writeFloat(this.rollSpeed);
156+
dest.writeFloat(this.pitchSpeed);
157+
dest.writeFloat(this.yawSpeed);
94158
}
95159

96160
private Attitude(Parcel in) {
97161
this.roll = in.readDouble();
98162
this.pitch = in.readDouble();
99163
this.yaw = in.readDouble();
164+
this.rollSpeed = in.readFloat();
165+
this.pitchSpeed = in.readFloat();
166+
this.yawSpeed = in.readFloat();
100167
}
101168

102169
public static final Parcelable.Creator<Attitude> CREATOR = new Parcelable.Creator<Attitude>() {

‎ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/MavLinkDrone.java

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,6 @@
1818
import org.droidplanner.services.android.core.drone.variables.Magnetometer;
1919
import org.droidplanner.services.android.core.drone.variables.MissionStats;
2020
import org.droidplanner.services.android.core.drone.variables.Navigation;
21-
import org.droidplanner.services.android.core.drone.variables.Orientation;
2221
import org.droidplanner.services.android.core.drone.variables.RC;
2322
import org.droidplanner.services.android.core.drone.variables.Speed;
2423
import org.droidplanner.services.android.core.drone.variables.State;
@@ -78,8 +77,6 @@ public interface MavLinkDrone extends Drone {
7877

7978
public Altitude getAltitude();
8079

81-
public Orientation getOrientation();
82-
8380
public Navigation getNavigation();
8481

8582
public Mission getMission();

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

Lines changed: 17 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,7 @@
4545
import com.o3dr.services.android.lib.drone.attribute.AttributeType;
4646
import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError;
4747
import com.o3dr.services.android.lib.drone.mission.action.MissionActions;
48+
import com.o3dr.services.android.lib.drone.property.Attitude;
4849
import com.o3dr.services.android.lib.drone.property.DroneAttribute;
4950
import com.o3dr.services.android.lib.drone.property.Signal;
5051
import com.o3dr.services.android.lib.drone.property.VehicleMode;
@@ -77,7 +78,6 @@
7778
import org.droidplanner.services.android.core.drone.variables.Magnetometer;
7879
import org.droidplanner.services.android.core.drone.variables.MissionStats;
7980
import org.droidplanner.services.android.core.drone.variables.Navigation;
80-
import org.droidplanner.services.android.core.drone.variables.Orientation;
8181
import org.droidplanner.services.android.core.drone.variables.RC;
8282
import org.droidplanner.services.android.core.drone.variables.Speed;
8383
import org.droidplanner.services.android.core.drone.variables.State;
@@ -111,7 +111,6 @@ public abstract class ArduPilot implements MavLinkDrone {
111111
private final MissionStats missionStats;
112112
private final StreamRates streamRates;
113113
private final Altitude altitude;
114-
private final Orientation orientation;
115114
private final Navigation navigation;
116115
private final GuidedPoint guidedPoint;
117116
private final AccelCalibration accelCalibrationSetup;
@@ -131,6 +130,7 @@ public abstract class ArduPilot implements MavLinkDrone {
131130

132131
private final Context context;
133132

133+
protected final Attitude attitude = new Attitude();
134134
protected final Signal signal = new Signal();
135135

136136
public ArduPilot(Context context, MAVLinkStreams.MAVLinkOutputStream mavClient,
@@ -159,7 +159,6 @@ public ArduPilot(Context context, MAVLinkStreams.MAVLinkOutputStream mavClient,
159159
this.missionStats = new MissionStats(this);
160160
this.streamRates = new StreamRates(this);
161161
this.altitude = new Altitude(this);
162-
this.orientation = new Orientation(this);
163162
this.navigation = new Navigation(this);
164163
this.guidedPoint = new GuidedPoint(this, handler);
165164
this.accelCalibrationSetup = new AccelCalibration(this, handler);
@@ -330,11 +329,6 @@ public Altitude getAltitude() {
330329
return altitude;
331330
}
332331

333-
@Override
334-
public Orientation getOrientation() {
335-
return orientation;
336-
}
337-
338332
@Override
339333
public Navigation getNavigation() {
340334
return navigation;
@@ -400,7 +394,7 @@ public DroneAttribute getAttribute(String attributeType) {
400394
return CommonApiUtils.getSpeed(this);
401395

402396
case AttributeType.ATTITUDE:
403-
return CommonApiUtils.getAttitude(this);
397+
return attitude;
404398

405399
case AttributeType.HOME:
406400
return CommonApiUtils.getHome(this);
@@ -654,8 +648,7 @@ public void onMavLinkMessageReceived(MAVLinkMessage message) {
654648

655649
case msg_attitude.MAVLINK_MSG_ID_ATTITUDE:
656650
msg_attitude m_att = (msg_attitude) message;
657-
getOrientation().setRollPitchYaw(m_att.roll * 180.0 / Math.PI,
658-
m_att.pitch * 180.0 / Math.PI, m_att.yaw * 180.0 / Math.PI);
651+
processAttitude(m_att);
659652
break;
660653

661654
case msg_vfr_hud.MAVLINK_MSG_ID_VFR_HUD:
@@ -752,6 +745,19 @@ public void onMavLinkMessageReceived(MAVLinkMessage message) {
752745
}
753746
}
754747

748+
private void processAttitude(msg_attitude m_att) {
749+
attitude.setRoll(CommonApiUtils.fromRadToDeg(m_att.roll));
750+
attitude.setRollSpeed(CommonApiUtils.fromRadToDeg(m_att.rollspeed));
751+
752+
attitude.setPitch(CommonApiUtils.fromRadToDeg(m_att.pitch));
753+
attitude.setPitchSpeed(CommonApiUtils.fromRadToDeg(m_att.pitchspeed));
754+
755+
attitude.setYaw(CommonApiUtils.fromRadToDeg(m_att.yaw));
756+
attitude.setYawSpeed(CommonApiUtils.fromRadToDeg(m_att.yawspeed));
757+
758+
notifyDroneEvent(DroneInterfaces.DroneEventsType.ATTITUDE);
759+
}
760+
755761
/**
756762
* Used to update the vehicle location.
757763
* @param gpi

‎ServiceApp/src/org/droidplanner/services/android/core/drone/variables/Camera.java

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,8 @@
1212

1313
import com.MAVLink.ardupilotmega.msg_camera_feedback;
1414
import com.MAVLink.ardupilotmega.msg_mount_status;
15+
import com.o3dr.services.android.lib.drone.attribute.AttributeType;
16+
import com.o3dr.services.android.lib.drone.property.Attitude;
1517

1618
public class Camera extends DroneVariable {
1719
private CameraInfo camera = new CameraInfo();
@@ -43,9 +45,11 @@ public Footprint getCurrentFieldOfView() {
4345
double altitude = myDrone.getAltitude().getAltitude();
4446
Coord2D position = myDrone.getGps().getPosition();
4547
//double pitch = myDrone.getOrientation().getPitch() - gimbal_pitch;
46-
double pitch = myDrone.getOrientation().getPitch();
47-
double roll = myDrone.getOrientation().getRoll();
48-
double yaw = myDrone.getOrientation().getYaw();
48+
49+
final Attitude attitude = (Attitude) myDrone.getAttribute(AttributeType.ATTITUDE);
50+
double pitch = attitude.getPitch();
51+
double roll = attitude.getRoll();
52+
double yaw = attitude.getYaw();
4953
return new Footprint(camera, position, altitude, pitch, roll, yaw);
5054
}
5155

‎ServiceApp/src/org/droidplanner/services/android/core/drone/variables/Orientation.java

Lines changed: 0 additions & 37 deletions
This file was deleted.

‎ServiceApp/src/org/droidplanner/services/android/core/mission/Mission.java

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,8 @@
55
import com.MAVLink.common.msg_mission_ack;
66
import com.MAVLink.common.msg_mission_item;
77
import com.MAVLink.enums.MAV_CMD;
8+
import com.o3dr.services.android.lib.drone.attribute.AttributeType;
9+
import com.o3dr.services.android.lib.drone.property.Attitude;
810

911
import org.droidplanner.services.android.core.drone.DroneInterfaces.DroneEventsType;
1012
import org.droidplanner.services.android.core.drone.DroneVariable;
@@ -373,7 +375,8 @@ public double makeAndUploadDronie() {
373375
return -1;
374376
}
375377

376-
final double bearing = 180 + myDrone.getOrientation().getYaw();
378+
final Attitude attitude = (Attitude) myDrone.getAttribute(AttributeType.ATTITUDE);
379+
final double bearing = 180 + attitude.getYaw();
377380
items.clear();
378381
items.addAll(createDronie(currentPosition,
379382
GeoTools.newCoordFromBearingAndDistance(currentPosition, bearing, 50.0)));

‎ServiceApp/src/org/droidplanner/services/android/utils/CommonApiUtils.java

Lines changed: 5 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,6 @@
2727
import com.o3dr.services.android.lib.drone.mission.item.complex.StructureScanner;
2828
import com.o3dr.services.android.lib.drone.mission.item.complex.Survey;
2929
import com.o3dr.services.android.lib.drone.property.Altitude;
30-
import com.o3dr.services.android.lib.drone.property.Attitude;
3130
import com.o3dr.services.android.lib.drone.property.Battery;
3231
import com.o3dr.services.android.lib.drone.property.CameraProxy;
3332
import com.o3dr.services.android.lib.drone.property.EkfStatus;
@@ -56,7 +55,6 @@
5655
import org.droidplanner.services.android.core.drone.variables.Camera;
5756
import org.droidplanner.services.android.core.drone.variables.GPS;
5857
import org.droidplanner.services.android.core.drone.variables.GuidedPoint;
59-
import org.droidplanner.services.android.core.drone.variables.Orientation;
6058
import org.droidplanner.services.android.core.drone.variables.calibration.AccelCalibration;
6159
import org.droidplanner.services.android.core.drone.variables.calibration.MagnetometerCalibrationImpl;
6260
import org.droidplanner.services.android.core.gcs.follow.Follow;
@@ -465,13 +463,12 @@ public static Speed getSpeed(MavLinkDrone drone) {
465463
return new Speed(droneSpeed.getVerticalSpeed(), droneSpeed.getGroundSpeed(), droneSpeed.getAirSpeed());
466464
}
467465

468-
public static Attitude getAttitude(MavLinkDrone drone) {
469-
if (drone == null)
470-
return new Attitude();
466+
public static float fromRadToDeg(float rad){
467+
return (float) (rad * 180f / Math.PI);
468+
}
471469

472-
Orientation droneOrientation = drone.getOrientation();
473-
return new Attitude(droneOrientation.getRoll(), droneOrientation.getPitch(),
474-
droneOrientation.getYaw());
470+
public static float fromDegToRad(float deg){
471+
return (float) (deg * Math.PI / 180f);
475472
}
476473

477474
public static Home getHome(MavLinkDrone drone) {

0 commit comments

Comments
 (0)