|
| 1 | +package org.droidplanner.services.android.core.drone.autopilot; |
| 2 | + |
| 3 | +import android.text.TextUtils; |
| 4 | + |
| 5 | +import com.MAVLink.Messages.MAVLinkMessage; |
| 6 | +import com.MAVLink.common.msg_attitude; |
| 7 | +import com.MAVLink.common.msg_heartbeat; |
| 8 | +import com.MAVLink.common.msg_radio_status; |
| 9 | +import com.o3dr.services.android.lib.drone.attribute.AttributeType; |
| 10 | +import com.o3dr.services.android.lib.drone.property.Altitude; |
| 11 | +import com.o3dr.services.android.lib.drone.property.Attitude; |
| 12 | +import com.o3dr.services.android.lib.drone.property.Battery; |
| 13 | +import com.o3dr.services.android.lib.drone.property.DroneAttribute; |
| 14 | +import com.o3dr.services.android.lib.drone.property.Signal; |
| 15 | +import com.o3dr.services.android.lib.drone.property.Speed; |
| 16 | +import com.o3dr.services.android.lib.util.MathUtils; |
| 17 | + |
| 18 | +import org.droidplanner.services.android.core.MAVLink.MAVLinkStreams; |
| 19 | +import org.droidplanner.services.android.core.drone.DroneEvents; |
| 20 | +import org.droidplanner.services.android.core.drone.DroneInterfaces; |
| 21 | +import org.droidplanner.services.android.core.drone.variables.Type; |
| 22 | +import org.droidplanner.services.android.utils.CommonApiUtils; |
| 23 | + |
| 24 | +/** |
| 25 | + * Base drone implementation. |
| 26 | + * Supports mavlink messages belonging to the common set: https://pixhawk.ethz.ch/mavlink/ |
| 27 | + * |
| 28 | + * Created by Fredia Huya-Kouadio on 9/10/15. |
| 29 | + */ |
| 30 | +public abstract class CommonMavLinkDrone implements MavLinkDrone { |
| 31 | + |
| 32 | + private final MAVLinkStreams.MAVLinkOutputStream MavClient; |
| 33 | + private final DroneEvents events; |
| 34 | + protected final Type type; |
| 35 | + |
| 36 | + protected final Altitude altitude = new Altitude(); |
| 37 | + protected final Speed speed = new Speed(); |
| 38 | + protected final Battery battery = new Battery(); |
| 39 | + protected final Signal signal = new Signal(); |
| 40 | + protected final Attitude attitude = new Attitude(); |
| 41 | + |
| 42 | + protected CommonMavLinkDrone(DroneInterfaces.Handler handler, MAVLinkStreams.MAVLinkOutputStream mavClient) { |
| 43 | + this.MavClient = mavClient; |
| 44 | + |
| 45 | + events = new DroneEvents(this, handler); |
| 46 | + this.type = new Type(this); |
| 47 | + } |
| 48 | + |
| 49 | + @Override |
| 50 | + public boolean isConnected() { |
| 51 | + return MavClient.isConnected(); |
| 52 | + } |
| 53 | + |
| 54 | + @Override |
| 55 | + public void addDroneListener(DroneInterfaces.OnDroneListener listener) { |
| 56 | + events.addDroneListener(listener); |
| 57 | + } |
| 58 | + |
| 59 | + @Override |
| 60 | + public void removeDroneListener(DroneInterfaces.OnDroneListener listener) { |
| 61 | + events.removeDroneListener(listener); |
| 62 | + } |
| 63 | + |
| 64 | + @Override |
| 65 | + public void notifyDroneEvent(final DroneInterfaces.DroneEventsType event) { |
| 66 | + switch (event) { |
| 67 | + case DISCONNECTED: |
| 68 | + signal.setValid(false); |
| 69 | + break; |
| 70 | + } |
| 71 | + |
| 72 | + events.notifyDroneEvent(event); |
| 73 | + } |
| 74 | + |
| 75 | + @Override |
| 76 | + public MAVLinkStreams.MAVLinkOutputStream getMavClient() { |
| 77 | + return MavClient; |
| 78 | + } |
| 79 | + |
| 80 | + @Override |
| 81 | + public DroneAttribute getAttribute(String attributeType) { |
| 82 | + if (TextUtils.isEmpty(attributeType)) |
| 83 | + return null; |
| 84 | + |
| 85 | + switch (attributeType) { |
| 86 | + case AttributeType.SPEED: |
| 87 | + return speed; |
| 88 | + |
| 89 | + case AttributeType.BATTERY: |
| 90 | + return battery; |
| 91 | + |
| 92 | + case AttributeType.SIGNAL: |
| 93 | + return signal; |
| 94 | + |
| 95 | + case AttributeType.ATTITUDE: |
| 96 | + return attitude; |
| 97 | + |
| 98 | + case AttributeType.ALTITUDE: |
| 99 | + return altitude; |
| 100 | + } |
| 101 | + |
| 102 | + return null; |
| 103 | + } |
| 104 | + |
| 105 | + @Override |
| 106 | + public void onMavLinkMessageReceived(MAVLinkMessage message) { |
| 107 | + switch (message.msgid) { |
| 108 | + case msg_radio_status.MAVLINK_MSG_ID_RADIO_STATUS: |
| 109 | + msg_radio_status m_radio_status = (msg_radio_status) message; |
| 110 | + processSignalUpdate(m_radio_status.rxerrors, m_radio_status.fixed, m_radio_status.rssi, |
| 111 | + m_radio_status.remrssi, m_radio_status.txbuf, m_radio_status.noise, m_radio_status.remnoise); |
| 112 | + break; |
| 113 | + |
| 114 | + case msg_attitude.MAVLINK_MSG_ID_ATTITUDE: |
| 115 | + msg_attitude m_att = (msg_attitude) message; |
| 116 | + processAttitude(m_att); |
| 117 | + break; |
| 118 | + |
| 119 | + case msg_heartbeat.MAVLINK_MSG_ID_HEARTBEAT: |
| 120 | + msg_heartbeat msg_heart = (msg_heartbeat) message; |
| 121 | + setType(msg_heart.type); |
| 122 | + break; |
| 123 | + } |
| 124 | + } |
| 125 | + |
| 126 | + protected void setType(int type) { |
| 127 | + this.type.setType(type); |
| 128 | + } |
| 129 | + |
| 130 | + @Override |
| 131 | + public int getType() { |
| 132 | + return type.getType(); |
| 133 | + } |
| 134 | + |
| 135 | + private void processAttitude(msg_attitude m_att) { |
| 136 | + attitude.setRoll(CommonApiUtils.fromRadToDeg(m_att.roll)); |
| 137 | + attitude.setRollSpeed(CommonApiUtils.fromRadToDeg(m_att.rollspeed)); |
| 138 | + |
| 139 | + attitude.setPitch(CommonApiUtils.fromRadToDeg(m_att.pitch)); |
| 140 | + attitude.setPitchSpeed(CommonApiUtils.fromRadToDeg(m_att.pitchspeed)); |
| 141 | + |
| 142 | + attitude.setYaw(CommonApiUtils.fromRadToDeg(m_att.yaw)); |
| 143 | + attitude.setYawSpeed(CommonApiUtils.fromRadToDeg(m_att.yawspeed)); |
| 144 | + |
| 145 | + notifyDroneEvent(DroneInterfaces.DroneEventsType.ATTITUDE); |
| 146 | + } |
| 147 | + |
| 148 | + protected void processSignalUpdate(int rxerrors, int fixed, short rssi, short remrssi, short txbuf, |
| 149 | + short noise, short remnoise) { |
| 150 | + signal.setValid(true); |
| 151 | + signal.setRxerrors(rxerrors & 0xFFFF); |
| 152 | + signal.setFixed(fixed & 0xFFFF); |
| 153 | + signal.setRssi(SikValueToDB(rssi & 0xFF)); |
| 154 | + signal.setRemrssi(SikValueToDB(remrssi & 0xFF)); |
| 155 | + signal.setNoise(SikValueToDB(noise & 0xFF)); |
| 156 | + signal.setRemnoise(SikValueToDB(remnoise & 0xFF)); |
| 157 | + signal.setTxbuf(txbuf & 0xFF); |
| 158 | + |
| 159 | + signal.setSignalStrength(MathUtils.getSignalStrength(signal.getFadeMargin(), signal.getRemFadeMargin())); |
| 160 | + |
| 161 | + notifyDroneEvent(DroneInterfaces.DroneEventsType.RADIO); |
| 162 | + } |
| 163 | + |
| 164 | + /** |
| 165 | + * Scalling done at the Si1000 radio More info can be found at: |
| 166 | + * http://copter.ardupilot.com/wiki/common-using-the-3dr-radio-for-telemetry-with-apm-and-px4/#Power_levels |
| 167 | + */ |
| 168 | + protected double SikValueToDB(int value) { |
| 169 | + return (value / 1.9) - 127; |
| 170 | + } |
| 171 | + |
| 172 | +} |
0 commit comments