Skip to content

improved detection of bebop vehicles. #335

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
m4gr3d merged 1 commit into from
Nov 8, 2015
Merged

improved detection of bebop vehicles. #335

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
53 changes: 16 additions & 37 deletions ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/ArduPilot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,10 +22,12 @@
import com.MAVLink.common.msg_rc_channels_raw;
import com.MAVLink.common.msg_servo_output_raw;
import com.MAVLink.common.msg_statustext;
import com.MAVLink.common.msg_sys_status;
import com.MAVLink.common.msg_vfr_hud;
import com.MAVLink.enums.MAV_MODE_FLAG;
import com.MAVLink.enums.MAV_MOUNT_MODE;
import com.MAVLink.enums.MAV_STATE;
import com.MAVLink.enums.MAV_SYS_STATUS_SENSOR;
import com.o3dr.services.android.lib.coordinate.LatLong;
import com.o3dr.services.android.lib.coordinate.LatLongAlt;
import com.o3dr.services.android.lib.drone.action.ControlActions;
Expand Down Expand Up @@ -399,13 +401,6 @@ public void onMavLinkMessageReceived(MAVLinkMessage message) {
getCalibrationSetup().processMessage(message);

switch (message.msgid) {
case msg_heartbeat.MAVLINK_MSG_ID_HEARTBEAT:
msg_heartbeat msg_heart = (msg_heartbeat) message;
checkIfFlying(msg_heart);
processState(msg_heart);
ApmModes newMode = ApmModes.getMode(msg_heart.custom_mode, getType());
getState().setMode(newMode);
break;

case msg_statustext.MAVLINK_MSG_ID_STATUSTEXT:
// These are any warnings sent from APM:Copter with
Expand Down Expand Up @@ -467,6 +462,20 @@ public void onMavLinkMessageReceived(MAVLinkMessage message) {
super.onMavLinkMessageReceived(message);
}

@Override
protected void processSysStatus(msg_sys_status m_sys){
super.processSysStatus(m_sys);
checkControlSensorsHealth(m_sys);
}

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;
if (isRCFailsafe) {
getState().parseAutopilotError("RC FAILSAFE");
}
}

protected void processVfrHud(msg_vfr_hud vfrHud) {
if (vfrHud == null)
return;
Expand Down Expand Up @@ -502,36 +511,6 @@ private void processNamedValueInt(msg_named_value_int message) {
}
}

private void checkIfFlying(msg_heartbeat msg_heart) {
short systemStatus = msg_heart.system_status;
boolean wasFlying = getState().isFlying();

boolean isFlying = systemStatus == MAV_STATE.MAV_STATE_ACTIVE
|| (wasFlying
&& (systemStatus == MAV_STATE.MAV_STATE_CRITICAL || systemStatus == MAV_STATE.MAV_STATE_EMERGENCY));

getState().setIsFlying(isFlying);
}

private void processState(msg_heartbeat msg_heart) {
checkArmState(msg_heart);
checkFailsafe(msg_heart);
}

private void checkFailsafe(msg_heartbeat msg_heart) {
boolean failsafe2 = msg_heart.system_status == MAV_STATE.MAV_STATE_CRITICAL
|| msg_heart.system_status == MAV_STATE.MAV_STATE_EMERGENCY;

if (failsafe2) {
getState().repeatWarning();
}
}

private void checkArmState(msg_heartbeat msg_heart) {
getState().setArmed(
(msg_heart.base_mode & MAV_MODE_FLAG.MAV_MODE_FLAG_SAFETY_ARMED) == MAV_MODE_FLAG.MAV_MODE_FLAG_SAFETY_ARMED);
}

protected void processStatusText(msg_statustext statusText) {
String message = statusText.getText();
if (TextUtils.isEmpty(message))
Expand Down
64 changes: 52 additions & 12 deletions ...c/org/droidplanner/services/android/core/drone/autopilot/generic/GenericMavLinkDrone.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@
import com.MAVLink.common.msg_radio_status;
import com.MAVLink.common.msg_sys_status;
import com.MAVLink.common.msg_vibration;
import com.MAVLink.enums.MAV_MODE_FLAG;
import com.MAVLink.enums.MAV_STATE;
import com.MAVLink.enums.MAV_SYS_STATUS_SENSOR;
import com.o3dr.services.android.lib.coordinate.LatLong;
import com.o3dr.services.android.lib.coordinate.LatLongAlt;
Expand Down Expand Up @@ -54,6 +56,7 @@
import org.droidplanner.services.android.core.drone.autopilot.MavLinkDrone;
import org.droidplanner.services.android.core.drone.autopilot.apm.APMConstants;
import org.droidplanner.services.android.core.drone.profiles.ParameterManager;
import org.droidplanner.services.android.core.drone.variables.ApmModes;
import org.droidplanner.services.android.core.drone.variables.Camera;
import org.droidplanner.services.android.core.drone.variables.GuidedPoint;
import org.droidplanner.services.android.core.drone.variables.HeartBeat;
Expand Down Expand Up @@ -417,7 +420,7 @@ public void onMavLinkMessageReceived(MAVLinkMessage message) {

case msg_heartbeat.MAVLINK_MSG_ID_HEARTBEAT:
msg_heartbeat msg_heart = (msg_heartbeat) message;
setType(msg_heart.type);
processHeartbeat(msg_heart);
break;

case msg_vibration.MAVLINK_MSG_ID_VIBRATION:
Expand All @@ -432,9 +435,7 @@ public void onMavLinkMessageReceived(MAVLinkMessage message) {

case msg_sys_status.MAVLINK_MSG_ID_SYS_STATUS:
msg_sys_status m_sys = (msg_sys_status) message;
processBatteryUpdate(m_sys.voltage_battery / 1000.0, m_sys.battery_remaining,
m_sys.current_battery / 100.0);
checkControlSensorsHealth(m_sys);
processSysStatus(m_sys);
break;

case msg_global_position_int.MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
Expand Down Expand Up @@ -464,21 +465,60 @@ public void onMavLinkMessageReceived(MAVLinkMessage message) {
}
}

protected void processSysStatus(msg_sys_status m_sys) {
processBatteryUpdate(m_sys.voltage_battery / 1000.0, m_sys.battery_remaining,
m_sys.current_battery / 100.0);
}

private void processHeartbeat(msg_heartbeat msg_heart) {
setType(msg_heart.type);
checkIfFlying(msg_heart);
processState(msg_heart);
processVehicleMode(msg_heart);
}

private void processVehicleMode(msg_heartbeat msg_heart){
ApmModes newMode = ApmModes.getMode(msg_heart.custom_mode, getType());
state.setMode(newMode);
}

private void processState(msg_heartbeat msg_heart) {
checkArmState(msg_heart);
checkFailsafe(msg_heart);
}

private void checkFailsafe(msg_heartbeat msg_heart) {
boolean failsafe2 = msg_heart.system_status == MAV_STATE.MAV_STATE_CRITICAL
|| msg_heart.system_status == MAV_STATE.MAV_STATE_EMERGENCY;

if (failsafe2) {
state.repeatWarning();
}
}

private void checkArmState(msg_heartbeat msg_heart) {
state.setArmed(
(msg_heart.base_mode & MAV_MODE_FLAG.MAV_MODE_FLAG_SAFETY_ARMED) == MAV_MODE_FLAG.MAV_MODE_FLAG_SAFETY_ARMED);
}

private void checkIfFlying(msg_heartbeat msg_heart) {
short systemStatus = msg_heart.system_status;
boolean wasFlying = getState().isFlying();

boolean isFlying = systemStatus == MAV_STATE.MAV_STATE_ACTIVE
|| (wasFlying
&& (systemStatus == MAV_STATE.MAV_STATE_CRITICAL || systemStatus == MAV_STATE.MAV_STATE_EMERGENCY));

state.setIsFlying(isFlying);
}

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;
if (isRCFailsafe) {
state.parseAutopilotError("RC FAILSAFE");
}
}

public void processHomeUpdate(msg_mission_item missionItem) {
if (missionItem.seq != APMConstants.HOME_WAYPOINT_INDEX) {
return;
Expand Down
6 changes: 5 additions & 1 deletion ServiceApp/src/org/droidplanner/services/android/core/drone/variables/State.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
import com.o3dr.services.android.lib.model.action.Action;

import org.droidplanner.services.android.core.MAVLink.MavLinkCommands;
import org.droidplanner.services.android.core.MAVLink.WaypointManager;
import org.droidplanner.services.android.core.drone.DroneInterfaces.DroneEventsType;
import org.droidplanner.services.android.core.drone.DroneVariable;
import org.droidplanner.services.android.core.drone.autopilot.generic.GenericMavLinkDrone;
Expand Down Expand Up @@ -110,7 +111,10 @@ public void setArmed(boolean newState) {
myDrone.notifyDroneEvent(DroneEventsType.ARMING);

if (newState) {
myDrone.getWaypointManager().getWaypoints();
WaypointManager waypointManager = myDrone.getWaypointManager();
if(waypointManager != null) {
waypointManager.getWaypoints();
}
}
}

Expand Down