Skip to content

Commit 5507d0d

Browse files
committed
multiple fixes to emergency disarm command
1 parent 0197d5a commit 5507d0d

File tree

5 files changed

+12
-8
lines changed

5 files changed

+12
-8
lines changed

ClientLib/src/main/java/com/o3dr/android/client/Drone.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -481,7 +481,7 @@ public void generateDronie() {
481481
}
482482

483483
public void arm(boolean arm) {
484-
DroneStateApi.arm(this, arm, false);
484+
DroneStateApi.arm(this, arm);
485485
}
486486

487487
/**

ClientLib/src/main/java/com/o3dr/android/client/apis/drone/DroneStateApi.java

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -23,10 +23,7 @@ public class DroneStateApi {
2323
* @param arm true to arm, false to disarm.
2424
*/
2525
public static void arm(Drone drone, boolean arm) {
26-
Bundle params = new Bundle();
27-
params.putBoolean(EXTRA_ARM, arm);
28-
params.putBoolean(EXTRA_EMERGENCY_DISARM, false);
29-
drone.performAsyncAction(new Action(ACTION_ARM, params));
26+
arm(drone, arm, false);
3027
}
3128

3229
/**

ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -400,7 +400,8 @@ public void performAction(Action action) throws RemoteException {
400400
//DRONE STATE ACTIONS
401401
case StateActions.ACTION_ARM:
402402
boolean doArm = data.getBoolean(StateActions.EXTRA_ARM);
403-
DroneApiUtils.arm(getDrone(), doArm);
403+
boolean emergencyDisarm = data.getBoolean(StateActions.EXTRA_EMERGENCY_DISARM);
404+
DroneApiUtils.arm(getDrone(), doArm, emergencyDisarm);
404405
break;
405406

406407
case StateActions.ACTION_SET_VEHICLE_MODE:

ServiceApp/src/org/droidplanner/services/android/api/DroneApiUtils.java

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -670,10 +670,14 @@ static float generateDronie(Drone drone) {
670670
return (float) drone.getMission().makeAndUploadDronie();
671671
}
672672

673+
static void arm(Drone drone, boolean arm) {
674+
arm(drone, arm, false);
675+
}
676+
673677
static void arm(Drone drone, boolean arm, boolean emergencyDisarm) {
674678
if (drone == null)
675679
return;
676-
MavLinkArm.sendArmMessage(drone, arm, false);
680+
MavLinkArm.sendArmMessage(drone, arm, emergencyDisarm);
677681
}
678682

679683
static void startMagnetometerCalibration(Drone drone, boolean retryOnFailure, boolean saveAutomatically, int

dependencyLibs/Core/src/org/droidplanner/core/MAVLink/MavLinkArm.java

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,14 +7,16 @@
77

88
public class MavLinkArm {
99

10+
public static final int EMERGENCY_DISARM_MAGIC_NUMBER = 21196;
11+
1012
public static void sendArmMessage(Drone drone, boolean arm, boolean emergencyDisarm) {
1113
msg_command_long msg = new msg_command_long();
1214
msg.target_system = drone.getSysid();
1315
msg.target_component = drone.getCompid();
1416

1517
msg.command = MAV_CMD.MAV_CMD_COMPONENT_ARM_DISARM;
1618
msg.param1 = arm ? 1 : 0;
17-
msg.param2 = emergencyDisarm ? 21196 : 0;//21196 is a magic number to disarm even if flying
19+
msg.param2 = emergencyDisarm ? EMERGENCY_DISARM_MAGIC_NUMBER : 0;
1820
msg.param3 = 0;
1921
msg.param4 = 0;
2022
msg.param5 = 0;

0 commit comments

Comments
 (0)