forked from dronekit/dronekit-android
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
updated mavlink library with the latest definitions files.
added experimentation spline follow modes.
- Loading branch information
Showing
33 changed files
with
527 additions
and
207 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Binary file not shown.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
121 changes: 74 additions & 47 deletions
121
dependencyLibs/Core/src/org/droidplanner/core/MAVLink/MavLinkModes.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,57 +1,84 @@ | ||
| package org.droidplanner.core.MAVLink; | ||
|
|
||
| import org.droidplanner.core.model.Drone; | ||
|
|
||
| import com.MAVLink.Messages.ApmModes; | ||
| import com.MAVLink.common.msg_mission_item; | ||
| import com.MAVLink.common.msg_set_position_target_global_int; | ||
| import com.MAVLink.common.msg_set_mode; | ||
| import com.MAVLink.enums.MAV_CMD; | ||
| import com.MAVLink.enums.MAV_FRAME; | ||
|
|
||
| import org.droidplanner.core.model.Drone; | ||
|
|
||
| public class MavLinkModes { | ||
| public static void setGuidedMode(Drone drone, double latitude, double longitude, double d) { | ||
| msg_mission_item msg = new msg_mission_item(); | ||
| msg.seq = 0; | ||
| msg.current = 2; // TODO use guided mode enum | ||
| msg.frame = MAV_FRAME.MAV_FRAME_GLOBAL; | ||
| msg.command = MAV_CMD.MAV_CMD_NAV_WAYPOINT; // | ||
| msg.param1 = 0; // TODO use correct parameter | ||
| msg.param2 = 0; // TODO use correct parameter | ||
| msg.param3 = 0; // TODO use correct parameter | ||
| msg.param4 = 0; // TODO use correct parameter | ||
| msg.x = (float) latitude; | ||
| msg.y = (float) longitude; | ||
| msg.z = (float) d; | ||
| msg.autocontinue = 1; // TODO use correct parameter | ||
| msg.target_system = drone.getSysid(); | ||
| msg.target_component = drone.getCompid(); | ||
| drone.getMavClient().sendMavPacket(msg.pack()); | ||
| } | ||
|
|
||
| public static void sendGuidedVelocity(Drone drone, double xVel, double yVel, double zVel) { | ||
| msg_mission_item msg = new msg_mission_item(); | ||
| msg.seq = 0; | ||
| msg.current = 2; // TODO use guided mode enum | ||
| msg.frame = MAV_FRAME.MAV_FRAME_GLOBAL; | ||
| msg.command = 91; // MAV_CMD_NAV_VELOCITY | ||
| msg.param1 = 0; // TODO use correct parameter | ||
| msg.param2 = 0; // TODO use correct parameter | ||
| msg.param3 = 0; // TODO use correct parameter | ||
| msg.param4 = 0; // TODO use correct parameter | ||
| msg.x = (float) (xVel ); | ||
| msg.y = (float) (yVel); | ||
| msg.z = (float) (zVel); | ||
| msg.autocontinue = 1; // TODO use correct parameter | ||
| msg.target_system = drone.getSysid(); | ||
| msg.target_component = drone.getCompid(); | ||
| drone.getMavClient().sendMavPacket(msg.pack()); | ||
| } | ||
|
|
||
| public static void changeFlightMode(Drone drone, ApmModes mode) { | ||
| msg_set_mode msg = new msg_set_mode(); | ||
| msg.target_system = drone.getSysid(); | ||
| msg.base_mode = 1; // TODO use meaningful constant | ||
| msg.custom_mode = mode.getNumber(); | ||
| drone.getMavClient().sendMavPacket(msg.pack()); | ||
| } | ||
|
|
||
| private static final int MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = ((1 << 0) | (1 << 1) | (1 << 2)); | ||
| private static final int MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE = ((1 << 3) | (1 << 4) | (1 << 5)); | ||
| private static final int MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE = ((1 << 6) | (1 << 7) | (1 << 8)); | ||
|
|
||
| public static void setGuidedMode(Drone drone, double latitude, double longitude, double d) { | ||
| msg_mission_item msg = new msg_mission_item(); | ||
| msg.seq = 0; | ||
| msg.current = 2; // TODO use guided mode enum | ||
| msg.frame = MAV_FRAME.MAV_FRAME_GLOBAL; | ||
| msg.command = MAV_CMD.MAV_CMD_NAV_WAYPOINT; // | ||
| msg.param1 = 0; // TODO use correct parameter | ||
| msg.param2 = 0; // TODO use correct parameter | ||
| msg.param3 = 0; // TODO use correct parameter | ||
| msg.param4 = 0; // TODO use correct parameter | ||
| msg.x = (float) latitude; | ||
| msg.y = (float) longitude; | ||
| msg.z = (float) d; | ||
| msg.autocontinue = 1; // TODO use correct parameter | ||
| msg.target_system = drone.getSysid(); | ||
| msg.target_component = drone.getCompid(); | ||
| drone.getMavClient().sendMavPacket(msg.pack()); | ||
| } | ||
|
|
||
| public static void sendGuidedPosition(Drone drone, double latitude, double longitude, double altitude){ | ||
| msg_set_position_target_global_int msg = new msg_set_position_target_global_int(); | ||
| msg.type_mask = MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE | MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; | ||
| msg.coordinate_frame = MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; | ||
| msg.lat_int = (int) (latitude * 1E7); | ||
| msg.lon_int = (int) (longitude * 1E7); | ||
| msg.alt = (float) altitude; | ||
| msg.target_system = drone.getSysid(); | ||
| msg.target_component = drone.getCompid(); | ||
| drone.getMavClient().sendMavPacket(msg.pack()); | ||
| } | ||
|
|
||
| public static void sendGuidedVelocity(Drone drone, double xVel, double yVel, double zVel){ | ||
| msg_set_position_target_global_int msg = new msg_set_position_target_global_int(); | ||
| msg.type_mask = MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE | MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; | ||
| msg.coordinate_frame = MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; | ||
| msg.vx = (float) xVel; | ||
| msg.vy = (float) yVel; | ||
| msg.vz = (float) zVel; | ||
| msg.target_system = drone.getSysid(); | ||
| msg.target_component = drone.getCompid(); | ||
| drone.getMavClient().sendMavPacket(msg.pack()); | ||
| } | ||
|
|
||
| public static void sendGuidedPositionAndVelocity(Drone drone, double latitude, double longitude, double altitude, | ||
| double xVel, double yVel, double zVel){ | ||
| msg_set_position_target_global_int msg = new msg_set_position_target_global_int(); | ||
| msg.type_mask = MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; | ||
| msg.coordinate_frame = MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; | ||
| msg.lat_int = (int) (latitude * 1E7); | ||
| msg.lon_int = (int) (longitude * 1E7); | ||
| msg.alt = (float) altitude; | ||
| msg.vx = (float) xVel; | ||
| msg.vy = (float) yVel; | ||
| msg.vz = (float) zVel; | ||
| msg.target_system = drone.getSysid(); | ||
| msg.target_component = drone.getCompid(); | ||
| drone.getMavClient().sendMavPacket(msg.pack()); | ||
| } | ||
|
|
||
| public static void changeFlightMode(Drone drone, ApmModes mode) { | ||
| msg_set_mode msg = new msg_set_mode(); | ||
| msg.target_system = drone.getSysid(); | ||
| msg.base_mode = 1; // TODO use meaningful constant | ||
| msg.custom_mode = mode.getNumber(); | ||
| drone.getMavClient().sendMavPacket(msg.pack()); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
31 changes: 31 additions & 0 deletions
31
dependencyLibs/Core/src/org/droidplanner/core/gcs/follow/FollowSplineAbove.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,31 @@ | ||
| package org.droidplanner.core.gcs.follow; | ||
|
|
||
| import org.droidplanner.core.gcs.location.Location; | ||
| import org.droidplanner.core.helpers.coordinates.Coord2D; | ||
| import org.droidplanner.core.helpers.units.Length; | ||
| import org.droidplanner.core.model.Drone; | ||
|
|
||
| /** | ||
| * Created by fhuya on 1/5/15. | ||
| */ | ||
| public class FollowSplineAbove extends FollowAlgorithm { | ||
| @Override | ||
| public void processNewLocation(Location location) { | ||
| Coord2D gcsLoc = new Coord2D(location.getCoord().getLat(), location.getCoord().getLng()); | ||
| double speed = location.getSpeed(); | ||
| double bearing = location.getBearing(); | ||
| double bearingInRad = Math.toRadians(bearing); | ||
| double xVel = speed * Math.cos(bearingInRad); | ||
| double yVel = speed * Math.sin(bearingInRad); | ||
| drone.getGuidedPoint().newGuidedPositionAndVelocity(gcsLoc, xVel, yVel, 0); | ||
| } | ||
|
|
||
| @Override | ||
| public FollowModes getType() { | ||
| return FollowModes.SPLINE_ABOVE; | ||
| } | ||
|
|
||
| public FollowSplineAbove(Drone drone, Length length) { | ||
| super(drone, length); | ||
| } | ||
| } |
Oops, something went wrong.