Commit 4174385c authored by Roque's avatar Roque

Drone simulator API refactoring

See merge request nexedi/erp5!1803
parents 720d86ee 0b781233
......@@ -43,9 +43,8 @@ var DroneLogAPI = /** @class */ (function () {
return log_entry_list;
}
var log = this._drone_info.log_content, entry_1, entry_2, interval,
map_dict = this._mapManager.getMapInfo(),
min_height = 15, converted_log_point_list = [],
i, splitted_log_entry, x, y, position, lat, lon, height, timestamp,
i, splitted_log_entry, position, lat, lon, height, timestamp,
time_offset = 1, log_entry_list = getLogEntries(log);
//XXX: Patch to determine log time format (if this is standarized, drop it)
if (log_entry_list[0] && log_entry_list[1]) {
......@@ -208,6 +207,9 @@ var DroneLogAPI = /** @class */ (function () {
DroneLogAPI.prototype.exit = function (drone) {
return;
};
DroneLogAPI.prototype.set_loiter_mode = function (loiter) {
return;
};
return DroneLogAPI;
}());
\ No newline at end of file
......@@ -246,7 +246,7 @@
</item>
<item>
<key> <string>serial</string> </key>
<value> <string>1006.43901.65103.9898</string> </value>
<value> <string>1009.38419.20212.62293</string> </value>
</item>
<item>
<key> <string>state</string> </key>
......@@ -266,7 +266,7 @@
</tuple>
<state>
<tuple>
<float>1677599947.8</float>
<float>1688635412.87</float>
<string>UTC</string>
</tuple>
</state>
......
......@@ -9,8 +9,6 @@ var FixedWingDroneAPI = /** @class */ (function () {
var DEFAULT_SPEED = 16,
EARTH_GRAVITY = 9.81,
LOITER_LIMIT = 30,
LOITER_RADIUS_FACTOR = 0.60,
LOITER_SPEED_FACTOR = 1.5,
MAX_ACCELERATION = 6,
MAX_DECELERATION = 1,
MIN_SPEED = 12,
......@@ -29,10 +27,8 @@ var FixedWingDroneAPI = /** @class */ (function () {
this._flight_parameters = flight_parameters;
this._id = id;
this._drone_info = drone_info;
this._loiter_radius = 0;
this._last_loiter_point_reached = -1;
this._loiter_radius = 100;
//this._start_altitude = 0;
//this._last_altitude_point_reached = -1;
this._loiter_mode = false;
this._drone_dict_list = [];
}
......@@ -146,9 +142,6 @@ var FixedWingDroneAPI = /** @class */ (function () {
*/
FixedWingDroneAPI.prototype.internal_post_update = function (drone) {
var _this = this, drone_position = drone.getCurrentPosition(), drone_info;
if (_this._loiter_mode) {
_this.loiter(drone);
}
/*if (_this._start_altitude > 0) { //TODO move start_altitude here
_this.reachAltitude(drone);
}*/
......@@ -158,7 +151,10 @@ var FixedWingDroneAPI = /** @class */ (function () {
'altitudeAbs' : _this._mapManager.getMapInfo().start_AMSL +
drone_position.z,
'latitude' : drone_position.x,
'longitude' : drone_position.y
'longitude' : drone_position.y,
'yaw': drone.getYaw(),
'speed': drone.getAirSpeed(),
'climbRate': drone.getClimbRate()
};
_this._drone_dict_list[_this._id] = drone_info;
//broadcast drone info using internal msg
......@@ -171,7 +167,7 @@ var FixedWingDroneAPI = /** @class */ (function () {
};
FixedWingDroneAPI.prototype._updateSpeed = function (drone, delta_time) {
var speed = drone.getSpeed(), speedDiff, speedUpdate;
var speed = drone.getAirSpeed(), speedDiff, speedUpdate;
if (speed !== this._targetSpeed) {
speedDiff = this._targetSpeed - speed;
speedUpdate = drone._acceleration * delta_time / 1000;
......@@ -185,13 +181,31 @@ var FixedWingDroneAPI = /** @class */ (function () {
};
FixedWingDroneAPI.prototype._updateDirection = function (drone, delta_time) {
var horizontalCoeff, newX, newY, newZ;
var horizontalCoeff, newX, newY, newZ, tangentYaw;
if (this._loiter_mode
&& Math.sqrt(
Math.pow(drone._targetCoordinates.x - drone.position.x, 2)
+ Math.pow(drone._targetCoordinates.y - drone.position.y, 2)
) <= this._loiter_radius) {
tangentYaw = this._computeBearing(
drone.position.x,
drone.position.y,
drone._targetCoordinates.x,
drone._targetCoordinates.y
) - 90;
// trigonometric circle is east oriented, yaw angle is clockwise
tangentYaw = this._toRad(-tangentYaw + 90);
newX = Math.cos(tangentYaw);
newZ = Math.sin(tangentYaw);
} else {
[newX, newZ] = this._getNewYaw(drone, delta_time);
}
newY = this._getNewAltitude(drone);
horizontalCoeff = Math.sqrt(
(
Math.pow(drone.getSpeed(), 2) - Math.pow(newY, 2)
Math.pow(drone.getAirSpeed(), 2) - Math.pow(newY, 2)
) / (
Math.pow(newX, 2) + Math.pow(newZ, 2)
)
......@@ -235,14 +249,14 @@ var FixedWingDroneAPI = /** @class */ (function () {
verticalSpeed = this._computeVerticalSpeed(
altitudeDiff,
this.getMaxClimbRate(),
drone.getSpeed(),
drone.getAirSpeed(),
this.getMaxPitchAngle()
);
} else {
verticalSpeed = -this._computeVerticalSpeed(
Math.abs(altitudeDiff),
this.getMaxSinkRate(),
drone.getSpeed(),
drone.getAirSpeed(),
-this.getMinPitchAngle()
);
}
......@@ -261,17 +275,13 @@ var FixedWingDroneAPI = /** @class */ (function () {
drone.rotation.z + y);
};
FixedWingDroneAPI.prototype.setAltitude = function (drone, altitude) {
drone._targetCoordinates.z = altitude;
};
FixedWingDroneAPI.prototype.setSpeed = function (drone, speed) {
this._targetSpeed = Math.max(
Math.min(speed, this.getMaxSpeed()),
this.getMinSpeed()
);
drone._acceleration = (this._targetSpeed > drone.getSpeed())
drone._acceleration = (this._targetSpeed > drone.getAirSpeed())
? this.getMaxAcceleration() : -this.getMaxDeceleration();
};
......@@ -290,34 +300,18 @@ var FixedWingDroneAPI = /** @class */ (function () {
this._drone_dict_list[id] = msg;
};
FixedWingDroneAPI.prototype.set_loiter_mode = function (radius) {
FixedWingDroneAPI.prototype.internal_setTargetCoordinates =
function (drone, coordinates, radius) {
if (radius) {
this._loiter_mode = true;
if (radius && radius > LOITER_LIMIT) {
this._loiter_radius = radius * LOITER_RADIUS_FACTOR;
this._loiter_center = this._last_target;
this._loiter_coordinates = [];
this._last_loiter_point_reached = -1;
var x1, y1, angle;
//for (var angle = 0; angle <360; angle+=8){ //counter-clockwise
for (angle = 360; angle > 0; angle -= 8) { //clockwise
x1 = this._loiter_radius *
Math.cos(this._toRad(angle)) + this._loiter_center.x;
y1 = this._loiter_radius *
Math.sin(this._toRad(angle)) + this._loiter_center.y;
this._loiter_coordinates.push(
this.getCurrentPosition(x1, y1, this._loiter_center.z)
);
}
if (radius >= LOITER_LIMIT) {
this._loiter_radius = radius;
}
};
FixedWingDroneAPI.prototype.internal_setTargetCoordinates =
function (drone, coordinates, loiter) {
if (!loiter) {
} else {
this._loiter_mode = false;
//save last target point to use as next loiter center
this._last_target = coordinates;
}
};
FixedWingDroneAPI.prototype.sendMsg = function (msg, to) {
var _this = this,
droneList = _this._gameManager._droneList;
......@@ -362,8 +356,8 @@ var FixedWingDroneAPI = /** @class */ (function () {
if (isNaN(lat) || isNaN(lon) || isNaN(z)) {
throw new Error('Target coordinates must be numbers');
}
var x = this._mapManager.longitudToX(lon, this._map_dict.width),
y = this._mapManager.latitudeToY(lat, this._map_dict.depth),
var x = this._mapManager.longitudToX(lon, this._map_dict.map_size),
y = this._mapManager.latitudeToY(lat, this._map_dict.map_size),
position = this._mapManager.normalize(x, y, this._map_dict),
processed_coordinates;
if (z > this._map_dict.start_AMSL) {
......@@ -381,62 +375,6 @@ var FixedWingDroneAPI = /** @class */ (function () {
FixedWingDroneAPI.prototype.getCurrentPosition = function (x, y, z) {
return this._mapManager.convertToGeoCoordinates(x, y, z, this._map_dict);
};
FixedWingDroneAPI.prototype.loiter = function (drone) {
if (this._loiter_radius > LOITER_LIMIT) {
var drone_pos = drone.getCurrentPosition(),
min = 9999,
min_i,
i,
d,
next_point;
//shift loiter circle to nearest point
if (this._last_loiter_point_reached === -1) {
if (!this.shifted) {
drone._maxSpeed = drone._maxSpeed * LOITER_SPEED_FACTOR;
for (i = 0; i < this._loiter_coordinates.length; i += 1) {
d = this._mapManager.latLonDistance([drone_pos.x, drone_pos.y],
[this._loiter_coordinates[i].x,
this._loiter_coordinates[i].y]);
if (d < min) {
min = d;
min_i = i;
}
}
this._loiter_coordinates = this._loiter_coordinates.concat(
this._loiter_coordinates.splice(0, min_i)
);
this.shifted = true;
}
} else {
this.shifted = false;
}
//stop
if (this._last_loiter_point_reached ===
this._loiter_coordinates.length - 1) {
if (drone._maxSpeed !== this.getMaxSpeed()) {
drone._maxSpeed = this.getMaxSpeed();
}
drone.setDirection(0, 0, 0);
return;
}
//loiter
next_point =
this._loiter_coordinates[this._last_loiter_point_reached + 1];
this.internal_setTargetCoordinates(drone, next_point, true);
if (this._mapManager.latLonDistance([drone_pos.x, drone_pos.y],
[next_point.x, next_point.y]) < 1) {
this._last_loiter_point_reached += 1;
if (this._last_loiter_point_reached ===
this._loiter_coordinates.length - 1) {
return;
}
next_point = this._loiter_coordinates[
this._last_loiter_point_reached + 1
];
this.internal_setTargetCoordinates(drone, next_point, true);
}
}
};
FixedWingDroneAPI.prototype.getDroneAI = function () {
return null;
};
......@@ -477,7 +415,7 @@ var FixedWingDroneAPI = /** @class */ (function () {
FixedWingDroneAPI.prototype.getYawVelocity = function (drone) {
return 360 * EARTH_GRAVITY
* Math.tan(this._toRad(this.getMaxRollAngle()))
/ (2 * Math.PI * drone.getSpeed());
/ (2 * Math.PI * drone.getAirSpeed());
};
FixedWingDroneAPI.prototype.getYaw = function (drone) {
var direction = drone.worldDirection;
......@@ -493,7 +431,8 @@ var FixedWingDroneAPI = /** @class */ (function () {
};
FixedWingDroneAPI.prototype._computeVerticalSpeed =
function (altitude_diff, max_climb_rate, speed, max_pitch) {
var maxVerticalSpeed = Math.min(altitude_diff, Math.min(max_climb_rate, speed));
var maxVerticalSpeed =
Math.min(altitude_diff, Math.min(max_climb_rate, speed));
return (this._toDeg(Math.asin(maxVerticalSpeed / speed)) > max_pitch)
? speed * Math.sin(this._toRad(max_pitch))
: maxVerticalSpeed;
......@@ -505,13 +444,13 @@ var FixedWingDroneAPI = /** @class */ (function () {
return angle * 180 / Math.PI;
};
FixedWingDroneAPI.prototype.getClimbRate = function (drone) {
return drone.worldDirection.y * drone.getSpeed();
return drone.worldDirection.y * drone.getAirSpeed();
};
FixedWingDroneAPI.prototype.getGroundSpeed = function (drone) {
var direction = drone.worldDirection;
return Math.sqrt(
Math.pow(direction.x * drone.getSpeed(), 2)
+ Math.pow(direction.z * drone.getSpeed(), 2)
Math.pow(direction.x * drone.getAirSpeed(), 2)
+ Math.pow(direction.z * drone.getAirSpeed(), 2)
);
};
FixedWingDroneAPI.prototype.triggerParachute = function (drone) {
......@@ -526,10 +465,10 @@ var FixedWingDroneAPI = /** @class */ (function () {
return;
};
FixedWingDroneAPI.prototype.getInitialAltitude = function () {
return 0;
return this._map_dict.start_AMSL;
};
FixedWingDroneAPI.prototype.getAltitudeAbs = function (altitude) {
return altitude;
return altitude + this._map_dict.start_AMSL;
};
FixedWingDroneAPI.prototype.getMinHeight = function () {
return 0;
......
......@@ -226,7 +226,7 @@
</item>
<item>
<key> <string>actor</string> </key>
<value> <string>zope</string> </value>
<value> <unicode>zope</unicode> </value>
</item>
<item>
<key> <string>comment</string> </key>
......@@ -240,7 +240,7 @@
</item>
<item>
<key> <string>serial</string> </key>
<value> <string>1006.36731.10993.50739</string> </value>
<value> <string>1009.59163.16294.47701</string> </value>
</item>
<item>
<key> <string>state</string> </key>
......@@ -260,7 +260,7 @@
</tuple>
<state>
<tuple>
<float>1677169724.0</float>
<float>1690395852.2</float>
<string>UTC</string>
</tuple>
</state>
......
......@@ -31,7 +31,6 @@ var DroneManager = /** @class */ (function () {
this._scene = scene;
this._canUpdate = true;
this._id = id;
this._leader_id = 0;
this._API = API; // var API created on AI evel
// Create the control mesh
this._controlMesh = BABYLON.Mesh.CreateBox(
......@@ -61,11 +60,6 @@ var DroneManager = /** @class */ (function () {
// swap y and z axis so z axis represents altitude
return new BABYLON.Vector3(vector.x, vector.z, vector.y);
};
Object.defineProperty(DroneManager.prototype, "leader_id", {
get: function () { return this._leader_id; },
enumerable: true,
configurable: true
});
Object.defineProperty(DroneManager.prototype, "drone_dict", {
get: function () { return this._API._drone_dict_list; },
enumerable: true,
......@@ -140,6 +134,10 @@ var DroneManager = /** @class */ (function () {
* Set a target point to move
*/
DroneManager.prototype.setTargetCoordinates = function (x, y, z) {
this._internal_setTargetCoordinates(x, y, z);
};
DroneManager.prototype._internal_setTargetCoordinates =
function (x, y, z, radius) {
if (!this._canPlay) {
return;
}
......@@ -147,7 +145,8 @@ var DroneManager = /** @class */ (function () {
this._targetCoordinates = this._API.processCoordinates(x, y, z);
return this._API.internal_setTargetCoordinates(
this,
this._targetCoordinates
this._targetCoordinates,
radius
);
};
DroneManager.prototype.internal_update = function (delta_time) {
......@@ -194,7 +193,7 @@ var DroneManager = /** @class */ (function () {
}
return this._API.setStartingPosition(this, x, y, z);
};
DroneManager.prototype.setSpeed = function (speed) {
DroneManager.prototype.setAirSpeed = function (speed) {
if (!this._canPlay) {
return;
}
......@@ -292,20 +291,11 @@ var DroneManager = /** @class */ (function () {
}
return null;
};
DroneManager.prototype.setAltitude = function (altitude) {
if (!this._canPlay) {
return;
}
return this._API.setAltitude(this, altitude);
};
/**
* Make the drone loiter (circle with a set radius)
*/
DroneManager.prototype.loiter = function (radius) {
if (!this._canPlay) {
return;
}
this._API.set_loiter_mode(radius);
DroneManager.prototype.loiter = function (x, y, z, radius) {
this._internal_setTargetCoordinates(x, y, z, radius);
};
DroneManager.prototype.getFlightParameters = function () {
if (this._API.getFlightParameters) {
......@@ -316,7 +306,7 @@ var DroneManager = /** @class */ (function () {
DroneManager.prototype.getYaw = function () {
return this._API.getYaw(this);
};
DroneManager.prototype.getSpeed = function () {
DroneManager.prototype.getAirSpeed = function () {
return this._speed;
};
DroneManager.prototype.getGroundSpeed = function () {
......
......@@ -240,7 +240,7 @@
</item>
<item>
<key> <string>serial</string> </key>
<value> <string>1009.7345.31305.44339</string> </value>
<value> <string>1009.36979.45339.6075</string> </value>
</item>
<item>
<key> <string>state</string> </key>
......@@ -260,7 +260,7 @@
</tuple>
<state>
<tuple>
<float>1687455790.77</float>
<float>1688635184.98</float>
<string>UTC</string>
</tuple>
</state>
......
......@@ -438,10 +438,10 @@
["my_map_height"]]
], [
"right",
[["my_start_AMSL"], ["my_drone_min_speed"], ["my_drone_speed"], ["my_drone_max_speed"],
["my_drone_max_acceleration"], ["my_drone_max_deceleration"],
["my_drone_max_roll"], ["my_drone_min_pitch"], ["my_drone_max_pitch"],
["my_drone_max_sink_rate"], ["my_drone_max_climb_rate"]]
[["my_start_AMSL"], ["my_drone_min_speed"], ["my_drone_speed"],
["my_drone_max_speed"], ["my_drone_max_acceleration"],
["my_drone_max_deceleration"], ["my_drone_max_roll"], ["my_drone_min_pitch"],
["my_drone_max_pitch"], ["my_drone_max_sink_rate"], ["my_drone_max_climb_rate"]]
], [
"bottom",
[["my_script"]]
......
......@@ -232,7 +232,7 @@
</item>
<item>
<key> <string>actor</string> </key>
<value> <string>zope</string> </value>
<value> <unicode>zope</unicode> </value>
</item>
<item>
<key> <string>comment</string> </key>
......@@ -246,7 +246,7 @@
</item>
<item>
<key> <string>serial</string> </key>
<value> <string>1006.23748.49086.52309</string> </value>
<value> <string>1009.22574.47206.13909</string> </value>
</item>
<item>
<key> <string>state</string> </key>
......@@ -266,7 +266,7 @@
</tuple>
<state>
<tuple>
<float>1677179158.33</float>
<float>1687785962.67</float>
<string>UTC</string>
</tuple>
</state>
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment