Added support for new coord frames
- Cleaned code
Esse commit está contido em:
Christopher Pereira
@@ -2102,13 +2102,11 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
|
||||
public int command = 0; // If not 0, send MAVLink ACK once the motion has finished
|
||||
|
||||
short coordFrame; // See: https://mavlink.io/en/messages/common.html#MAV_FRAME
|
||||
|
||||
DroneModel.PositionTargetTypeMask mask;
|
||||
|
||||
double x; // Lat
|
||||
double y; // Lng
|
||||
double z; // Alt
|
||||
double lat; // Lat
|
||||
double lng; // Lng
|
||||
double alt; // Alt
|
||||
double yaw;
|
||||
|
||||
double speed = 3; // m/s
|
||||
@@ -2124,21 +2122,81 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
// Used internally (not for MAVLink commands)
|
||||
Motion(double lat, double lng, double alt) {
|
||||
type = DroneModel.MotionType.FLY_TO;
|
||||
coordFrame = MAV_FRAME_GLOBAL;
|
||||
this.x = lat;
|
||||
this.y = lng;
|
||||
this.z = alt;
|
||||
mask = new PositionTargetTypeMask(0);
|
||||
this.lat = lat;
|
||||
this.lng = lng;
|
||||
this.alt = alt;
|
||||
}
|
||||
|
||||
// Triggered by a MAVLink commands => sends ACK and does cancelAllTasks()
|
||||
// TODO: Implement mask in MotionTask.run()
|
||||
Motion(int command, int mavLinkMask) {
|
||||
this.command = command;
|
||||
cancelAllTasks();
|
||||
type = DroneModel.MotionType.POSITION_TARGET;
|
||||
this.mask = new DroneModel.PositionTargetTypeMask(mavLinkMask);
|
||||
command = command;
|
||||
mask = new PositionTargetTypeMask(mavLinkMask);
|
||||
type = MotionType.POSITION_TARGET;
|
||||
send_command_ack(command, MAV_RESULT.MAV_RESULT_IN_PROGRESS);
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts (x, y, z, coordFrame) --> (lat, lng, alt)
|
||||
*
|
||||
* @param coordFrame See: https://mavlink.io/en/messages/common.html#MAV_FRAME
|
||||
* @param x Has different meanings
|
||||
* @param y Has different meanings
|
||||
* @param z Has different meanings
|
||||
*/
|
||||
public void setDestination(int coordFrame, float x, float y, float z) {
|
||||
if(coordFrame == MAV_FRAME_BODY_OFFSET_NED) coordFrame = MAV_FRAME_BODY_FRD;
|
||||
|
||||
if(coordFrame == MAV_FRAME_LOCAL_NED || coordFrame == MAV_FRAME_LOCAL_OFFSET_NED || coordFrame == MAV_FRAME_BODY_FRD) {
|
||||
FlightControllerState coord = djiAircraft.getFlightController().getState();
|
||||
this.lat = coord.getAircraftLocation().getLatitude();
|
||||
this.lng = coord.getAircraftLocation().getLongitude();
|
||||
this.alt = coord.getAircraftLocation().getAltitude();
|
||||
}
|
||||
|
||||
if(coordFrame == MAV_FRAME_GLOBAL) {
|
||||
// (x, y, z) = (lat, lng, alt)
|
||||
this.lat = x;
|
||||
this.lng = y;
|
||||
this.alt = z;
|
||||
|
||||
} else if(coordFrame == MAV_FRAME_GLOBAL_INT) {
|
||||
// (x, y, z) = (latInt, lngInt, alt)
|
||||
this.lat = x / 10000000;
|
||||
this.lng = y / 10000000;
|
||||
this.alt = z;
|
||||
|
||||
} else if(coordFrame == MAV_FRAME_LOCAL_NED) {
|
||||
// (x, y, z) = (northMeters, eastMeters, downMeters)
|
||||
double dCord[] = Functions.metersToLatLng(x, y, lat);
|
||||
this.lat = dCord[0];
|
||||
this.lng = dCord[1];
|
||||
this.alt = -z; // TODO: Check sign (depends on frame)
|
||||
|
||||
} else if(coordFrame == MAV_FRAME_LOCAL_OFFSET_NED) {
|
||||
// (x, y, z) = (dNorthMeters, dEastMeters, dDownMeters)
|
||||
double dCord[] = Functions.metersToLatLng(x, y, lat);
|
||||
this.lat += dCord[0];
|
||||
this.lat += dCord[1];
|
||||
this.alt -= z; // TODO: Check sign (depends on frame)
|
||||
|
||||
} else if(coordFrame == MAV_FRAME_BODY_FRD) {
|
||||
// (x, y, z) = (dForwardMeters, dRightMeters, dDownMeters)
|
||||
|
||||
double yaw = mFlightController.getState().getAttitude().yaw;
|
||||
|
||||
double[] dLatLng = Functions.getLatLngDiff(lat, Math.toRadians(yaw), x, y);
|
||||
this.lat += dLatLng[0];
|
||||
this.lng += dLatLng[1];
|
||||
this.alt -= z;
|
||||
|
||||
} else {
|
||||
// TODO:
|
||||
Log.e(TAG, "CoordFrame not supported");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public Motion newMotion(int command, int mavLinkMask) {
|
||||
@@ -2196,19 +2254,14 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
double lng = coord.getAircraftLocation().getLongitude();
|
||||
double alt = coord.getAircraftLocation().getAltitude();
|
||||
|
||||
double destX = motion.x;
|
||||
double destY = motion.y;
|
||||
double destZ = motion.z;
|
||||
if(motion.coordFrame == MAV_FRAME_BODY_FRD) {
|
||||
destX += lat;
|
||||
destY += lng;
|
||||
} else {
|
||||
Log.e(TAG, "Frame not supported");
|
||||
}
|
||||
double destX = motion.lat;
|
||||
double destY = motion.lng;
|
||||
double destZ = motion.alt;
|
||||
|
||||
// Reset according to mask
|
||||
if(motion.mask.ignorePosX) destX = lat;
|
||||
if(motion.mask.ignorePosY) destX = lng;
|
||||
if(motion.mask.ignorePosZ) destX = alt;
|
||||
if(motion.mask.ignorePosY) destY = lng;
|
||||
if(motion.mask.ignorePosZ) destZ = alt;
|
||||
|
||||
// Distance to destination
|
||||
double dist = getRangeBetweenWaypoints_m(destX, destY, destZ, lat, lng, alt);
|
||||
@@ -2767,27 +2820,6 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
return sign;
|
||||
}
|
||||
|
||||
public double[] get_location_metres(double dForward, double dRight, double alt) {
|
||||
FlightControllerState coord = djiAircraft.getFlightController().getState();
|
||||
double ro = Math.toRadians(coord.getAttitude().yaw);
|
||||
double lat = coord.getAircraftLocation().getLatitude();
|
||||
double lon = coord.getAircraftLocation().getLongitude();
|
||||
double newalt = coord.getAircraftLocation().getAltitude() + alt;
|
||||
|
||||
double dNorth = dForward * Math.cos(ro) - dRight * Math.sin(ro);
|
||||
double dEast = dForward * Math.sin(ro) + dRight * Math.cos(ro);
|
||||
|
||||
//Coordinate offsets in radians
|
||||
double dLat = dNorth / EARTH_RADIUS;
|
||||
double dLon = dEast / (EARTH_RADIUS * Math.cos(Math.toRadians(lat)));
|
||||
|
||||
// New position in decimal degrees
|
||||
double newlat = lat + Math.toDegrees(dLat);
|
||||
double newlon = lon + Math.toDegrees(dLon);
|
||||
|
||||
return new double[]{newlat, newlon, newalt};
|
||||
}
|
||||
|
||||
private double[] get_location_intermediet(double lat, double lon, double dist, double yaw) {
|
||||
|
||||
double ro = Math.toRadians(yaw);
|
||||
|
||||
@@ -669,10 +669,7 @@ public class DummyProduct extends Aircraft {
|
||||
double throttleVel = flightControlData.getVerticalThrottle();
|
||||
|
||||
double rad = Math.toRadians(yaw);
|
||||
|
||||
double dNorth = rightVel * Math.sin(rad) + fwdVel * Math.cos(rad);
|
||||
double dEast = rightVel * Math.cos(rad) + fwdVel * Math.sin(rad);
|
||||
double[] dLatLng = Functions.metersToLatLng(dNorth, dEast, lat);
|
||||
double[] dLatLng = Functions.getLatLngDiff(lat, rad, fwdVel, rightVel);
|
||||
|
||||
lat += dLatLng[0] / dT;
|
||||
lng += dLatLng[1] / dT;
|
||||
|
||||
@@ -9,4 +9,33 @@ public class Functions {
|
||||
double dLng = eastMeters / (EARTH_RADIUS * Math.cos(lat / c)) * c;
|
||||
return new double[] {dLat, dLng};
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns North and East increments if we fly forwardMeters and rightMeters while we are heading to angle 'yaw'.
|
||||
* See also: getLatLngDiff()
|
||||
*
|
||||
* @param yaw Current yaw (heading) in radians. If we are heading north (NED), a yaw angle of 90° is considered pointing to the east
|
||||
* @param forwardMeters Meters to add in forward direction
|
||||
* @param rightMeters Meters to add to right direction
|
||||
* @return (dNorth, dEast)
|
||||
*/
|
||||
public static double[] getNorthEastDiff(double yaw, double forwardMeters, double rightMeters) {
|
||||
double dNorth = forwardMeters * Math.cos(yaw) - rightMeters * Math.sin(yaw);
|
||||
double dEast = forwardMeters * Math.sin(yaw) + rightMeters * Math.cos(yaw);
|
||||
return new double[] {dNorth, dEast};
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns Latitude and Longitude increments if we fly forwardMeters and rightMeters while we are heading to angle 'yaw'.
|
||||
*
|
||||
* @param lat Current Latitude (Longitude is not required).
|
||||
* @param yaw Current yaw (heading) in radians. If we are heading north (NED), a yaw angle of 90° is considered pointing to the east
|
||||
* @param forwardMeters Meters to add in forward direction
|
||||
* @param rightMeters Meters to add to right direction
|
||||
* @return (dLat, dLng)
|
||||
*/
|
||||
public static double[] getLatLngDiff(double lat, double yaw, double forwardMeters, double rightMeters) {
|
||||
double dMeters[] = getNorthEastDiff(yaw, forwardMeters, rightMeters);
|
||||
return Functions.metersToLatLng(dMeters[0], dMeters[1], lat);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -18,7 +18,6 @@ import com.MAVLink.common.msg_file_transfer_protocol;
|
||||
import com.MAVLink.common.msg_set_position_target_local_ned;
|
||||
import com.MAVLink.common.msg_timesync;
|
||||
import com.MAVLink.enums.MAV_CMD;
|
||||
import com.MAVLink.enums.MAV_FRAME;
|
||||
import com.MAVLink.enums.MAV_MISSION_RESULT;
|
||||
import com.MAVLink.enums.MAV_RESULT;
|
||||
|
||||
@@ -47,6 +46,7 @@ import static com.MAVLink.common.msg_home_position.MAVLINK_MSG_ID_HOME_POSITION;
|
||||
import static com.MAVLink.common.msg_ping.MAVLINK_MSG_ID_PING;
|
||||
import static com.MAVLink.common.msg_timesync.MAVLINK_MSG_ID_TIMESYNC;
|
||||
import static com.MAVLink.enums.MAV_CMD.MAV_CMD_REQUEST_MESSAGE;
|
||||
import static com.MAVLink.enums.MAV_FRAME.MAV_FRAME_GLOBAL;
|
||||
import static com.MAVLink.minimal.msg_heartbeat.MAVLINK_MSG_ID_HEARTBEAT;
|
||||
import static com.MAVLink.common.msg_manual_control.MAVLINK_MSG_ID_MANUAL_CONTROL;
|
||||
import static com.MAVLink.common.msg_mission_ack.MAVLINK_MSG_ID_MISSION_ACK;
|
||||
@@ -270,9 +270,7 @@ public class MAVLinkReceiver {
|
||||
|
||||
} else if (msg_cmd.param2 == MAV_GOTO_HOLD_AT_SPECIFIED_POSITION) {
|
||||
DroneModel.Motion motion = mModel.newMotion(MAV_GOTO_HOLD_AT_SPECIFIED_POSITION, 0b0000111111111000);
|
||||
motion.x = msg_cmd.param5;
|
||||
motion.y = msg_cmd.param6;
|
||||
motion.z = msg_cmd.param7;
|
||||
motion.setDestination(MAV_FRAME_GLOBAL, msg_cmd.param5, msg_cmd.param6, msg_cmd.param7);
|
||||
motion.yaw = msg_cmd.param4;
|
||||
mModel.startMotion(motion);
|
||||
}
|
||||
@@ -348,12 +346,8 @@ public class MAVLinkReceiver {
|
||||
|
||||
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
|
||||
msg_set_position_target_local_ned msg_param = (msg_set_position_target_local_ned) msg;
|
||||
double[] pos = mModel.get_location_metres(msg_param.x, msg_param.y, msg_param.z);
|
||||
DroneModel.Motion motion = mModel.newMotion(MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, msg_param.type_mask);
|
||||
motion.coordFrame = msg_param.coordinate_frame;
|
||||
motion.x = pos[0];
|
||||
motion.y = pos[1];
|
||||
motion.z = pos[2];
|
||||
motion.setDestination(msg_param.coordinate_frame, msg_param.x, msg_param.y, msg_param.z);
|
||||
motion.yaw = msg_param.yaw;
|
||||
motion.yawRate = msg_param.yaw_rate;
|
||||
motion.vx = msg_param.vx;
|
||||
@@ -541,6 +535,7 @@ public class MAVLinkReceiver {
|
||||
|
||||
// If position is set to zero then it must be a velocity command... We should use rather the mask ...
|
||||
if (((msg.type_mask & 0b0000100000000111) == 0x007)) { // If no move and we use yaw rate...
|
||||
// TODO: Rewrite/implement in Motion class
|
||||
float dNorth = msg.vx;
|
||||
float dEast = msg.vy;
|
||||
double ro = Math.toRadians(mModel.getCurrentYaw());
|
||||
@@ -551,9 +546,7 @@ public class MAVLinkReceiver {
|
||||
motion.yaw = Math.toDegrees(msg.yaw_rate);
|
||||
|
||||
} else {
|
||||
motion.x = msg.lat_int / 10000000;
|
||||
motion.y = msg.lon_int / 10000000;
|
||||
motion.z = msg.alt;
|
||||
motion.setDestination(msg.coordinate_frame, msg.lat_int, msg.lon_int, msg.alt);
|
||||
motion.yaw = msg.yaw;
|
||||
motion.yawRate = msg.yaw_rate;
|
||||
}
|
||||
|
||||
Referência em uma Nova Issue
Bloquear um usuário